Utils.cpp
Go to the documentation of this file.
00001 /*
00002  *      This file is part of qpOASES.
00003  *
00004  *      qpOASES -- An Implementation of the Online Active Set Strategy.
00005  *      Copyright (C) 2007-2011 by Hans Joachim Ferreau, Andreas Potschka,
00006  *      Christian Kirches et al. All rights reserved.
00007  *
00008  *      qpOASES is free software; you can redistribute it and/or
00009  *      modify it under the terms of the GNU Lesser General Public
00010  *      License as published by the Free Software Foundation; either
00011  *      version 2.1 of the License, or (at your option) any later version.
00012  *
00013  *      qpOASES is distributed in the hope that it will be useful,
00014  *      but WITHOUT ANY WARRANTY; without even the implied warranty of
00015  *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
00016  *      See the GNU Lesser General Public License for more details.
00017  *
00018  *      You should have received a copy of the GNU Lesser General Public
00019  *      License along with qpOASES; if not, write to the Free Software
00020  *      Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
00021  *
00022  */
00023 
00024 
00035 #include <math.h>
00036 
00037 #if defined(__WIN32__) || defined(WIN32)
00038   #include <windows.h>
00039 #elif defined(LINUX)
00040   #include <sys/stat.h>
00041   #include <sys/time.h>
00042 #endif
00043 
00044 #ifdef __MATLAB__
00045   #include "mex.h"
00046 #endif
00047 
00048 
00049 #include <qpOASES/Utils.hpp>
00050 
00051 
00052 BEGIN_NAMESPACE_QPOASES
00053 
00054 
00055 /*
00056  *      p r i n t
00057  */
00058 returnValue print( const real_t* const v, int n )
00059 {
00060         #ifndef __SUPPRESSANYOUTPUT__
00061         #ifndef __XPCTARGET__
00062         int i;
00063         char myPrintfString[160];
00064 
00065         /* Print a vector. */
00066         myPrintf( "[\t" );
00067         for( i=0; i<n; ++i )
00068         {
00069                 snprintf( myPrintfString,160," %.16e\t", v[i] );
00070                 myPrintf( myPrintfString );
00071         }
00072         myPrintf( "]\n" );
00073         #endif
00074         #endif
00075 
00076         return SUCCESSFUL_RETURN;
00077 }
00078 
00079 
00080 /*
00081  *      p r i n t
00082  */
00083 returnValue print(      const real_t* const v, int n,
00084                                         const int* const V_idx
00085                                         )
00086 {
00087         #ifndef __SUPPRESSANYOUTPUT__
00088         #ifndef __XPCTARGET__
00089         int i;
00090         char myPrintfString[160];
00091 
00092         /* Print a permuted vector. */
00093         myPrintf( "[\t" );
00094         for( i=0; i<n; ++i )
00095         {
00096                 snprintf( myPrintfString,160," %.16e\t", v[ V_idx[i] ] );
00097                 myPrintf( myPrintfString );
00098         }
00099         myPrintf( "]\n" );
00100         #endif
00101         #endif
00102 
00103         return SUCCESSFUL_RETURN;
00104 }
00105 
00106 
00107 /*
00108  *      p r i n t
00109  */
00110 returnValue print(      const real_t* const v, int n,
00111                                         const char* name
00112                                         )
00113 {
00114         #ifndef __SUPPRESSANYOUTPUT__
00115         #ifndef __XPCTARGET__
00116         char myPrintfString[160];
00117 
00118         /* Print vector name ... */
00119         snprintf( myPrintfString,160,"%s = ", name );
00120         myPrintf( myPrintfString );
00121         #endif
00122         #endif
00123 
00124         /* ... and the vector itself. */
00125         return print( v, n );
00126 }
00127 
00128 /*
00129  *      p r i n t
00130  */
00131 returnValue print( const real_t* const M, int nrow, int ncol )
00132 {
00133         #ifndef __SUPPRESSANYOUTPUT__
00134         #ifndef __XPCTARGET__
00135         int i;
00136 
00137         /* Print a matrix as a collection of row vectors. */
00138         for( i=0; i<nrow; ++i )
00139                 print( &(M[i*ncol]), ncol );
00140         myPrintf( "\n" );
00141         #endif
00142         #endif
00143 
00144         return SUCCESSFUL_RETURN;
00145 }
00146 
00147 
00148 /*
00149  *      p r i n t
00150  */
00151 returnValue print(      const real_t* const M, int nrow, int ncol,
00152                                         const int* const ROW_idx, const int* const COL_idx
00153                                         )
00154 {
00155         #ifndef __SUPPRESSANYOUTPUT__
00156         #ifndef __XPCTARGET__
00157         int i;
00158 
00159         /* Print a permuted matrix as a collection of permuted row vectors. */
00160         for( i=0; i<nrow; ++i )
00161                 print( &( M[ ROW_idx[i]*ncol ] ), ncol, COL_idx );
00162         myPrintf( "\n" );
00163         #endif
00164         #endif
00165 
00166         return SUCCESSFUL_RETURN;
00167 }
00168 
00169 
00170 /*
00171  *      p r i n t
00172  */
00173 returnValue print(      const real_t* const M, int nrow, int ncol,
00174                                         const char* name
00175                                         )
00176 {
00177         #ifndef __SUPPRESSANYOUTPUT__
00178         #ifndef __XPCTARGET__
00179         char myPrintfString[160];
00180 
00181         /* Print matrix name ... */
00182         snprintf( myPrintfString,160,"%s = ", name );
00183         myPrintf( myPrintfString );
00184         #endif
00185         #endif
00186 
00187         /* ... and the matrix itself. */
00188         return print( M, nrow, ncol );
00189 }
00190 
00191 
00192 /*
00193  *      p r i n t
00194  */
00195 returnValue print( const int* const index, int n )
00196 {
00197         #ifndef __SUPPRESSANYOUTPUT__
00198         #ifndef __XPCTARGET__
00199         int i;
00200         char myPrintfString[160];
00201 
00202         /* Print a indexlist. */
00203         myPrintf( "[\t" );
00204         for( i=0; i<n; ++i )
00205         {
00206                 snprintf( myPrintfString,160," %d\t", index[i] );
00207                 myPrintf( myPrintfString );
00208         }
00209         myPrintf( "]\n" );
00210         #endif
00211         #endif
00212 
00213         return SUCCESSFUL_RETURN;
00214 }
00215 
00216 
00217 /*
00218  *      p r i n t
00219  */
00220 returnValue print(      const int* const index, int n,
00221                                         const char* name
00222                                         )
00223 {
00224         #ifndef __SUPPRESSANYOUTPUT__
00225         #ifndef __XPCTARGET__
00226         char myPrintfString[160];
00227 
00228         /* Print indexlist name ... */
00229         snprintf( myPrintfString,160,"%s = ", name );
00230         myPrintf( myPrintfString );
00231         #endif
00232         #endif
00233 
00234         /* ... and the indexlist itself. */
00235         return print( index, n );
00236 }
00237 
00238 
00239 /*
00240  *      m y P r i n t f
00241  */
00242 returnValue myPrintf( const char* s )
00243 {
00244         #ifndef __SUPPRESSANYOUTPUT__
00245         #ifndef __XPCTARGET__
00246           #ifdef __MATLAB__
00247           mexPrintf( s );
00248           #else
00249           FILE* outputfile = getGlobalMessageHandler( )->getOutputFile( );
00250           if ( outputfile == 0 )
00251                   return THROWERROR( RET_NO_GLOBAL_MESSAGE_OUTPUTFILE );
00252 
00253           fprintf( outputfile, "%s", s );
00254           #endif
00255         #endif
00256         #endif
00257 
00258         return SUCCESSFUL_RETURN;
00259 }
00260 
00261 
00262 /*
00263  *      p r i n t C o p y r i g h t N o t i c e
00264  */
00265 returnValue printCopyrightNotice( )
00266 {
00267         #ifndef __SUPPRESSANYOUTPUT__
00268                 #ifndef __XPCTARGET__
00269                 #ifndef __DSPACE__
00270                 #ifndef __NO_COPYRIGHT__
00271                 myPrintf( "\nqpOASES -- An Implementation of the Online Active Set Strategy.\nCopyright (C) 2007-2011 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" );
00272                 #endif
00273                 #endif
00274                 #endif
00275         #endif
00276         return SUCCESSFUL_RETURN;
00277 }
00278 
00279 
00280 /*
00281  *      r e a d F r o m F i l e
00282  */
00283 returnValue readFromFile(       real_t* data, int nrow, int ncol,
00284                                                         const char* datafilename
00285                                                         )
00286 {
00287         #ifndef __XPCTARGET__
00288         int i, j;
00289         real_t float_data;
00290         FILE* datafile;
00291 
00292         /* 1) Open file. */
00293         if ( ( datafile = fopen( datafilename, "r" ) ) == 0 )
00294         {
00295                 char errstr[80];
00296                 snprintf( errstr,80,"(%s)",datafilename );
00297                 return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
00298         }
00299 
00300         /* 2) Read data from file. */
00301         for( i=0; i<nrow; ++i )
00302         {
00303                 for( j=0; j<ncol; ++j )
00304                 {
00305                         #ifdef __USE_SINGLE_PRECISION__
00306                         if ( fscanf( datafile, "%f ", &float_data ) == 0 )
00307                         #else
00308                         if ( fscanf( datafile, "%lf ", &float_data ) == 0 )
00309                         #endif /* __USE_SINGLE_PRECISION__ */
00310                         {
00311                                 fclose( datafile );
00312                                 char errstr[80];
00313                                 snprintf( errstr,80,"(%s)",datafilename );
00314                                 return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_READ_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
00315                         }
00316                         data[i*ncol + j] = ( (real_t) float_data );
00317                 }
00318         }
00319 
00320         /* 3) Close file. */
00321         fclose( datafile );
00322 
00323         return SUCCESSFUL_RETURN;
00324         #else
00325 
00326         return RET_NOT_YET_IMPLEMENTED;
00327 
00328         #endif
00329 }
00330 
00331 
00332 /*
00333  *      r e a d F r o m F i l e
00334  */
00335 returnValue readFromFile(       real_t* data, int n,
00336                                                         const char* datafilename
00337                                                         )
00338 {
00339         return readFromFile( data, n, 1, datafilename );
00340 }
00341 
00342 
00343 
00344 /*
00345  *      r e a d F r o m F i l e
00346  */
00347 returnValue readFromFile(       int* data, int n,
00348                                                         const char* datafilename
00349                                                         )
00350 {
00351         #ifndef __XPCTARGET__
00352         int i;
00353         FILE* datafile;
00354 
00355         /* 1) Open file. */
00356         if ( ( datafile = fopen( datafilename, "r" ) ) == 0 )
00357         {
00358                 char errstr[80];
00359                 snprintf( errstr,80,"(%s)",datafilename );
00360                 return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
00361         }
00362 
00363         /* 2) Read data from file. */
00364         for( i=0; i<n; ++i )
00365         {
00366                 if ( fscanf( datafile, "%d\n", &(data[i]) ) == 0 )
00367                 {
00368                         fclose( datafile );
00369                         char errstr[80];
00370                         snprintf( errstr,80,"(%s)",datafilename );
00371                         return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_READ_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
00372                 }
00373         }
00374 
00375         /* 3) Close file. */
00376         fclose( datafile );
00377 
00378         return SUCCESSFUL_RETURN;
00379         #else
00380 
00381         return RET_NOT_YET_IMPLEMENTED;
00382 
00383         #endif
00384 }
00385 
00386 
00387 /*
00388  *      w r i t e I n t o F i l e
00389  */
00390 returnValue writeIntoFile(      const real_t* const data, int nrow, int ncol,
00391                                                         const char* datafilename, BooleanType append
00392                                                         )
00393 {
00394         #ifndef __XPCTARGET__
00395         int i, j;
00396         FILE* datafile;
00397 
00398         /* 1) Open file. */
00399         if ( append == BT_TRUE )
00400         {
00401                 /* append data */
00402                 if ( ( datafile = fopen( datafilename, "a" ) ) == 0 )
00403                 {
00404                         char errstr[80];
00405                         snprintf( errstr,80,"(%s)",datafilename );
00406                         return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
00407                 }
00408         }
00409         else
00410         {
00411                 /* do not append data */
00412                 if ( ( datafile = fopen( datafilename, "w" ) ) == 0 )
00413                 {
00414                         char errstr[80];
00415                         snprintf( errstr,80,"(%s)",datafilename );
00416                         return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
00417                 }
00418         }
00419 
00420         /* 2) Write data into file. */
00421         for( i=0; i<nrow; ++i )
00422         {
00423                 for( j=0; j<ncol; ++j )
00424                         fprintf( datafile, "%.16e ", data[i*ncol+j] );
00425 
00426                 fprintf( datafile, "\n" );
00427         }
00428 
00429         /* 3) Close file. */
00430         fclose( datafile );
00431 
00432         return SUCCESSFUL_RETURN;
00433         #else
00434 
00435         return RET_NOT_YET_IMPLEMENTED;
00436 
00437         #endif
00438 }
00439 
00440 
00441 /*
00442  *      w r i t e I n t o F i l e
00443  */
00444 returnValue writeIntoFile(      const real_t* const data, int n,
00445                                                         const char* datafilename, BooleanType append
00446                                                         )
00447 {
00448         return writeIntoFile( data,1,n,datafilename,append );
00449 }
00450 
00451 
00452 /*
00453  *      w r i t e I n t o F i l e
00454  */
00455 returnValue writeIntoFile(      const int* const integer, int n,
00456                                                         const char* datafilename, BooleanType append
00457                                                         )
00458 {
00459         #ifndef __XPCTARGET__
00460         int i;
00461 
00462         FILE* datafile;
00463 
00464         /* 1) Open file. */
00465         if ( append == BT_TRUE )
00466         {
00467                 /* append data */
00468                 if ( ( datafile = fopen( datafilename, "a" ) ) == 0 )
00469                 {
00470                         char errstr[80];
00471                         snprintf( errstr,80,"(%s)",datafilename );
00472                         return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
00473                 }
00474         }
00475         else
00476         {
00477                 /* do not append data */
00478                 if ( ( datafile = fopen( datafilename, "w" ) ) == 0 )
00479                 {
00480                         char errstr[80];
00481                         snprintf( errstr,80,"(%s)",datafilename );
00482                         return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
00483                 }
00484         }
00485 
00486         /* 2) Write data into file. */
00487         for( i=0; i<n; ++i )
00488                 fprintf( datafile, "%d\n", integer[i] );
00489 
00490         /* 3) Close file. */
00491         fclose( datafile );
00492 
00493         return SUCCESSFUL_RETURN;
00494         #else
00495 
00496         return RET_NOT_YET_IMPLEMENTED;
00497 
00498         #endif
00499 }
00500 
00501 
00502 /*
00503  *      g e t C P U t i m e
00504  */
00505 real_t getCPUtime( )
00506 {
00507         real_t current_time = -1.0;
00508 
00509         #if defined(__WIN32__) || defined(WIN32)
00510         LARGE_INTEGER counter, frequency;
00511         QueryPerformanceFrequency(&frequency);
00512         QueryPerformanceCounter(&counter);
00513         current_time = ((real_t) counter.QuadPart) / ((real_t) frequency.QuadPart);
00514         #elif defined(LINUX)
00515         struct timeval theclock;
00516         gettimeofday( &theclock,0 );
00517         current_time = 1.0*theclock.tv_sec + 1.0e-6*theclock.tv_usec;
00518         #endif
00519 
00520         return current_time;
00521 }
00522 
00523 
00524 /*
00525  *      g e t N o r m
00526  */
00527 real_t getNorm( const real_t* const v, int n )
00528 {
00529         int i;
00530 
00531         real_t norm = 0.0;
00532 
00533         for( i=0; i<n; ++i )
00534                 norm += v[i]*v[i];
00535 
00536         return sqrt( norm );
00537 }
00538 
00539 
00540 
00541 /*
00542  *      g e t K K T R e s i d u a l
00543  */
00544 void getKKTResidual(    int nV, int nC,
00545                                                 const real_t* const H, const real_t* const g,
00546                                                 const real_t* const A, const real_t* const lb, const real_t* const ub,
00547                                                 const real_t* const lbA, const real_t* const ubA,
00548                                                 const real_t* const x, const real_t* const y,
00549                                                 real_t& stat, real_t& feas, real_t& cmpl)
00550 {
00551         /* Tolerance for dual variables considered zero. */
00552         const real_t dualActiveTolerance = 1.0e3 * EPS;
00553 
00554         int i, j;
00555         real_t sum, prod;
00556 
00557         /* Initialize residuals */
00558         stat = feas = cmpl = 0.0;
00559 
00560         /* Pointers to data and solution of current QP */
00561         const real_t* gCur   = g;
00562         const real_t* lbCur  = lb;
00563         const real_t* ubCur  = ub;
00564         const real_t* lbACur = lbA ? lbA : 0;
00565         const real_t* ubACur = ubA ? ubA : 0;
00566         const real_t* xCur   = x;
00567         const real_t* yCur   = y;
00568 
00569         /* check stationarity */
00570         for (i = 0; i < nV; i++)
00571         {
00572                 /* g term and variable bounds dual term */
00573                 sum = gCur[i] - yCur[i];
00574 
00575                 /* H*x term */
00576                 for (j = 0; j < nV; j++) sum += H[i*nV+j] * xCur[j];
00577                 /* A'*y term */
00578                 for (j = 0; j < nC; j++) sum -= A[j*nV+i] * yCur[j+nV];
00579 
00580                 /* update stat */
00581                 if (fabs(sum) > stat) stat = fabs(sum);
00582         }
00583 
00584         /* check primal feasibility and complementarity */
00585         /* variable bounds */
00586         for (i = 0; i < nV; i++)
00587         {
00588                 /* feasibility */
00589                 if (lbCur[i] - xCur[i] > feas) 
00590                         feas = lbCur[i] - xCur[i];
00591                 if (xCur[i] - ubCur[i] > feas) 
00592                         feas = xCur[i] - ubCur[i];
00593 
00594                 /* complementarity */
00595                 prod = 0.0;
00596                 if (yCur[i] > dualActiveTolerance) /* lower bound */
00597                         prod = (xCur[i] - lbCur[i]) * yCur[i];
00598                 if (yCur[i] < -dualActiveTolerance) /* upper bound */
00599                         prod = (xCur[i] - ubCur[i]) * yCur[i];
00600                 if (fabs(prod) > cmpl) cmpl = fabs(prod);
00601         }
00602         /* A*x bounds */
00603         for (i = 0; i < nC; i++)
00604         {
00605                 /* compute sum = (A*x)_i */
00606                 sum = 0.0;
00607                 for (j = 0; j < nV; j++) 
00608                         sum += A[i*nV+j] * xCur[j];
00609 
00610                 /* feasibility */
00611                 if (lbACur[i] - sum > feas) 
00612                         feas = lbACur[i] - sum;
00613                 if (sum - ubACur[i] > feas) 
00614                         feas = sum - ubACur[i];
00615 
00616                 /* complementarity */
00617                 prod = 0.0;
00618                 if (yCur[nV+i] > dualActiveTolerance) /* lower bound */
00619                         prod = (sum - lbACur[i]) * yCur[nV+i];
00620                 if (yCur[nV+i] < -dualActiveTolerance) /* upper bound */
00621                         prod = (sum - ubACur[i]) * yCur[nV+i];
00622                 if (fabs(prod) > cmpl) cmpl = fabs(prod);
00623         }
00624 }
00625 
00626 
00627 returnValue convertBooleanTypeToString( BooleanType value, char* const string )
00628 {
00629         if ( value == BT_TRUE )
00630                 snprintf( string,20,"BT_TRUE" );
00631         else
00632                 snprintf( string,20,"BT_FALSE" );
00633 
00634         return SUCCESSFUL_RETURN;
00635 }
00636 
00637 
00638 returnValue convertSubjectToStatusToString( SubjectToStatus value, char* const string )
00639 {
00640         switch( value )
00641         {
00642                 case ST_INACTIVE:
00643                         snprintf( string,20,"ST_INACTIVE" );
00644                         break;
00645 
00646                 case ST_LOWER:
00647                         snprintf( string,20,"ST_LOWER" );
00648                         break;
00649 
00650                 case ST_UPPER:
00651                         snprintf( string,20,"ST_UPPER" );
00652                         break;
00653 
00654                 case ST_UNDEFINED:
00655                         snprintf( string,20,"ST_UNDEFINED" );
00656                         break;
00657         }
00658 
00659         return SUCCESSFUL_RETURN;
00660 }
00661 
00662 
00663 returnValue convertPrintLevelToString( PrintLevel value, char* const string )
00664 {
00665         switch( value )
00666         {
00667                 case PL_NONE:
00668                         snprintf( string,20,"PL_NONE" );
00669                         break;
00670 
00671                 case PL_LOW:
00672                         snprintf( string,20,"PL_LOW" );
00673                         break;
00674 
00675                 case PL_MEDIUM:
00676                         snprintf( string,20,"PL_MEDIUM" );
00677                         break;
00678 
00679                 case PL_HIGH:
00680                         snprintf( string,20,"PL_HIGH" );
00681                         break;
00682                         
00683                 case PL_TABULAR:
00684                         snprintf( string,20,"PL_TABULAR" );
00685                         break;
00686         }
00687 
00688         return SUCCESSFUL_RETURN;
00689 }
00690 
00691 
00692 
00693 #ifdef __DEBUG__
00694 extern "C" void gdb_printmat(const char *fname, real_t *M, int n, int m, int ldim)
00695 {
00696         int i, j;
00697         FILE *fid;
00698 
00699         fid = fopen(fname, "wt");
00700         if (!fid) 
00701         {
00702                 perror("Error opening file: ");
00703                 return;
00704         }
00705 
00706         for (i = 0; i < n; i++)
00707         {
00708                 for (j = 0; j < m; j++)
00709                         fprintf(fid, " %23.16e", M[j*ldim+i]);
00710                 fprintf(fid, "\n");
00711         }
00712         fclose(fid);
00713 }
00714 #endif /* __DEBUG__ */
00715 
00716 
00717 
00718 END_NAMESPACE_QPOASES
00719 
00720 
00721 /*
00722  *      end of file
00723  */


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