SparseSolver.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of qpOASES.
3  *
4  * qpOASES -- An Implementation of the Online Active Set Strategy.
5  *
6  * Copyright (C) 2012 by Andreas Waechter. All rights reserved.
7  *
8  * qpOASES is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU Lesser General Public
10  * License as published by the Free Software Foundation; either
11  * version 2.1 of the License, or (at your option) any later version.
12  *
13  * qpOASES is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
16  * See the GNU Lesser General Public License for more details.
17  *
18  * You should have received a copy of the GNU Lesser General Public
19  * License along with qpOASES; if not, write to the Free Software
20  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
21  *
22  */
23 
24 
36 #include <qpOASES/SparseSolver.hpp>
37 
38 #ifndef __MATLAB__
39 # include <cstdarg>
40 void MyPrintf(const char* pformat, ... );
41 #else
42 # include <mex.h>
43 # define MyPrintf mexPrintf
44 #endif
45 
46 //#define __DEBUG_ITER__
47 
48 BEGIN_NAMESPACE_QPOASES
49 
50 
51 /*****************************************************************************
52  * P U B L I C *
53  ****************************************************************************/
54 
55 
56 /*
57  * S p a r s e S o l v e r B a s e
58  */
59 SparseSolver::SparseSolver( )
60 {
61 }
62 
63 
64 /*
65  * S p a r s e S o l v e r B a s e
66  */
67 SparseSolver::SparseSolver( const SparseSolver& rhs )
68 {
69  copy( rhs );
70 }
71 
72 
73 /*
74  * ~ S p a r s e S o l v e r B a s e
75  */
76 SparseSolver::~SparseSolver( )
77 {
78  clear( );
79 }
80 
81 
82 /*
83  * o p e r a t o r =
84  */
85 SparseSolver& SparseSolver::operator=( const SparseSolver& rhs )
86 {
87  if ( this != &rhs )
88  {
89  clear( );
90  copy( rhs );
91  }
92 
93  return *this;
94 }
95 
96 /*
97  * r e s e t
98  */
99 returnValue SparseSolver::reset( )
100 {
101  return SUCCESSFUL_RETURN;
102 }
103 
104 /*
105  * g e t N e g a t i v e E i g e n v a l u e s
106  */
107 int_t SparseSolver::getNegativeEigenvalues( )
108 {
109  return -1;
110 }
111 
112 /*
113  * g e t R a n k
114  */
115 int_t SparseSolver::getRank( )
116 {
117  return -1;
118 }
119 
120 /*
121  * g e t Z e r o P i v o t s
122  */
123 returnValue SparseSolver::getZeroPivots( int_t *&zeroPivots )
124 {
125  if ( zeroPivots ) delete[] zeroPivots;
126  zeroPivots = 0;
127  return SUCCESSFUL_RETURN;
128 }
129 
130 /*****************************************************************************
131  * P R O T E C T E D *
132  *****************************************************************************/
133 
134 /*
135  * c l e a r
136  */
137 returnValue SparseSolver::clear( )
138 {
139  return SUCCESSFUL_RETURN;
140 }
141 
142 
143 /*
144  * c o p y
145  */
146 returnValue SparseSolver::copy( const SparseSolver& rhs
147  )
148 {
149  return SUCCESSFUL_RETURN;
150 }
151 
152 #ifdef SOLVER_MA27
153 
154 /*****************************************************************************
155  *****************************************************************************
156  *****************************************************************************
157  * M A 2 7 S P A R E S E S O L V E R *
158  *****************************************************************************
159  *****************************************************************************
160  *****************************************************************************/
161 
162 #define MA27ID ma27id_
163 #define MA27AD ma27ad_
164 #define MA27BD ma27bd_
165 #define MA27CD ma27cd_
166 
167 extern "C" {
168  void MA27ID(fint* ICNTL, double* CNTL);
169  void MA27AD(fint *N, fint *NZ, const fint *IRN, const fint* ICN,
170  fint *IW, fint* LIW, fint* IKEEP, fint *IW1,
171  fint* NSTEPS, fint* IFLAG, fint* ICNTL,
172  double* CNTL, fint *INFO, double* OPS);
173  void MA27BD(fint *N, fint *NZ, const fint *IRN, const fint* ICN,
174  double* A, fint* LA, fint* IW, fint* LIW,
175  fint* IKEEP, fint* NSTEPS, fint* MAXFRT,
176  fint* IW1, fint* ICNTL, double* CNTL,
177  fint* INFO);
178  void MA27CD(fint *N, double* A, fint* LA, fint* IW,
179  fint* LIW, double* W, fint* MAXFRT,
180  double* RHS, fint* IW1, fint* NSTEPS,
181  fint* ICNTL, double* CNTL);
182 }
183 
184 /*****************************************************************************
185  * P U B L I C *
186  ****************************************************************************/
187 
188 
189 /*
190  * S p a r s e S o l v e r B a s e
191  */
192 Ma27SparseSolver::Ma27SparseSolver( ) : SparseSolver()
193 {
194  a_ma27 = 0;
195  irn_ma27 = 0;
196  jcn_ma27 = 0;
197  iw_ma27 = 0;
198  ikeep_ma27 = 0;
199  clear( );
200 
201  /* Set default options for MA27 */
202  MA27ID(icntl_ma27, cntl_ma27);
203  icntl_ma27[0] = 0; // Suppress error messages
204  icntl_ma27[1] = 0; // Suppress diagnostic messages
205  cntl_ma27[0] = 1e-8; // Set pivot tolerance
206 }
207 
208 
209 /*
210  * S p a r s e S o l v e r B a s e
211  */
212 Ma27SparseSolver::Ma27SparseSolver( const Ma27SparseSolver& rhs )
213 {
214  copy( rhs );
215 }
216 
217 
218 /*
219  * ~ S p a r s e S o l v e r B a s e
220  */
221 Ma27SparseSolver::~Ma27SparseSolver( )
222 {
223  clear( );
224 }
225 
226 
227 /*
228  * o p e r a t o r =
229  */
230 Ma27SparseSolver& Ma27SparseSolver::operator=( const SparseSolver& rhs )
231 {
232  const Ma27SparseSolver* ma27_rhs = dynamic_cast<const Ma27SparseSolver*>(&rhs);
233  if (!ma27_rhs)
234  {
235  fprintf(getGlobalMessageHandler()->getOutputFile(),"Error in Ma27SparseSolver& Ma27SparseSolver::operator=( const SparseSolver& rhs )\n");
236  throw; // TODO: More elegant exit?
237  }
238  if ( this != ma27_rhs )
239  {
240  clear( );
241  SparseSolver::operator=( rhs );
242  copy( *ma27_rhs );
243  }
244 
245  return *this;
246 }
247 
248 /*
249  * s e t M a t r i x D a t a
250  */
251 returnValue Ma27SparseSolver::setMatrixData( int_t dim_,
252  int_t numNonzeros_,
253  const int_t* const irn,
254  const int_t* const jcn,
255  const real_t* const avals
256  )
257 {
258  reset( );
259  dim = dim_;
260  numNonzeros = numNonzeros_;
261 
262  if ( numNonzeros_ > 0 )
263  {
264  a_ma27 = new double[numNonzeros_];
265  irn_ma27 = new fint[numNonzeros_];
266  jcn_ma27 = new fint[numNonzeros_];
267 
268  numNonzeros=0;
269  for (int_t i=0; i<numNonzeros_; ++i)
270  if ( avals[i] != 0 )
271  {
272  a_ma27[numNonzeros] = avals[i];
273  irn_ma27[numNonzeros] = irn[i];
274  jcn_ma27[numNonzeros] = jcn[i];
275  numNonzeros++;
276  }
277  }
278  else
279  {
280  numNonzeros = 0;
281  a_ma27 = 0;
282  irn_ma27 = 0;
283  jcn_ma27 = 0;
284  }
285 
286  return SUCCESSFUL_RETURN;
287 }
288 
289 
290 /*
291  * f a c t o r i z e
292  */
293 returnValue Ma27SparseSolver::factorize( )
294 {
295  if ( dim == 0 )
296  {
297  have_factorization = true;
298  neig = 0;
299  rank = 0;
300  return SUCCESSFUL_RETURN;
301  }
302 
303  /******************************************
304  * Call MA27AD for symbolic factorization *
305  ******************************************/
306 
307  // Overstimation factor for LIW (20% recommended in MA27 documentation)
308  const double LiwFact = 2.0; // This is 200% overestimation
309  liw_ma27 = (fint)(LiwFact*(double(2*numNonzeros+3*dim+1)));
310  iw_ma27 = new fint[liw_ma27];
311 
312  ikeep_ma27 = new fint[3*dim];
313 
314  fint iflag_ma27 = 0;
315  double ops_ma27;
316  fint info_ma27[20];
317  fint* iw1_ma27 = new fint[2*dim];
318  MA27AD(&dim, &numNonzeros, irn_ma27, jcn_ma27, iw_ma27, &liw_ma27, ikeep_ma27,
319  iw1_ma27, &nsteps_ma27, &iflag_ma27, icntl_ma27, cntl_ma27,
320  info_ma27, &ops_ma27);
321 
322  /* Receive some information from MA27AD */
323  fint iflag = info_ma27[0]; // Information flag
324  fint ierror = info_ma27[1]; // Error flag
325  fint nrlnec = info_ma27[4]; // recommended value for la
326  fint nirnec = info_ma27[5]; // recommended value for liw
327  if (iflag != 0)
328  {
329  MyPrintf("MA27AD returns iflag = %d with ierror = %d\n", iflag, ierror);
330  delete [] iw1_ma27;
331  clear( );
332  return THROWERROR(RET_MATRIX_FACTORISATION_FAILED);
333  }
334 
335  /* Allocate memory for actual factorization */
336  delete [] iw_ma27;
337  double liw_init_factor = 5.0; // This could be an option.
338  liw_ma27 = (fint)(liw_init_factor * (double)(nirnec));
339  iw_ma27 = new fint[liw_ma27];
340 
341  double la_init_factor = 20.0; // This could be an option.
342  la_ma27 = getMax(numNonzeros,(fint)(la_init_factor * (double)(nrlnec)));
343  double* a_new = new double[la_ma27];
344  for (int_t i=0; i<numNonzeros; ++i)
345  a_new[i] = a_ma27[i];
346  delete [] a_ma27;
347  a_ma27 = a_new;
348 
349  /*******************************************
350  * Call MA27BD for numerical factorization *
351  *******************************************/
352 
353  MA27BD(&dim, &numNonzeros, irn_ma27, jcn_ma27, a_ma27,
354  &la_ma27, iw_ma27, &liw_ma27, ikeep_ma27, &nsteps_ma27,
355  &maxfrt_ma27, iw1_ma27, icntl_ma27, cntl_ma27, info_ma27);
356 
357  delete [] iw1_ma27;
358  /* Receive some information from MA27BD */
359  iflag = info_ma27[0]; // Information flag
360  ierror = info_ma27[1]; // Error flag
361  neig = info_ma27[14]; // Number of negative eigenvalues
362  if (iflag == 3)
363  {
364  rank = info_ma27[1];
365  return RET_KKT_MATRIX_SINGULAR;
366  }
367  else if (iflag == -5)
368  { //DJ: I think this is more severe. Can this actually happen?
369  rank = -1;
370  return RET_KKT_MATRIX_SINGULAR;
371  }
372  else if (iflag != 0)
373  {
374  MyPrintf("MA27BD returns iflag = %d with ierror = %d\n", iflag, ierror);
375  clear( );
376  return THROWERROR(RET_MATRIX_FACTORISATION_FAILED);
377  }
378  else
379  rank = dim;
380 
381  have_factorization = true;
382  return SUCCESSFUL_RETURN;
383 }
384 
385 
386 /*
387  * s o l v e
388  */
389 returnValue Ma27SparseSolver::solve( int_t dim_,
390  const real_t* const rhs,
391  real_t* const sol
392  )
393 {
394  /* consistency check */
395  if ( dim_ != dim )
396  return THROWERROR( RET_INVALID_ARGUMENTS );
397 
398  if ( !have_factorization )
399  {
400  MyPrintf("Factorization not called before solve in Ma27SparseSolver::solve.\n");
401  return THROWERROR( RET_INVALID_ARGUMENTS );
402  }
403 
404  if ( dim == 0 )
405  return SUCCESSFUL_RETURN;
406 
407  /* Call MA27CD to solve the system */
408  double* w_ma27 = new double[maxfrt_ma27];
409  fint* iw1_ma27 = new fint[nsteps_ma27];
410 
411  /* MA27CD overwrites rhs */
412  for (int_t i=0; i<dim; ++i) sol[i] = rhs[i];
413  MA27CD(&dim, a_ma27, &la_ma27, iw_ma27, &liw_ma27, w_ma27, &maxfrt_ma27,
414  sol, iw1_ma27, &nsteps_ma27, icntl_ma27, cntl_ma27);
415 
416  delete [] iw1_ma27;
417  delete [] w_ma27;
418 
419  return SUCCESSFUL_RETURN;
420 }
421 
422 /*
423  * r e s e t
424  */
425 returnValue Ma27SparseSolver::reset( )
426 {
427  /* AW: We probably want to avoid resetting factorization in QProblem */
428  if ( SparseSolver::reset( ) != SUCCESSFUL_RETURN )
429  return THROWERROR( RET_RESET_FAILED );
430 
431  clear( );
432  return SUCCESSFUL_RETURN;
433 }
434 
435 /*
436  * g e t N e g a t i v e E i g e n v a l u e s
437  */
438 int_t Ma27SparseSolver::getNegativeEigenvalues( )
439 {
440  if( !have_factorization )
441  return -1;
442  else
443  return neig;
444 }
445 
446 /*
447  * g e t R a n k
448  */
449 int_t Ma27SparseSolver::getRank( )
450 {
451  return rank;
452 }
453 
454 /*****************************************************************************
455  * P R O T E C T E D *
456  *****************************************************************************/
457 
458 /*
459  * c l e a r
460  */
461 returnValue Ma27SparseSolver::clear( )
462 {
463  delete [] a_ma27;
464  delete [] irn_ma27;
465  delete [] jcn_ma27;
466  delete [] iw_ma27;
467  delete [] ikeep_ma27;
468 
469  dim = -1;
470  numNonzeros = -1;
471  neig = -1;
472  rank = -1;
473  la_ma27 = -1;
474  a_ma27 = 0;
475  irn_ma27 = 0;
476  jcn_ma27 = 0;
477 
478  liw_ma27 = -1;
479  iw_ma27 = 0;
480  ikeep_ma27 = 0;
481  nsteps_ma27 = -1;
482  maxfrt_ma27 = -1;
483 
484  have_factorization = false;
485  return SUCCESSFUL_RETURN;
486 }
487 
488 
489 /*
490  * c o p y
491  */
492 returnValue Ma27SparseSolver::copy( const Ma27SparseSolver& rhs
493  )
494 {
495  dim = rhs.dim;
496  numNonzeros = rhs.numNonzeros;
497  la_ma27 = rhs.la_ma27;
498  if ( rhs.a_ma27 != 0 )
499  {
500  if (rhs.have_factorization)
501  {
502  a_ma27 = new double[la_ma27];
503  memcpy( a_ma27,rhs.a_ma27,la_ma27*sizeof(double) );
504  }
505  else
506  {
507  a_ma27 = new double[numNonzeros];
508  memcpy( a_ma27,rhs.a_ma27,numNonzeros*sizeof(double) );
509  }
510  }
511  else
512  a_ma27 = 0;
513 
514  if ( rhs.irn_ma27 != 0 )
515  {
516  irn_ma27 = new fint[numNonzeros];
517  memcpy( irn_ma27,rhs.irn_ma27,numNonzeros*sizeof(fint) );
518  }
519  else
520  irn_ma27 = 0;
521 
522  if ( rhs.jcn_ma27 != 0 )
523  {
524  jcn_ma27 = new fint[numNonzeros];
525  memcpy( jcn_ma27,rhs.jcn_ma27,numNonzeros*sizeof(fint) );
526  }
527  else
528  jcn_ma27 = 0;
529 
530  for ( int_t i=0; i<30; ++i)
531  icntl_ma27[i] = rhs.icntl_ma27[i];
532 
533  for ( int_t i=0; i<5; ++i)
534  cntl_ma27[i] = rhs.cntl_ma27[i];
535 
536  liw_ma27 = rhs.liw_ma27;
537 
538  if ( rhs.iw_ma27 != 0 )
539  {
540  iw_ma27 = new fint[liw_ma27];
541  memcpy( iw_ma27,rhs.iw_ma27,liw_ma27*sizeof(fint) );
542  }
543  else
544  iw_ma27 = 0;
545 
546  if ( rhs.ikeep_ma27 != 0 )
547  {
548  ikeep_ma27 = new fint[3*dim];
549  memcpy( ikeep_ma27,rhs.ikeep_ma27,3*dim*sizeof(fint) );
550  }
551  else
552  ikeep_ma27 = 0;
553 
554  nsteps_ma27 = rhs.nsteps_ma27;
555  maxfrt_ma27 = rhs.maxfrt_ma27;
556 
557  have_factorization = rhs.have_factorization;
558  neig = rhs.neig;
559  rank = rhs.rank;
560 
561  return SUCCESSFUL_RETURN;
562 }
563 
564 #endif // SOLVER_MA27
565 
566 #ifdef SOLVER_MA57
567 
568 /*****************************************************************************
569  *****************************************************************************
570  *****************************************************************************
571  * M A 5 7 S P A R E S E S O L V E R *
572  *****************************************************************************
573  *****************************************************************************
574  *****************************************************************************/
575 
576 #define MA57ID ma57id_
577 #define MA57AD ma57ad_
578 #define MA57BD ma57bd_
579 #define MA57CD ma57cd_
580 
581 extern "C"
582 {
583  /*
584  * MA57ID -- Initialize solver.
585  */
586  extern void MA57ID (
587  double *cntl,
588  fint *icntl);
589 
590  /*
591  * MA57AD -- Symbolic Factorization.
592  */
593  extern void MA57AD (
594  fint *n, /* Order of matrix. */
595  fint *ne, /* Number of entries. */
596  const fint *irn, /* Matrix nonzero row structure */
597  const fint *jcn, /* Matrix nonzero column structure */
598  fint *lkeep, /* Workspace for the pivot order of lenght 3*n */
599  fint *keep, /* Workspace for the pivot order of lenght 3*n */
600  /* Automatically iflag = 0; ikeep pivot order iflag = 1 */
601  fint *iwork, /* Integer work space. */
602  fint *icntl, /* Integer Control parameter of length 30*/
603  fint *info, /* Statistical Information; Integer array of length 20 */
604  double *rinfo);/* Double Control parameter of length 5 */
605 
606  /*
607  * MA57BD -- Numerical Factorization.
608  */
609  extern void MA57BD (
610  fint *n, /* Order of matrix. */
611  fint *ne, /* Number of entries. */
612  double *a, /* Numerical values. */
613  double *fact, /* Entries of factors. */
614  fint *lfact, /* Length of array `fact'. */
615  fint *ifact, /* Indexing info for factors. */
616  fint *lifact, /* Length of array `ifact'. */
617  fint *lkeep, /* Length of array `keep'. */
618  fint *keep, /* Integer array. */
619  fint *iwork, /* Workspace of length `n'. */
620  fint *icntl, /* Integer Control parameter of length 20. */
621  double *cntl, /* Double Control parameter of length 5. */
622  fint *info, /* Statistical Information; Integer array of length 40. */
623  double *rinfo); /* Statistical Information; Real array of length 20. */
624 
625  /*
626  * MA57CD -- Solution.
627  */
628  extern void MA57CD (
629  fint *job, /* Solution job. Solve for... */
630  /* JOB <= 1: A */
631  /* JOB == 2: PLP^t */
632  /* JOB == 3: PDP^t */
633  /* JOB >= 4: PL^t P^t */
634  fint *n, /* Order of matrix. */
635  double *fact, /* Entries of factors. */
636  fint *lfact, /* Length of array `fact'. */
637  fint *ifact, /* Indexing info for factors. */
638  fint *lifact, /* Length of array `ifact'. */
639  fint *nrhs, /* Number of right hand sides. */
640  double *rhs, /* Numerical Values. */
641  fint *lrhs, /* Leading dimensions of `rhs'. */
642  double *work, /* Real workspace. */
643  fint *lwork, /* Length of `work', >= N*NRHS. */
644  fint *iwork, /* Integer array of length `n'. */
645  fint *icntl, /* Integer Control parameter array of length 20. */
646  fint *info); /* Statistical Information; Integer array of length 40. */
647 }
648 
649 /*****************************************************************************
650  * P U B L I C *
651  ****************************************************************************/
652 
653 
654 /*
655  * S p a r s e S o l v e r B a s e
656  */
657 Ma57SparseSolver::Ma57SparseSolver( ) : SparseSolver()
658 {
659  a_ma57 = 0;
660  irn_ma57 = 0;
661  jcn_ma57 = 0;
662  fact_ma57 = 0;
663  ifact_ma57 = 0;
664  pivots = 0;
665  clear( );
666 
667  /* Set default options for MA57 */
668  MA57ID( cntl_ma57, icntl_ma57 );
669 
670  icntl_ma57[0] = -1; // Suppress error messages
671  icntl_ma57[1] = -1; // Suppress warning messages
672  icntl_ma57[2] = -1; // Suppress monitoring messages
673  //icntl_ma57[4] = 4; // Print everything (for debugging)
674  icntl_ma57[15] = 1; // Place small pivots at the end of the factorization (default: 0)
675 
677  //cntl_ma57[1] = 5.0e-16; // Pivots smaller than this are treated as zero and are placed at the end of the factorization (default: 1e-20)
678  //cntl_ma57[0] = 0.5; // Set pivot tolerance: Higher values = more stable but slower/less sparse (default: 0.01, max 0.5)
679 }
680 
681 
682 /*
683  * S p a r s e S o l v e r B a s e
684  */
685 Ma57SparseSolver::Ma57SparseSolver( const Ma57SparseSolver& rhs )
686 {
687  copy( rhs );
688 }
689 
690 
691 /*
692  * ~ S p a r s e S o l v e r B a s e
693  */
694 Ma57SparseSolver::~Ma57SparseSolver( )
695 {
696  clear( );
697 }
698 
699 
700 /*
701  * o p e r a t o r =
702  */
703 Ma57SparseSolver& Ma57SparseSolver::operator=( const SparseSolver& rhs )
704 {
705  const Ma57SparseSolver* ma57_rhs = dynamic_cast<const Ma57SparseSolver*>(&rhs);
706  if (!ma57_rhs)
707  {
708  fprintf(getGlobalMessageHandler()->getOutputFile(),"Error in Ma57SparseSolver& Ma57SparseSolver::operator=( const SparseSolver& rhs )\n");
709  throw; // TODO: More elegant exit?
710  }
711  if ( this != ma57_rhs )
712  {
713  clear( );
714  SparseSolver::operator=( rhs );
715  copy( *ma57_rhs );
716  }
717 
718  return *this;
719 }
720 
721 /*
722  * s e t M a t r i x D a t a
723  */
724 returnValue Ma57SparseSolver::setMatrixData( int_t dim_,
725  int_t numNonzeros_,
726  const int_t* const irn,
727  const int_t* const jcn,
728  const real_t* const avals
729  )
730 {
731  reset( );
732  dim = dim_;
733  numNonzeros = numNonzeros_;
734 
735  if ( numNonzeros_ > 0 )
736  {
737  a_ma57 = new double[numNonzeros_];
738  irn_ma57 = new fint[numNonzeros_];
739  jcn_ma57 = new fint[numNonzeros_];
740 
741  numNonzeros=0;
742  for (int_t i=0; i<numNonzeros_; ++i)
743  if ( isZero(avals[i]) == BT_FALSE )
744  {
745  a_ma57[numNonzeros] = avals[i];
746  irn_ma57[numNonzeros] = irn[i];
747  jcn_ma57[numNonzeros] = jcn[i];
748  numNonzeros++;
749  }
750  }
751  else
752  {
753  numNonzeros = 0;
754  a_ma57 = 0;
755  irn_ma57 = 0;
756  jcn_ma57 = 0;
757  }
758 
759  return SUCCESSFUL_RETURN;
760 }
761 
762 
763 /*
764  * f a c t o r i z e
765  */
766 returnValue Ma57SparseSolver::factorize( )
767 {
768  if ( dim == 0 )
769  {
770  have_factorization = true;
771  neig = 0;
772  rank = 0;
773  return SUCCESSFUL_RETURN;
774  }
775 
776  /******************************************
777  * Call MA57AD for symbolic factorization *
778  ******************************************/
779 
780  fint lkeep_ma57 = 5*dim + numNonzeros + getMax(numNonzeros,dim) + 42;
781  fint *keep_ma57 = new fint[lkeep_ma57];
782  fint *iwork_ma57 = new fint[5*dim];
783 
784  fint info_ma57[40];
785  double rinfo_ma57[20];
786 
787  MA57AD(&dim, &numNonzeros, irn_ma57, jcn_ma57, &lkeep_ma57, keep_ma57,
788  iwork_ma57, icntl_ma57, info_ma57, rinfo_ma57);
789 
790  /* Receive some information from MA57AD */
791  fint iflag = info_ma57[0]; // Information flag
792  fint ierror = info_ma57[1]; // Error flag
793  if (iflag != 0)
794  {
795  MyPrintf("MA57AD returns iflag = %d with ierror = %d\n", iflag, ierror);
796  delete [] keep_ma57;
797  delete [] iwork_ma57;
798  clear( );
799  return THROWERROR(RET_MATRIX_FACTORISATION_FAILED);
800  }
801 
802  /* Allocate memory for actual factorization */
803  double lfact_factor = 10.0; // This could be an option
804 
805  lfact_ma57 = (fint)(lfact_factor * (double)(info_ma57[8]));
806  fact_ma57 = new double[lfact_ma57];
807 
808  lifact_ma57 = (fint)(lfact_factor * (double)(info_ma57[9]));
809  ifact_ma57 = new int_t[lifact_ma57];
810 
811  /*******************************************
812  * Call MA57BD for numerical factorization *
813  *******************************************/
814 
815  MA57BD( &dim, &numNonzeros, a_ma57, fact_ma57, &lfact_ma57,
816  ifact_ma57, &lifact_ma57, &lkeep_ma57, keep_ma57,
817  iwork_ma57, icntl_ma57, cntl_ma57, info_ma57, rinfo_ma57 );
818 
819  delete [] iwork_ma57;
820  delete [] keep_ma57;
821 
822  /* Receive some information from MA57BD */
823  iflag = info_ma57[0]; // Information flag
824  ierror = info_ma57[1]; // Error flag
825  neig = info_ma57[23]; // Number of negative eigenvalues
826  rank = info_ma57[24]; // Rank of matrix
827 
828  /* Read pivot sequence (see MA57UD source code) */
829  pivots = new fint[dim];
830  fint nrows, ncols;
831  fint nblk = ifact_ma57[2];
832  int_t iwpos = 3; // = 4-1
833  int_t k, kk, count = 0;
834  for ( k=0; k<nblk; k++ )
835  {
836  ncols = ifact_ma57[iwpos];
837  nrows = ifact_ma57[iwpos+1];
838 
839  for ( kk=0; kk<nrows; kk++ )
840  pivots[count++] = ifact_ma57[iwpos+2+kk]-1; //convert Fortran to C indices!
841 
842  iwpos = iwpos+ncols+2;
843  }
844 
845  if (iflag == 4)
846  {
847  //MyPrintf("dim = %i, rank = %i. Pivots: ", dim, rank);
848  //for( k=rank; k<dim; k++ )
849  //MyPrintf("%i ", pivots[k]);
850  //MyPrintf("\n");
851 
852  return RET_KKT_MATRIX_SINGULAR;
853  }
854  else if (iflag != 0)
855  {
856  MyPrintf("MA57BD returns iflag = %d with ierror = %d\n", iflag, ierror);
857  clear( );
858  return THROWERROR(RET_MATRIX_FACTORISATION_FAILED);
859  }
860 
861  have_factorization = true;
862  return SUCCESSFUL_RETURN;
863 }
864 
865 
866 /*
867  * s o l v e
868  */
869 returnValue Ma57SparseSolver::solve( int_t dim_,
870  const real_t* const rhs,
871  real_t* const sol
872  )
873 {
874  /* consistency check */
875  if ( dim_ != dim )
876  return THROWERROR( RET_INVALID_ARGUMENTS );
877 
878  if ( !have_factorization )
879  {
880  MyPrintf("Factorization not called before solve in Ma57SparseSolver::solve.\n");
881  return THROWERROR( RET_INVALID_ARGUMENTS );
882  }
883 
884  if ( dim == 0 )
885  return SUCCESSFUL_RETURN;
886 
887  /* Call MA57CD to solve the system */
888  fint job_ma57 = 1;
889  fint nrhs_ma57 = 1;
890  fint lrhs_ma57 = dim;
891  fint info_ma57[40];
892 
893  fint lwork_ma57 = dim*nrhs_ma57;
894  double* work_ma57 = new double[lwork_ma57];
895  fint* iwork_ma57 = new fint[dim];
896 
897  /* MA57CD overwrites rhs */
898  for (int_t i=0; i<dim; ++i) sol[i] = rhs[i];
899  MA57CD(&job_ma57, &dim, fact_ma57, &lfact_ma57, ifact_ma57, &lifact_ma57,
900  &nrhs_ma57, sol, &lrhs_ma57, work_ma57, &lwork_ma57, iwork_ma57,
901  icntl_ma57, info_ma57);
902 
903  delete [] work_ma57;
904  delete [] iwork_ma57;
905 
906  fint iflag = info_ma57[0]; // Information flag
907  fint ierror = info_ma57[1]; // Error flag
908  if (iflag != 0)
909  {
910  MyPrintf("MA57CD returns iflag = %d with ierror = %d\n", iflag, ierror);
911  clear( );
912  return THROWERROR(RET_MATRIX_FACTORISATION_FAILED);
913  }
914 
915  return SUCCESSFUL_RETURN;
916 }
917 
918 /*
919  * r e s e t
920  */
921 returnValue Ma57SparseSolver::reset( )
922 {
923  /* AW: We probably want to avoid resetting factorization in QProblem */
924  if ( SparseSolver::reset( ) != SUCCESSFUL_RETURN )
925  return THROWERROR( RET_RESET_FAILED );
926 
927  clear( );
928  return SUCCESSFUL_RETURN;
929 }
930 
931 /*
932  * g e t N e g a t i v e E i g e n v a l u e s
933  */
934 int_t Ma57SparseSolver::getNegativeEigenvalues( )
935 {
936  if( !have_factorization )
937  return -1;
938  else
939  return neig;
940 }
941 
942 /*
943  * g e t R a n k
944  */
945 int_t Ma57SparseSolver::getRank( )
946 {
947  return rank;
948 }
949 
950 /*
951  * g e t Z e r o P i v o t s
952  */
953 returnValue Ma57SparseSolver::getZeroPivots( int_t *&zeroPivots )
954 {
955  for ( int_t k=0; k<dim-rank; k++ )
956  zeroPivots[k] = pivots[rank+k];
957 
958  return SUCCESSFUL_RETURN;
959 }
960 
961 /*****************************************************************************
962  * P R O T E C T E D *
963  *****************************************************************************/
964 
965 /*
966  * c l e a r
967  */
968 returnValue Ma57SparseSolver::clear( )
969 {
970  delete [] a_ma57;
971  delete [] irn_ma57;
972  delete [] jcn_ma57;
973  delete [] fact_ma57;
974  delete [] ifact_ma57;
975  delete [] pivots;
976 
977  dim = -1;
978  numNonzeros = -1;
979  neig = -1;
980  rank = -1;
981  pivots = 0;
982 
983  a_ma57 = 0;
984  irn_ma57 = 0;
985  jcn_ma57 = 0;
986 
987  fact_ma57 = 0;
988  lfact_ma57 = -1;
989  ifact_ma57 = 0;
990  lifact_ma57 = -1;
991 
992  have_factorization = false;
993  return SUCCESSFUL_RETURN;
994 }
995 
996 
997 /*
998  * c o p y
999  */
1000 returnValue Ma57SparseSolver::copy( const Ma57SparseSolver& rhs
1001  )
1002 {
1003  dim = rhs.dim;
1004  numNonzeros = rhs.numNonzeros;
1005  neig = rhs.neig;
1006  rank = rhs.rank;
1007  have_factorization = rhs.have_factorization;
1008 
1009  if ( rhs.a_ma57 != 0 )
1010  {
1011  a_ma57 = new double[numNonzeros];
1012  memcpy( a_ma57,rhs.a_ma57,numNonzeros*sizeof(double) );
1013  }
1014  else
1015  a_ma57 = 0;
1016 
1017  if ( rhs.irn_ma57 != 0 )
1018  {
1019  irn_ma57 = new fint[numNonzeros];
1020  memcpy( irn_ma57,rhs.irn_ma57,numNonzeros*sizeof(fint) );
1021  }
1022  else
1023  irn_ma57 = 0;
1024 
1025  if ( rhs.jcn_ma57 != 0 )
1026  {
1027  jcn_ma57 = new fint[numNonzeros];
1028  memcpy( jcn_ma57,rhs.jcn_ma57,numNonzeros*sizeof(fint) );
1029  }
1030  else
1031  jcn_ma57 = 0;
1032 
1033  for ( int_t i=0; i<30; ++i)
1034  icntl_ma57[i] = rhs.icntl_ma57[i];
1035 
1036  for ( int_t i=0; i<5; ++i)
1037  cntl_ma57[i] = rhs.cntl_ma57[i];
1038 
1039  lfact_ma57 = rhs.lfact_ma57;
1040  if ( rhs.fact_ma57 != 0 )
1041  {
1042  fact_ma57 = new double[lfact_ma57];
1043  memcpy( fact_ma57,rhs.fact_ma57,lfact_ma57*sizeof(double) );
1044  }
1045  else
1046  fact_ma57 = 0;
1047 
1048  lifact_ma57 = rhs.lifact_ma57;
1049  if ( rhs.ifact_ma57 != 0 )
1050  {
1051  ifact_ma57 = new fint[lifact_ma57];
1052  memcpy( ifact_ma57,rhs.ifact_ma57,lifact_ma57*sizeof(fint) );
1053  }
1054  else
1055  ifact_ma57 = 0;
1056 
1057  if ( have_factorization )
1058  {
1059  pivots = new fint[dim];
1060  memcpy( pivots, rhs.pivots, dim*sizeof(fint) );
1061  }
1062  else
1063  pivots = 0;
1064 
1065  return SUCCESSFUL_RETURN;
1066 }
1067 
1068 #endif // SOLVER_MA57
1069 
1070 
1071 #ifdef SOLVER_NONE
1072 
1073 returnValue DummySparseSolver::setMatrixData( int_t dim,
1074  int_t numNonzeros,
1075  const int_t* const airn,
1076  const int_t* const acjn,
1077  const real_t* const avals
1078  )
1079 {
1080  return THROWERROR(RET_NO_SPARSE_SOLVER);
1081 }
1082 
1083 returnValue DummySparseSolver::factorize( )
1084 {
1085  return THROWERROR(RET_NO_SPARSE_SOLVER);
1086 }
1087 
1088 returnValue DummySparseSolver::solve( int_t dim,
1089  const real_t* const rhs,
1090  real_t* const sol
1091  )
1092 {
1093  return THROWERROR(RET_NO_SPARSE_SOLVER);
1094 }
1095 
1096 #endif // SOLVER_NONE
1097 
1098 END_NAMESPACE_QPOASES
1099 
1100 
1101 /*
1102  * end of file
1103  */
getMax
int getMax(int x, int y)
N
#define N
Definition: crane_kul_mhe_test.cpp:44
RET_MATRIX_FACTORISATION_FAILED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:166
RET_RESET_FAILED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:99
SparseSolver::operator=
virtual SparseSolver & operator=(const SparseSolver &rhs)
Definition: SparseSolver.cpp:85
isZero
BooleanType isZero(real_t x, real_t TOL=ZERO)
SUCCESSFUL_RETURN
Definition: acado_types.hpp:836
RET_INVALID_ARGUMENTS
Definition: acado_types.hpp:839
MyPrintf
void MyPrintf(const char *pformat,...)
Definition: SQProblemSchur.cpp:42
returnValue
Allows to pass back messages to the calling function.
Definition: acado_types.hpp:1139
THROWERROR
#define THROWERROR(retval)
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:391
fint
int fint
Definition: external_packages/qpOASES-3.2.0/include/qpOASES/Types.hpp:180
RET_KKT_MATRIX_SINGULAR
Definition: external_packages/qpOASES-3.2.0/include/qpOASES/MessageHandling.hpp:221
SparseSolver::getRank
virtual int_t getRank()
Definition: SparseSolver.cpp:115
int_t
int int_t
Definition: external_packages/qpOASES-3.2.0/include/qpOASES/Types.hpp:174
SparseSolver::~SparseSolver
virtual ~SparseSolver()
Definition: sparse_solver.cpp:52
SparseSolver::getNegativeEigenvalues
virtual int_t getNegativeEigenvalues()
Definition: SparseSolver.cpp:107
rhs
void rhs(const real_t *x, real_t *f)
Definition: examples/code_generation/simulation/externModel_export/model.c:5
END_NAMESPACE_QPOASES
#define END_NAMESPACE_QPOASES
Definition: external_packages/qpOASES-3.0beta/include/qpOASES/Types.hpp:120
SparseSolver
Generic interface for sparse solvers to be coupled with ACADO Toolkit.
Definition: sparse_solver.hpp:66
SparseSolver::reset
virtual returnValue reset()
Definition: SparseSolver.cpp:99
SparseSolver::clear
returnValue clear()
Definition: SparseSolver.cpp:137
BT_FALSE
#define BT_FALSE
Definition: acado_types.hpp:49
RET_NO_SPARSE_SOLVER
Definition: external_packages/qpOASES-3.2.0/include/qpOASES/MessageHandling.hpp:224
BEGIN_NAMESPACE_QPOASES
#define BEGIN_NAMESPACE_QPOASES
Definition: external_packages/qpOASES-3.0beta/include/qpOASES/Types.hpp:117
real_t
double real_t
Definition: AD_test.c:10
SparseSolver::copy
returnValue copy(const SparseSolver &rhs)
Definition: SparseSolver.cpp:146
getGlobalMessageHandler
MessageHandling * getGlobalMessageHandler()
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/SRC/MessageHandling.cpp:515
SparseSolver::getZeroPivots
virtual returnValue getZeroPivots(int_t *&zeroPivots)
Definition: SparseSolver.cpp:123

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