conjugate_gradient_method.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ACADO Toolkit.
3  *
4  * ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
5  * Copyright (C) 2008-2014 by Boris Houska, Hans Joachim Ferreau,
6  * Milan Vukov, Rien Quirynen, KU Leuven.
7  * Developed within the Optimization in Engineering Center (OPTEC)
8  * under supervision of Moritz Diehl. All rights reserved.
9  *
10  * ACADO Toolkit is free software; you can redistribute it and/or
11  * modify it under the terms of the GNU Lesser General Public
12  * License as published by the Free Software Foundation; either
13  * version 3 of the License, or (at your option) any later version.
14  *
15  * ACADO Toolkit is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
18  * Lesser General Public License for more details.
19  *
20  * You should have received a copy of the GNU Lesser General Public
21  * License along with ACADO Toolkit; if not, write to the Free Software
22  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
23  *
24  */
25 
26 
35 
36 #include <iostream>
37 
39 
40 
41 
42 //
43 // PUBLIC MEMBER FUNCTIONS:
44 //
45 
46 
47 
49 
50  dim = 0 ;
51  nDense = 0 ;
52  A = 0 ;
53  x = 0 ;
54  norm2 = 0 ;
55  p = 0 ;
56  r = 0 ;
57  TOL = 1.e-10;
58  printLevel = LOW;
59  pCounter = 0 ;
60  condScale = 0 ;
61 }
62 
63 
65 
66  int run1, run2;
67 
68  dim = arg.dim;
69  nDense = arg.nDense;
70 
71  if( arg.A == 0 ) A = 0;
72  else{
73  A = new double[nDense];
74  for( run1 = 0; run1 < nDense; run1++ )
75  A[run1] = arg.A[run1];
76  }
77 
78  if( arg.x == 0 ) x = 0;
79  else{
80  x = new double[dim];
81  for( run1 = 0; run1 < dim; run1++ )
82  x[run1] = arg.x[run1];
83  }
84 
85  pCounter = arg.pCounter;
86 
87 
88  if( arg.norm2 == 0 ) norm2 = 0;
89  else{
90  norm2 = new double[2*dim+1];
91  for( run1 = 0; run1 < 2*dim+1; run1++ )
92  norm2[run1] = arg.norm2[run1];
93  }
94 
95 
96  if( arg.p == 0 ) p = 0;
97  else{
98  p = new double*[2*dim+1];
99  for( run1 = 0; run1 < 2*dim+1; run1++ ){
100  p[run1] = new double[dim];
101  for( run2 = 0; run2 < dim; run2++ ){
102  p[run1][run2] = arg.p[run1][run2];
103  }
104  }
105  }
106 
107  if( arg.r == 0 ) r = 0;
108  else{
109  r = new double[dim];
110  for( run1 = 0; run1 < dim; run1++ )
111  r[run1] = arg.r[run1];
112  }
113 
114  TOL = arg.TOL;
115  printLevel = arg.printLevel;
116 
117  if( arg.condScale == 0 ) condScale = 0;
118  else{
119  condScale = new double[dim];
120  for( run1 = 0; run1 < dim; run1++ )
121  condScale[run1] = arg.condScale[run1];
122  }
123 }
124 
125 
127 
128  int run1;
129 
130  if( A != 0 ) delete[] A;
131  if( x != 0 ) delete[] x;
132 
133  if( norm2 != 0 ) delete[] norm2;
134 
135  if( p != 0 ){
136  for( run1 = 0; run1 < 2*dim+1; run1++ )
137  delete[] p[run1];
138  delete[] p;
139  }
140 
141  if( r != 0 ) delete[] r;
142 
143  if( condScale != 0) delete[] condScale;
144 }
145 
146 
147 
148 
150 
151  // CONSISTENCY CHECKS:
152  // -------------------
153  if( dim <= 0 ) return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
154  if( nDense <= 0 ) return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
155  if( A == 0 ) return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
156 
157 
158  int run1, run2;
159 
160  double *aux = new double[dim];
161  double auxR, auxR2, norm1;
162  double alpha, beta;
163 
164  // INITIALISE X:
165  // --------------
166 
167  for( run1 = 0; run1 < dim; run1++ )
168  x[run1] = 0.0;
169 
170 
171  // RESET THE COUNTER IF TOO LARGE:
172  // -------------------------------
173  if( pCounter > dim ) pCounter = dim-1;
174 
175 
176  // APPLY THE PRECONDITIONER ON b:
177  // ------------------------------
178 
179  applyPreconditioner( b );
180 
181 
182  // COMPUTE WARM START INITIALIZATION FOR X BASED ON PREVIOUS
183  // DECOMPOSITIONS:
184  // ----------------------------------------------------------
185 
186  for( run1 = 0; run1 < pCounter; run1++ ){
187  alpha = scalarProduct( p[run1], r )/norm2[run1];
188  for( run2 = 0; run2 < dim; run2++ ){
189  x[run2] += alpha*p[run1][run2];
190  }
191  }
192 
193  // COMPUTE INITIAL RESIDUUM:
194  // -------------------------
195 
196  multiply( x, aux );
197 
198  for( run1 = 0; run1 < dim; run1++ )
199  r[run1] -= aux[run1];
200 
201 
202  for( run1 = 0; run1 < dim; run1++ )
203  p[pCounter][run1] = r[run1];
204 
205 
206  while( pCounter <= 2*dim-1 ){
207 
208  norm1 = 0.0;
209 
210  for( run1 = 0; run1 < dim; run1++ ){
211  if( r[run1] >= 0 && norm1 < r[run1] ) norm1 = r[run1];
212  if( r[run1] <= 0 && norm1 < -r[run1] ) norm1 = -r[run1];
213  }
214 
215  if( printLevel == HIGH )
216  std::cout << "STEP NUMBER " << pCounter << ", WEIGHTED NORM = "
217  << std::scientific << norm1 << std::endl;
218 
219  if( norm1 < TOL ) break;
220 
221  auxR = scalarProduct( r,r );
222  multiply( p[pCounter], aux );
223  norm2[pCounter] = scalarProduct( p[pCounter], aux );
224  alpha = auxR/norm2[pCounter];
225 
226  for( run1 = 0; run1 < dim; run1++ ){
227  x [run1] += alpha*p[pCounter][run1];
228  r [run1] -= alpha*aux[run1];
229  }
230 
231  auxR2 = scalarProduct( r,r );
232  beta = auxR2/auxR;
233 
234  for( run1 = 0; run1 < dim; run1++ )
235  p[pCounter+1][run1] = r[run1] + beta*p[pCounter][run1];
236 
237  pCounter++;
238  }
239 
240  delete[] aux ;
241 
242  if( pCounter >= 2*dim ){
243  if( printLevel == MEDIUM || printLevel == HIGH )
246  }
247 
248  return SUCCESSFUL_RETURN;
249 }
250 
251 
253 
254  int run1;
255 
256  dim = n;
257 
258  if( x != 0 ){
259  delete[] x;
260  x = 0;
261  }
262  x = new double[dim];
263 
264  if( norm2 != 0 ){
265  delete[] norm2;
266  norm2 = 0;
267  }
268  norm2 = new double[2*dim+1];
269 
270  if( p != 0 ){
271  for( run1 = 0; run1 < 2*dim+1; run1++ )
272  delete[] p[run1];
273  delete[] p;
274  }
275  p = new double*[2*dim+1];
276  for( run1 = 0; run1 < 2*dim+1; run1++ ){
277  p[run1] = new double[dim];
278  }
279 
280 
281  if( r != 0 ){
282  delete[] r;
283  r = 0;
284  }
285  r = new double[dim];
286 
287  if( condScale != 0 ){
288  delete[] condScale;
289  condScale = 0;
290  }
291  condScale = new double[dim];
292 
293  pCounter = 0;
294 
295  return SUCCESSFUL_RETURN;
296 }
297 
298 
299 
301 
302  if( A != 0 ){
303  delete[] A;
304  A = 0;
305  }
306 
307  nDense = nDense_;
308 
309  return SUCCESSFUL_RETURN;
310 }
311 
312 
313 
314 
316 
317  if( dim <= 0 ) return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
318  if( nDense <= 0 ) return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
319 
320  pCounter = 0;
321 
322  A = new double[nDense];
323  return computePreconditioner( A_ );
324 }
325 
326 
327 
328 
330 
331  return applyInversePreconditioner( x_ );
332 }
333 
334 
336 
337  TOL = TOL_;
338  return SUCCESSFUL_RETURN;
339 }
340 
341 
343 
344  printLevel = printLevel_;
345  return SUCCESSFUL_RETURN;
346 }
347 
348 
349 //
350 // PROTECTED MEMBER FUNCTIONS:
351 //
352 
353 
354 
355 double ConjugateGradientMethod::scalarProduct( double *aa, double *bb ){
356 
357  int run1;
358  double aux = 0.0;
359 
360  for( run1 = 0; run1 < dim; run1++ )
361  aux += aa[run1]*bb[run1];
362 
363  return aux;
364 }
365 
366 
367 
369 
370 
371 /*
372  * end of file
373  */
virtual returnValue setNumberOfEntries(const int &nDense_)
virtual returnValue getX(double *x_)
Allows to pass back messages to the calling function.
virtual returnValue setMatrix(double *A_)
double scalarProduct(double *aa, double *bb)
#define CLOSE_NAMESPACE_ACADO
virtual void multiply(double *xx, double *result)=0
virtual returnValue setDimension(const int &n)
virtual returnValue applyPreconditioner(double *b)=0
virtual returnValue solve(double *b)
virtual returnValue setPrintLevel(PrintLevel printLevel_)
Implements a conjugate gradient method as sparse linear algebra solver.
PrintLevel
#define ACADOWARNING(retval)
#define BEGIN_NAMESPACE_ACADO
virtual returnValue setTolerance(double TOL_)
virtual returnValue applyInversePreconditioner(double *x_)=0
virtual returnValue computePreconditioner(double *A_)=0
#define ACADOERROR(retval)


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Mon Jun 10 2019 12:34:31