Utils.c
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  * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka,
6  * Christian Kirches et al. 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 
35 #if defined(__WIN32__) || defined(WIN32)
36  #include <windows.h>
37 #elif defined(LINUX) || defined(__LINUX__)
38  #include <sys/stat.h>
39  #include <sys/time.h>
40 #endif
41 
42 #ifdef __MATLAB__
43  #include "mex.h"
44 #endif
45 
46 
47 #include <qpOASES_e/Utils.h>
48 
49 
50 #ifdef __NO_SNPRINTF__
51 #if (!defined(_MSC_VER)) || defined(__DSPACE__) || defined(__XPCTARGET__)
52 /* If snprintf is not available, provide an empty implementation. */
53 int snprintf( char* s, size_t n, const char* format, ... )
54 {
55  if ( n > 0 )
56  s[0] = '\0';
57 
58  return 0;
59 }
60 #endif
61 #endif /* __NO_SNPRINTF__ */
62 
63 
65 
66 
67 /*
68  * p r i n t
69  */
71  int n
72  )
73 {
74  #ifndef __SUPPRESSANYOUTPUT__
75 
76  int i;
77  char myPrintfString[QPOASES_MAX_STRING_LENGTH];
78 
79  /* Print a vector. */
80  for( i=0; i<n; ++i )
81  {
82  snprintf( myPrintfString,QPOASES_MAX_STRING_LENGTH," %.16e\t", v[i] );
83  qpOASES_myPrintf( myPrintfString );
84  }
85  qpOASES_myPrintf( "\n" );
86 
87  #endif /* __SUPPRESSANYOUTPUT__ */
88 
89  return SUCCESSFUL_RETURN;
90 }
91 
92 
93 /*
94  * p r i n t
95  */
96 returnValue qpOASES_printPV( const real_t* const v, int n,
97  const int* const V_idx
98  )
99 {
100  #ifndef __SUPPRESSANYOUTPUT__
101 
102  int i;
103  char myPrintfString[QPOASES_MAX_STRING_LENGTH];
104 
105  /* Print a permuted vector. */
106  qpOASES_myPrintf( "\t" );
107  for( i=0; i<n; ++i )
108  {
109  snprintf( myPrintfString,QPOASES_MAX_STRING_LENGTH," %.16e\t", v[ V_idx[i] ] );
110  qpOASES_myPrintf( myPrintfString );
111  }
112  qpOASES_myPrintf( "\n" );
113 
114  #endif /* __SUPPRESSANYOUTPUT__ */
115 
116  return SUCCESSFUL_RETURN;
117 }
118 
119 
120 /*
121  * p r i n t
122  */
123 returnValue qpOASES_printNV( const real_t* const v, int n,
124  const char* name
125  )
126 {
127  #ifndef __SUPPRESSANYOUTPUT__
128 
129  char myPrintfString[QPOASES_MAX_STRING_LENGTH];
130 
131  /* Print vector name ... */
132  if ( name != 0 )
133  {
134  snprintf( myPrintfString,QPOASES_MAX_STRING_LENGTH,"%s = ", name );
135  qpOASES_myPrintf( myPrintfString );
136  }
137 
138  #endif /* __SUPPRESSANYOUTPUT__ */
139 
140  /* ... and the vector itself. */
141  return qpOASES_printV( v, n );
142 }
143 
144 /*
145  * p r i n t
146  */
148  int nrow,
149  int ncol
150  )
151 {
152  #ifndef __SUPPRESSANYOUTPUT__
153 
154  int i;
155 
156  /* Print a matrix as a collection of row vectors. */
157  for( i=0; i<nrow; ++i )
158  qpOASES_printV( &(M[i*ncol]), ncol );
159  qpOASES_myPrintf( "\n" );
160 
161  #endif /* __SUPPRESSANYOUTPUT__ */
162 
163  return SUCCESSFUL_RETURN;
164 }
165 
166 
167 /*
168  * p r i n t
169  */
170 returnValue qpOASES_printPM( const real_t* const M, int nrow, int ncol,
171  const int* const ROW_idx, const int* const COL_idx
172  )
173 {
174  #ifndef __SUPPRESSANYOUTPUT__
175 
176  int i;
177 
178  /* Print a permuted matrix as a collection of permuted row vectors. */
179  for( i=0; i<nrow; ++i )
180  qpOASES_printPV( &( M[ ROW_idx[i]*ncol ] ), ncol, COL_idx );
181  qpOASES_myPrintf( "\n" );
182 
183  #endif /* __SUPPRESSANYOUTPUT__ */
184 
185  return SUCCESSFUL_RETURN;
186 }
187 
188 
189 /*
190  * p r i n t
191  */
192 returnValue qpOASES_printNM( const real_t* const M, int nrow, int ncol,
193  const char* name
194  )
195 {
196  #ifndef __SUPPRESSANYOUTPUT__
197 
198  char myPrintfString[QPOASES_MAX_STRING_LENGTH];
199 
200  /* Print matrix name ... */
201  if ( name != 0 )
202  {
203  snprintf( myPrintfString,QPOASES_MAX_STRING_LENGTH,"%s = ", name );
204  qpOASES_myPrintf( myPrintfString );
205  }
206 
207  #endif /* __SUPPRESSANYOUTPUT__ */
208 
209  /* ... and the matrix itself. */
210  return qpOASES_printM( M, nrow, ncol );
211 }
212 
213 
214 /*
215  * p r i n t
216  */
217 returnValue qpOASES_printI( const int* const _index,
218  int n
219  )
220 {
221  #ifndef __SUPPRESSANYOUTPUT__
222 
223  int i;
224  char myPrintfString[QPOASES_MAX_STRING_LENGTH];
225 
226  /* Print a indexlist. */
227  qpOASES_myPrintf( "\t" );
228  for( i=0; i<n; ++i )
229  {
230  snprintf( myPrintfString,QPOASES_MAX_STRING_LENGTH," %d\t", _index[i] );
231  qpOASES_myPrintf( myPrintfString );
232  }
233  qpOASES_myPrintf( "\n" );
234 
235  #endif /* __SUPPRESSANYOUTPUT__ */
236 
237  return SUCCESSFUL_RETURN;
238 }
239 
240 
241 /*
242  * p r i n t
243  */
244 returnValue qpOASES_printNI( const int* const _index, int n,
245  const char* name
246  )
247 {
248  #ifndef __SUPPRESSANYOUTPUT__
249 
250  char myPrintfString[QPOASES_MAX_STRING_LENGTH];
251 
252  /* Print indexlist name ... */
253  if ( name != 0 )
254  {
255  snprintf( myPrintfString,QPOASES_MAX_STRING_LENGTH,"%s = ", name );
256  qpOASES_myPrintf( myPrintfString );
257  }
258 
259  #endif /* __SUPPRESSANYOUTPUT__ */
260 
261  /* ... and the indexlist itself. */
262  return qpOASES_printI( _index, n );
263 }
264 
265 
266 /*
267  * m y P r i n t f
268  */
270 {
271  #ifndef __SUPPRESSANYOUTPUT__
272 
273  #ifdef __MATLAB__
274  if ( s == 0 )
275  return RET_INVALID_ARGUMENTS;
276  mexPrintf( s );
277  #else
278  FILE* outputfile;
279  if ( s == 0 )
280  return RET_INVALID_ARGUMENTS;
282  if ( outputfile == 0 )
284  fprintf( outputfile, "%s", s );
285  #endif /* __MATLAB__ */
286 
287  #endif /* __SUPPRESSANYOUTPUT__ */
288 
289  return SUCCESSFUL_RETURN;
290 }
291 
292 
293 /*
294  * p r i n t C o p y r i g h t N o t i c e
295  */
297 {
298  #ifndef __SUPPRESSANYOUTPUT__
299  #ifndef __XPCTARGET__
300  #ifndef __DSPACE__
301  #ifndef __NO_COPYRIGHT__
302  qpOASES_myPrintf( "\nqpOASES_embedded -- An Embedded Implementation of the Online Active Set Strategy.\nCopyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka,\nChristian Kirches et al. All rights reserved.\n\nqpOASES is distributed under the terms of the \nGNU Lesser General Public License 2.1 in the hope that it will be \nuseful, but WITHOUT ANY WARRANTY; without even the implied warranty \nof MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. \nSee the GNU Lesser General Public License for more details.\n\n" );
303  #endif /* __NO_COPYRIGHT__ */
304  #endif /* __DSPACE__ */
305  #endif /* __XPCTARGET__ */
306  #endif /* __SUPPRESSANYOUTPUT__ */
307  return SUCCESSFUL_RETURN;
308 }
309 
310 
311 /*
312  * r e a d F r o m F i l e
313  */
314 returnValue qpOASES_readFromFileM( real_t* data, int nrow, int ncol,
315  const char* datafilename
316  )
317 {
318  #ifndef __SUPPRESSANYOUTPUT__
319 
320  int i, j;
321  real_t float_data;
322  FILE* datafile;
323  char errstr[QPOASES_MAX_STRING_LENGTH];
324 
325  /* 1) Open file. */
326  if ( ( datafile = fopen( datafilename, "r" ) ) == 0 )
327  {
328  snprintf( errstr,QPOASES_MAX_STRING_LENGTH,"(%s)",datafilename );
330  }
331 
332  /* 2) Read data from file. */
333  for( i=0; i<nrow; ++i )
334  {
335  for( j=0; j<ncol; ++j )
336  {
337  #ifdef __USE_SINGLE_PRECISION__
338  if ( fscanf( datafile, "%f ", &float_data ) == 0 )
339  #else
340  if ( fscanf( datafile, "%lf ", &float_data ) == 0 )
341  #endif /* __USE_SINGLE_PRECISION__ */
342  {
343  fclose( datafile );
344  snprintf( errstr,QPOASES_MAX_STRING_LENGTH,"(%s)",datafilename );
346  }
347  data[i*ncol + j] = ( (real_t) float_data );
348  }
349  }
350 
351  /* 3) Close file. */
352  fclose( datafile );
353 
354  return SUCCESSFUL_RETURN;
355 
356  #else
357 
359 
360  #endif /* __SUPPRESSANYOUTPUT__ */
361 }
362 
363 
364 /*
365  * r e a d F r o m F i l e
366  */
368  const char* datafilename
369  )
370 {
371  return qpOASES_readFromFileM( data, n, 1, datafilename );
372 }
373 
374 
375 
376 /*
377  * r e a d F r o m F i l e
378  */
380  const char* datafilename
381  )
382 {
383  #ifndef __SUPPRESSANYOUTPUT__
384 
385  int i;
386  FILE* datafile;
387  char errstr[QPOASES_MAX_STRING_LENGTH];
388 
389  /* 1) Open file. */
390  if ( ( datafile = fopen( datafilename, "r" ) ) == 0 )
391  {
392  snprintf( errstr,QPOASES_MAX_STRING_LENGTH,"(%s)",datafilename );
394  }
395 
396  /* 2) Read data from file. */
397  for( i=0; i<n; ++i )
398  {
399  if ( fscanf( datafile, "%d\n", &(data[i]) ) == 0 )
400  {
401  fclose( datafile );
402  snprintf( errstr,QPOASES_MAX_STRING_LENGTH,"(%s)",datafilename );
404  }
405  }
406 
407  /* 3) Close file. */
408  fclose( datafile );
409 
410  return SUCCESSFUL_RETURN;
411 
412  #else
413 
415 
416  #endif /* __SUPPRESSANYOUTPUT__ */
417 }
418 
419 
420 /*
421  * w r i t e I n t o F i l e
422  */
423 returnValue qpOASES_writeIntoFileM( const real_t* const data, int nrow, int ncol,
424  const char* datafilename, BooleanType append
425  )
426 {
427  #ifndef __SUPPRESSANYOUTPUT__
428 
429  int i, j;
430  FILE* datafile;
431  char errstr[QPOASES_MAX_STRING_LENGTH];
432 
433  /* 1) Open file. */
434  if ( append == BT_TRUE )
435  {
436  /* append data */
437  if ( ( datafile = fopen( datafilename, "a" ) ) == 0 )
438  {
439  snprintf( errstr,QPOASES_MAX_STRING_LENGTH,"(%s)",datafilename );
441  }
442  }
443  else
444  {
445  /* do not append data */
446  if ( ( datafile = fopen( datafilename, "w" ) ) == 0 )
447  {
448  snprintf( errstr,QPOASES_MAX_STRING_LENGTH,"(%s)",datafilename );
450  }
451  }
452 
453  /* 2) Write data into file. */
454  for( i=0; i<nrow; ++i )
455  {
456  for( j=0; j<ncol; ++j )
457  fprintf( datafile, "%.16e ", data[i*ncol+j] );
458 
459  fprintf( datafile, "\n" );
460  }
461 
462  /* 3) Close file. */
463  fclose( datafile );
464 
465  return SUCCESSFUL_RETURN;
466 
467  #else /* __SUPPRESSANYOUTPUT__ */
468 
470 
471  #endif
472 }
473 
474 
475 /*
476  * w r i t e I n t o F i l e
477  */
478 returnValue qpOASES_writeIntoFileV( const real_t* const data, int n,
479  const char* datafilename, BooleanType append
480  )
481 {
482  return qpOASES_writeIntoFileM( data,1,n,datafilename,append );
483 }
484 
485 
486 /*
487  * w r i t e I n t o F i l e
488  */
489 returnValue qpOASES_writeIntoFileI( const int* const integer, int n,
490  const char* datafilename, BooleanType append
491  )
492 {
493  #ifndef __SUPPRESSANYOUTPUT__
494 
495  int i;
496 
497  FILE* datafile;
498  char errstr[QPOASES_MAX_STRING_LENGTH];
499 
500  /* 1) Open file. */
501  if ( append == BT_TRUE )
502  {
503  /* append data */
504  if ( ( datafile = fopen( datafilename, "a" ) ) == 0 )
505  {
506  snprintf( errstr,QPOASES_MAX_STRING_LENGTH,"(%s)",datafilename );
508  }
509  }
510  else
511  {
512  /* do not append data */
513  if ( ( datafile = fopen( datafilename, "w" ) ) == 0 )
514  {
515  snprintf( errstr,QPOASES_MAX_STRING_LENGTH,"(%s)",datafilename );
517  }
518  }
519 
520  /* 2) Write data into file. */
521  for( i=0; i<n; ++i )
522  fprintf( datafile, "%d\n", integer[i] );
523 
524  /* 3) Close file. */
525  fclose( datafile );
526 
527  return SUCCESSFUL_RETURN;
528 
529  #else /* __SUPPRESSANYOUTPUT__ */
530 
532 
533  #endif
534 }
535 
536 
537 /*
538  * w r i t e I n t o M a t F i l e
539  */
541  const double* const data, int nRows, int nCols, const char* name
542  )
543 {
544  /* Note, this code snippet has been inspired from the document
545  * "Matlab(R) MAT-file Format, R2013b" by MathWorks */
546 
547  #ifndef __SUPPRESSANYOUTPUT__
548 
549  int ii, jj;
550  MatMatrixHeader var;
551 
552  if ( ( matFile == 0 ) || ( data == 0 ) || ( nRows < 0 ) || ( nCols < 0 ) || ( name == 0 ) )
553  return RET_INVALID_ARGUMENTS;
554 
555  /* setup variable header */
556  var.numericFormat = 0000; /* IEEE Little Endian - reserved - double precision (64 bits) - numeric full matrix */
557  var.nRows = nRows; /* number of rows */
558  var.nCols = nCols; /* number of columns */
559  var.imaginaryPart = 0; /* no imaginary part */
560  var.nCharName = (long)(strlen(name))+1; /* matrix name length */
561 
562  /* write variable header to mat file */
563  if ( fwrite( &var, sizeof(MatMatrixHeader),1, matFile ) < 1 )
565 
566  if ( fwrite( name, sizeof(char),(unsigned long)(var.nCharName), matFile ) < 1 )
568 
569  for ( ii=0; ii<nCols; ++ii )
570  for ( jj=0; jj<nRows; ++jj )
571  if ( fwrite( &(data[jj*nCols+ii]), sizeof(double),1, matFile ) < 1 )
573 
574  return SUCCESSFUL_RETURN;
575 
576  #else /* __SUPPRESSANYOUTPUT__ */
577 
579 
580  #endif
581 }
582 
583 
584 /*
585  * w r i t e I n t o M a t F i l e
586  */
588  const int* const data, int nRows, int nCols, const char* name
589  )
590 {
591  int ii, jj;
592 
593  myStatic real_t realData[NVMAX*NCMAX]; /* TODO: fix this!! */
594 
595  for ( ii=0; ii<nRows; ++ii )
596  for ( jj=0; jj<nCols; ++jj )
597  realData[ ii*nCols+jj ] = (real_t) data[ ii*nCols+jj ];
598 
599  return qpOASES_writeIntoMatFile( matFile,realData,nRows,nCols,name );
600 }
601 
602 
603 /*
604  * g e t C P U t i m e
605  */
607 {
608  real_t current_time = -1.0;
609 
610  #if defined(__WIN32__) || defined(WIN32)
611  LARGE_INTEGER counter, frequency;
612  QueryPerformanceFrequency(&frequency);
613  QueryPerformanceCounter(&counter);
614  current_time = ((real_t) counter.QuadPart) / ((real_t) frequency.QuadPart);
615  #elif defined(LINUX) || defined(__LINUX__)
616  struct timeval theclock;
617  gettimeofday( &theclock,0 );
618  current_time = 1.0*theclock.tv_sec + 1.0e-6*theclock.tv_usec;
619  #endif
620 
621  return current_time;
622 }
623 
624 
625 
626 /*
627  * g e t N o r m
628  */
629 real_t qpOASES_getNorm( const real_t* const v, int n, int type )
630 {
631  int i;
632 
633  real_t norm = 0.0;
634 
635  switch ( type )
636  {
637  case 2:
638  for( i=0; i<n; ++i )
639  norm += v[i]*v[i];
640  return qpOASES_getSqrt( norm );
641 
642  case 1:
643  for( i=0; i<n; ++i )
644  norm += qpOASES_getAbs( v[i] );
645  return norm;
646 
647  default:
649  return -QPOASES_INFTY;
650  }
651 }
652 
653 
654 
655 /*
656  * g e t K k t V i o l a t i o n
657  */
659  const real_t* const H, const real_t* const g,
660  const real_t* const A, const real_t* const lb, const real_t* const ub,
661  const real_t* const lbA, const real_t* const ubA,
662  const real_t* const x, const real_t* const y,
663  real_t* const _stat, real_t* const feas, real_t* const cmpl
664  )
665 {
666  /* Tolerance for dual variables considered zero. */
667  const real_t dualActiveTolerance = 1.0e3 * QPOASES_EPS;
668 
669  int i, j;
670  real_t sum, prod;
671 
672  /* Initialize residuals */
673  *_stat = *feas = *cmpl = 0.0;
674 
675  /* check stationarity */
676  for (i = 0; i < nV; i++)
677  {
678  /* g term and variable bounds dual term */
679  if ( g != 0 )
680  sum = g[i] - y[i];
681  else
682  sum = 0.0 - y[i];
683 
684  /* H*x term */
685  if ( H != 0 )
686  for (j = 0; j < nV; j++) sum += H[i*nV+j] * x[j];
687 
688  /* A'*y term */
689  if ( A != 0 )
690  for (j = 0; j < nC; j++) sum -= A[j*nV+i] * y[nV+j];
691 
692  /* update stat */
693  if (qpOASES_getAbs(sum) > *_stat)
694  *_stat = qpOASES_getAbs(sum);
695  }
696 
697  /* check primal feasibility and complementarity */
698  /* variable bounds */
699  for (i = 0; i < nV; i++)
700  {
701  /* feasibility */
702  if ( lb != 0 )
703  if (lb[i] - x[i] > *feas)
704  *feas = lb[i] - x[i];
705 
706  if ( ub != 0 )
707  if (x[i] - ub[i] > *feas)
708  *feas = x[i] - ub[i];
709 
710  /* complementarity */
711  prod = 0.0;
712 
713  if ( lb != 0 )
714  if (y[i] > dualActiveTolerance) /* lower bound */
715  prod = (x[i] - lb[i]) * y[i];
716 
717  if ( ub != 0 )
718  if (y[i] < -dualActiveTolerance) /* upper bound */
719  prod = (x[i] - ub[i]) * y[i];
720 
721  if (qpOASES_getAbs(prod) > *cmpl)
722  *cmpl = qpOASES_getAbs(prod);
723  }
724  /* A*x bounds */
725  for (i = 0; i < nC; i++)
726  {
727  /* compute sum = (A*x)_i */
728  sum = 0.0;
729 
730  if ( A != 0 )
731  for (j = 0; j < nV; j++)
732  sum += A[i*nV+j] * x[j];
733 
734  /* feasibility */
735  if ( lbA != 0 )
736  if (lbA[i] - sum > *feas)
737  *feas = lbA[i] - sum;
738  if ( ubA != 0 )
739  if (sum - ubA[i] > *feas)
740  *feas = sum - ubA[i];
741 
742  /* complementarity */
743  prod = 0.0;
744 
745  if ( lbA != 0 )
746  if (y[nV+i] > dualActiveTolerance) /* lower bound */
747  prod = (sum - lbA[i]) * y[nV+i];
748 
749  if ( ubA != 0 )
750  if (y[nV+i] < -dualActiveTolerance) /* upper bound */
751  prod = (sum - ubA[i]) * y[nV+i];
752 
753  if (qpOASES_getAbs(prod) > *cmpl)
754  *cmpl = qpOASES_getAbs(prod);
755  }
756 
757  return SUCCESSFUL_RETURN;
758 }
759 
760 
761 /*
762  * g e t K k t V i o l a t i o n S B
763  */
765  const real_t* const H, const real_t* const g,
766  const real_t* const lb, const real_t* const ub,
767  const real_t* const x, const real_t* const y,
768  real_t* const _stat, real_t* const feas, real_t* const cmpl
769  )
770 {
771  return qpOASES_getKktViolation( nV,0,
772  H,g,0,lb,ub,0,0,
773  x,y,
774  _stat,feas,cmpl
775  );
776 }
777 
778 
779 /*
780  * c o n v e r t B o o l e a n T y p e T o S t r i n g
781  */
783 {
784  #ifndef __SUPPRESSANYOUTPUT__
785  if ( value == BT_FALSE )
786  snprintf( string,20,"BT_FALSE" );
787  else
788  snprintf( string,20,"BT_TRUE" );
789  #endif /* __SUPPRESSANYOUTPUT__ */
790 
791  return SUCCESSFUL_RETURN;
792 }
793 
794 
795 /*
796  * c o n v e r t S u b j e c t T o S t a t u s T o S t r i n g
797  */
799 {
800  #ifndef __SUPPRESSANYOUTPUT__
801  switch( value )
802  {
803  case ST_INACTIVE:
804  snprintf( string,20,"ST_INACTIVE" );
805  break;
806 
807  case ST_LOWER:
808  snprintf( string,20,"ST_LOWER" );
809  break;
810 
811  case ST_UPPER:
812  snprintf( string,20,"ST_UPPER" );
813  break;
814 
815  case ST_UNDEFINED:
816  snprintf( string,20,"ST_UNDEFINED" );
817  break;
818 
819  case ST_INFEASIBLE_LOWER:
820  snprintf( string,20,"ST_INFEASIBLE_LOWER" );
821  break;
822 
823  case ST_INFEASIBLE_UPPER:
824  snprintf( string,20,"ST_INFEASIBLE_UPPER" );
825  break;
826 
827  default:
828  snprintf( string,20,"<invalid value>" );
829  break;
830  }
831  #endif /* __SUPPRESSANYOUTPUT__ */
832 
833  return SUCCESSFUL_RETURN;
834 }
835 
836 
837 /*
838  * c o n v e r t P r i n t L e v e l T o S t r i n g
839  */
841 {
842  #ifndef __SUPPRESSANYOUTPUT__
843  switch( value )
844  {
845  case PL_NONE:
846  snprintf( string,20,"PL_NONE" );
847  break;
848 
849  case PL_LOW:
850  snprintf( string,20,"PL_LOW" );
851  break;
852 
853  case PL_MEDIUM:
854  snprintf( string,20,"PL_MEDIUM" );
855  break;
856 
857  case PL_HIGH:
858  snprintf( string,20,"PL_HIGH" );
859  break;
860 
861  case PL_TABULAR:
862  snprintf( string,20,"PL_TABULAR" );
863  break;
864 
865  case PL_DEBUG_ITER:
866  snprintf( string,20,"PL_DEBUG_ITER" );
867  break;
868 
869  default:
870  snprintf( string,20,"<invalid value>" );
871  break;
872  }
873  #endif /* __SUPPRESSANYOUTPUT__ */
874 
875  return SUCCESSFUL_RETURN;
876 }
877 
878 
879 /*
880  * g e t S i m p l e S t a t u s
881  */
883  BooleanType doPrintStatus
884  )
885 {
886  int retValNumber;
887  int simpleStatus = -1;
888 
889  /* determine simple status from returnvalue */
890  switch ( returnvalue )
891  {
892  case SUCCESSFUL_RETURN:
893  simpleStatus = 0;
894  break;
895 
897  simpleStatus = 1;
898  break;
899 
902  simpleStatus = -2;
903  break;
904 
907  simpleStatus = -3;
908  break;
909 
910  default:
911  simpleStatus = -1;
912  break;
913  }
914 
915  if ( doPrintStatus == BT_TRUE )
916  {
920 
921  retValNumber = (int)RET_SIMPLE_STATUS_P0 - simpleStatus;
922  THROWINFO( (returnValue)retValNumber );
923 
925  }
926 
927  return simpleStatus;
928 }
929 
930 
931 /*
932  * n o r m a l i s e C o n s t r a i n t s
933  */
935  real_t* A, real_t* lbA, real_t* ubA,
936  int type
937  )
938 {
939  int ii, jj;
940  real_t curNorm;
941 
942  if ( ( nV <= 0 ) || ( nC <= 0 ) || ( A == 0 ) )
944 
945  for( ii=0; ii<nC; ++ii )
946  {
947  /* get row norm */
948  curNorm = qpOASES_getNorm( &(A[ii*nV]),nV,type );
949 
950  if ( curNorm > QPOASES_EPS )
951  {
952  /* normalise if norm is positive */
953  for( jj=0; jj<nV; ++jj )
954  A[ii*nV + jj] /= curNorm;
955 
956  if ( lbA != 0 ) lbA[ii] /= curNorm;
957  if ( ubA != 0 ) ubA[ii] /= curNorm;
958  }
959  else
960  {
961  /* if row norm is (close to) zero, kind of erase constraint */
962  if ( type == 1 )
963  {
964  for( jj=0; jj<nV; ++jj )
965  A[ii*nV + jj] = 1.0 / ((real_t)nV);
966  }
967  else
968  {
969  /* assume type == 2 */
970  for( jj=0; jj<nV; ++jj )
971  A[ii*nV + jj] = 1.0 / qpOASES_getSqrt((real_t)nV);
972  }
973 
974  if ( lbA != 0 ) lbA[ii] = -QPOASES_INFTY;
975  if ( ubA != 0 ) ubA[ii] = QPOASES_INFTY;
976  }
977  }
978 
979  return SUCCESSFUL_RETURN;
980 }
981 
982 
983 #ifdef __DEBUG__
984 /*
985  * g d b _ p r i n t m at
986  */
987 void gdb_printmat( const char *fname, real_t *M, int n, int m, int ldim )
988 {
989  #ifdef __SUPPRESSANYOUTPUT__
990 
991  int i, j;
992  FILE *fid;
993 
994  fid = fopen(fname, "wt");
995  if (!fid)
996  {
997  perror("Error opening file: ");
998  return;
999  }
1000 
1001  for (i = 0; i < n; i++)
1002  {
1003  for (j = 0; j < m; j++)
1004  fprintf(fid, " %23.16e", M[j*ldim+i]);
1005  fprintf(fid, "\n");
1006  }
1007  fclose(fid);
1008 
1009  #endif /* __SUPPRESSANYOUTPUT__ */
1010 }
1011 #endif /* __DEBUG__ */
1012 
1013 
1014 #if defined(__DSPACE__) || defined(__XPCTARGET__)
1015 /*
1016  * _ _ c x a _ p u r e _ v i r t u a l
1017  */
1018 void __cxa_pure_virtual( void )
1019 {
1020  /* put your customized implementation here! */
1021 }
1022 #endif /* __DSPACE__ || __XPCTARGET__*/
1023 
1024 
1026 
1027 
1028 /*
1029  * end of file
1030  */
returnValue qpOASES_printNI(const int *const _index, int n, const char *name)
Definition: Utils.c:244
int counter
Definition: powerkite_c.cpp:40
#define ST_LOWER
returnValue qpOASES_printM(const real_t *const M, int nrow, int ncol)
Definition: Utils.c:147
returnValue qpOASES_convertPrintLevelToString(PrintLevel value, char *const string)
Definition: Utils.c:840
static returnValue MessageHandling_setErrorCount(MessageHandling *_THIS, int _errorCount)
#define __LINE__
returnValue qpOASES_printCopyrightNotice()
Definition: Utils.c:296
returnValue qpOASES_convertBooleanTypeToString(BooleanType value, char *const string)
Definition: Utils.c:782
returnValue qpOASES_writeIntoFileI(const int *const integer, int n, const char *datafilename, BooleanType append)
Definition: Utils.c:489
returnValue qpOASES_writeIntoFileM(const real_t *const data, int nrow, int ncol, const char *datafilename, BooleanType append)
Definition: Utils.c:423
returnValue qpOASES_printPV(const real_t *const v, int n, const int *const V_idx)
Definition: Utils.c:96
#define ST_INFEASIBLE_UPPER
Allows to pass back messages to the calling function.
returnValue qpOASES_writeIntoMatFile(FILE *const matFile, const double *const data, int nRows, int nCols, const char *name)
Definition: Utils.c:540
static FILE * MessageHandling_getOutputFile(MessageHandling *_THIS)
returnValue qpOASES_writeIntoMatFileI(FILE *const matFile, const int *const data, int nRows, int nCols, const char *name)
Definition: Utils.c:587
#define PL_MEDIUM
#define ST_INFEASIBLE_LOWER
#define PL_NONE
#define ST_UNDEFINED
#define ST_INACTIVE
returnValue qpOASES_printI(const int *const _index, int n)
Definition: Utils.c:217
returnValue qpOASES_printPM(const real_t *const M, int nrow, int ncol, const int *const ROW_idx, const int *const COL_idx)
Definition: Utils.c:170
#define PL_HIGH
static void MessageHandling_setInfoVisibilityStatus(MessageHandling *_THIS, VisibilityStatus _infoVisibility)
static real_t qpOASES_getAbs(real_t x)
Definition: Utils.h:468
static VisibilityStatus MessageHandling_getInfoVisibilityStatus(MessageHandling *_THIS)
returnValue qpOASES_readFromFileI(int *data, int n, const char *datafilename)
Definition: Utils.c:379
returnValue qpOASES_printNM(const real_t *const M, int nrow, int ncol, const char *name)
Definition: Utils.c:192
returnValue qpOASES_convertSubjectToStatusToString(SubjectToStatus value, char *const string)
Definition: Utils.c:798
returnValue normaliseConstraints(int nV, int nC, real_t *A, real_t *lbA, real_t *ubA, int type)
Definition: Utils.c:934
#define PL_DEBUG_ITER
real_t qpOASES_getCPUtime()
Definition: Utils.c:606
returnValue qpOASES_writeIntoFileV(const real_t *const data, int n, const char *datafilename, BooleanType append)
Definition: Utils.c:478
static real_t qpOASES_getSqrt(real_t x)
Definition: Utils.h:481
returnValue MessageHandling_throwError(MessageHandling *_THIS, returnValue Enumber, const char *additionaltext, const char *functionname, const char *filename, const unsigned long linenumber, VisibilityStatus localVisibilityStatus)
#define PL_LOW
static const real_t QPOASES_INFTY
returnValue qpOASES_readFromFileM(real_t *data, int nrow, int ncol, const char *datafilename)
Definition: Utils.c:314
#define v
PrintLevel
MessageHandling * qpOASES_getGlobalMessageHandler()
#define BT_TRUE
Definition: acado_types.hpp:47
returnValue qpOASES_getKktViolation(int nV, int nC, const real_t *const H, const real_t *const g, const real_t *const A, const real_t *const lb, const real_t *const ub, const real_t *const lbA, const real_t *const ubA, const real_t *const x, const real_t *const y, real_t *const _stat, real_t *const feas, real_t *const cmpl)
Definition: Utils.c:658
returnValue qpOASES_readFromFileV(real_t *data, int n, const char *datafilename)
Definition: Utils.c:367
static const real_t QPOASES_EPS
Struct containing the variable header for mat file.
#define __FILE__
#define QPOASES_MAX_STRING_LENGTH
#define BT_FALSE
Definition: acado_types.hpp:49
real_t qpOASES_getNorm(const real_t *const v, int n, int type)
Definition: Utils.c:629
returnValue qpOASES_printNV(const real_t *const v, int n, const char *name)
Definition: Utils.c:123
#define PL_TABULAR
#define ST_UPPER
double real_t
Definition: AD_test.c:10
returnValue qpOASES_getKktViolationSB(int nV, const real_t *const H, const real_t *const g, const real_t *const lb, const real_t *const ub, const real_t *const x, const real_t *const y, real_t *const _stat, real_t *const feas, real_t *const cmpl)
Definition: Utils.c:764
BEGIN_NAMESPACE_QPOASES returnValue qpOASES_printV(const real_t *const v, int n)
Definition: Utils.c:70
#define myStatic
Definition: Types.h:103
int qpOASES_getSimpleStatus(returnValue returnvalue, BooleanType doPrintStatus)
Definition: Utils.c:882
returnValue qpOASES_myPrintf(const char *s)
Definition: Utils.c:269


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