conjugate_gradient_method.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 
00034 #include <acado/sparse_solver/sparse_solver.hpp>
00035 
00036 #include <iostream>
00037 
00038 BEGIN_NAMESPACE_ACADO
00039 
00040 
00041 
00042 //
00043 // PUBLIC MEMBER FUNCTIONS:
00044 //
00045 
00046 
00047 
00048 ConjugateGradientMethod::ConjugateGradientMethod( ){
00049 
00050     dim        = 0     ;
00051     nDense     = 0     ;
00052     A          = 0     ;
00053     x          = 0     ;
00054     norm2      = 0     ;
00055     p          = 0     ;
00056     r          = 0     ;
00057     TOL        = 1.e-10;
00058     printLevel = LOW;
00059     pCounter   = 0     ;
00060     condScale  = 0     ;
00061 }
00062 
00063 
00064 ConjugateGradientMethod::ConjugateGradientMethod( const ConjugateGradientMethod &arg ){
00065 
00066     int run1, run2;
00067 
00068     dim        = arg.dim;
00069     nDense     = arg.nDense;
00070 
00071     if( arg.A == 0 )  A = 0;
00072     else{
00073         A = new double[nDense];
00074         for( run1 = 0; run1 < nDense; run1++ )
00075             A[run1] = arg.A[run1];
00076     }
00077 
00078     if( arg.x == 0 )  x = 0;
00079     else{
00080         x = new double[dim];
00081         for( run1 = 0; run1 < dim; run1++ )
00082             x[run1] = arg.x[run1];
00083     }
00084 
00085     pCounter = arg.pCounter;
00086 
00087 
00088     if( arg.norm2 == 0 )  norm2 = 0;
00089     else{
00090         norm2 = new double[2*dim+1];
00091         for( run1 = 0; run1 < 2*dim+1; run1++ )
00092             norm2[run1] = arg.norm2[run1];
00093     }
00094 
00095 
00096     if( arg.p == 0 ) p = 0;
00097     else{
00098         p = new double*[2*dim+1];
00099         for( run1 = 0; run1 < 2*dim+1; run1++ ){
00100             p[run1] = new double[dim];
00101             for( run2 = 0; run2 < dim; run2++ ){
00102                 p[run1][run2] = arg.p[run1][run2];
00103             }
00104         }
00105     }
00106 
00107     if( arg.r == 0 )  r = 0;
00108     else{
00109         r = new double[dim];
00110         for( run1 = 0; run1 < dim; run1++ )
00111             r[run1] = arg.r[run1];
00112     }
00113 
00114     TOL = arg.TOL;
00115     printLevel = arg.printLevel;
00116 
00117     if( arg.condScale == 0 ) condScale = 0;
00118     else{
00119         condScale = new double[dim];
00120         for( run1 = 0; run1 < dim; run1++ )
00121             condScale[run1] = arg.condScale[run1];
00122     }
00123 }
00124 
00125 
00126 ConjugateGradientMethod::~ConjugateGradientMethod( ){
00127 
00128     int run1;
00129 
00130     if( A != 0 ) delete[] A;
00131     if( x != 0 ) delete[] x;
00132 
00133     if( norm2 != 0 ) delete[] norm2;
00134 
00135     if( p != 0 ){
00136         for( run1 = 0; run1 < 2*dim+1; run1++ )
00137             delete[] p[run1];
00138         delete[] p;
00139     }
00140 
00141     if( r != 0 ) delete[] r;
00142 
00143     if( condScale != 0) delete[] condScale;
00144 }
00145 
00146 
00147 
00148 
00149 returnValue ConjugateGradientMethod::solve( double *b ){
00150 
00151     // CONSISTENCY CHECKS:
00152     // -------------------
00153     if( dim    <= 0 )  return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
00154     if( nDense <= 0 )  return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
00155     if( A      == 0 )  return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
00156 
00157 
00158     int run1, run2;
00159 
00160     double *aux  = new double[dim];
00161     double auxR, auxR2, norm1;
00162     double alpha, beta;
00163 
00164     // INITIALISE  X:
00165     // --------------
00166 
00167     for( run1 = 0; run1 < dim; run1++ )
00168         x[run1] = 0.0;
00169 
00170 
00171     // RESET THE COUNTER IF TOO LARGE:
00172     // -------------------------------
00173     if( pCounter > dim ) pCounter = dim-1;
00174 
00175 
00176     // APPLY THE PRECONDITIONER ON b:
00177     // ------------------------------
00178 
00179     applyPreconditioner( b );
00180 
00181 
00182     // COMPUTE WARM START INITIALIZATION FOR X BASED ON PREVIOUS
00183     // DECOMPOSITIONS:
00184     // ----------------------------------------------------------
00185 
00186     for( run1 = 0; run1 < pCounter; run1++ ){
00187         alpha = scalarProduct( p[run1], r )/norm2[run1];
00188         for( run2 = 0; run2 < dim; run2++ ){
00189             x[run2] += alpha*p[run1][run2];
00190         }
00191     }
00192 
00193     // COMPUTE INITIAL RESIDUUM:
00194     // -------------------------
00195 
00196     multiply( x, aux );
00197 
00198     for( run1 = 0; run1 < dim; run1++ )
00199         r[run1] -= aux[run1];
00200 
00201 
00202     for( run1 = 0; run1 < dim; run1++ )
00203         p[pCounter][run1] = r[run1];
00204 
00205 
00206     while( pCounter <= 2*dim-1 ){
00207 
00208         norm1 = 0.0;
00209 
00210         for( run1 = 0; run1 < dim; run1++ ){
00211             if( r[run1] >= 0 && norm1 <  r[run1] ) norm1 =  r[run1];
00212             if( r[run1] <= 0 && norm1 < -r[run1] ) norm1 = -r[run1];
00213         }
00214 
00215         if( printLevel == HIGH )
00216             std::cout << "STEP NUMBER " << pCounter << ",  WEIGHTED NORM = "
00217                  << std::scientific << norm1 << std::endl;
00218 
00219         if( norm1 < TOL ) break;
00220 
00221         auxR = scalarProduct( r,r );
00222         multiply( p[pCounter], aux );
00223         norm2[pCounter] = scalarProduct( p[pCounter], aux );
00224         alpha = auxR/norm2[pCounter];
00225 
00226         for( run1 = 0; run1 < dim; run1++ ){
00227             x   [run1] += alpha*p[pCounter][run1];
00228             r   [run1] -= alpha*aux[run1];
00229         }
00230 
00231         auxR2 = scalarProduct( r,r );
00232         beta  = auxR2/auxR;
00233 
00234         for( run1 = 0; run1 < dim; run1++ )
00235             p[pCounter+1][run1] = r[run1] + beta*p[pCounter][run1];
00236 
00237         pCounter++;
00238     }
00239 
00240     delete[] aux ;
00241 
00242     if( pCounter >= 2*dim ){
00243         if( printLevel == MEDIUM || printLevel == HIGH )
00244             return ACADOWARNING( RET_LINEAR_SYSTEM_NUMERICALLY_SINGULAR );
00245         else return RET_LINEAR_SYSTEM_NUMERICALLY_SINGULAR;
00246     }
00247 
00248     return SUCCESSFUL_RETURN;
00249 }
00250 
00251 
00252 returnValue ConjugateGradientMethod::setDimension( const int &n ){
00253 
00254     int run1;
00255 
00256     dim = n;
00257 
00258     if( x != 0 ){
00259         delete[] x;
00260         x = 0;
00261     }
00262     x = new double[dim];
00263 
00264     if( norm2 != 0 ){
00265         delete[] norm2;
00266         norm2 = 0;
00267     }
00268     norm2 = new double[2*dim+1];
00269 
00270     if( p != 0 ){
00271         for( run1 = 0; run1 < 2*dim+1; run1++ )
00272             delete[] p[run1];
00273         delete[] p;
00274     }
00275     p = new double*[2*dim+1];
00276     for( run1 = 0; run1 < 2*dim+1; run1++ ){
00277         p[run1] = new double[dim];
00278     }
00279 
00280 
00281     if( r != 0 ){
00282         delete[] r;
00283         r = 0;
00284     }
00285     r = new double[dim];
00286 
00287     if( condScale != 0 ){
00288         delete[] condScale;
00289         condScale = 0;
00290     }
00291     condScale = new double[dim];
00292 
00293     pCounter = 0;
00294 
00295     return SUCCESSFUL_RETURN;
00296 }
00297 
00298 
00299 
00300 returnValue ConjugateGradientMethod::setNumberOfEntries( const int &nDense_ ){
00301 
00302     if( A != 0 ){
00303         delete[] A;
00304         A = 0;
00305     }
00306 
00307     nDense = nDense_;
00308 
00309     return SUCCESSFUL_RETURN;
00310 }
00311 
00312 
00313 
00314 
00315 returnValue ConjugateGradientMethod::setMatrix( double *A_ ){
00316 
00317     if( dim    <= 0 )  return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
00318     if( nDense <= 0 )  return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
00319 
00320     pCounter = 0;
00321 
00322     A = new double[nDense];
00323     return computePreconditioner( A_ );
00324 }
00325 
00326 
00327 
00328 
00329 returnValue ConjugateGradientMethod::getX( double *x_ ){
00330 
00331     return applyInversePreconditioner( x_ );
00332 }
00333 
00334 
00335 returnValue ConjugateGradientMethod::setTolerance( double TOL_ ){
00336 
00337     TOL = TOL_;
00338     return SUCCESSFUL_RETURN;
00339 }
00340 
00341 
00342 returnValue ConjugateGradientMethod::setPrintLevel( PrintLevel printLevel_ ){
00343 
00344     printLevel = printLevel_;
00345     return SUCCESSFUL_RETURN;
00346 }
00347 
00348 
00349 //
00350 // PROTECTED MEMBER FUNCTIONS:
00351 //
00352 
00353 
00354 
00355 double ConjugateGradientMethod::scalarProduct( double *aa, double *bb ){
00356 
00357     int run1;
00358     double aux = 0.0;
00359 
00360     for( run1 = 0; run1 < dim; run1++ )
00361         aux += aa[run1]*bb[run1];
00362 
00363     return aux;
00364 }
00365 
00366 
00367 
00368 CLOSE_NAMESPACE_ACADO
00369 
00370 
00371 /*
00372  *   end of file
00373  */


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