SolutionAnalysis.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 
00037 #include <qpOASES/extras/SolutionAnalysis.hpp>
00038 
00039 
00040 BEGIN_NAMESPACE_QPOASES
00041 
00042 
00043 /*****************************************************************************
00044  *  P U B L I C                                                              *
00045  *****************************************************************************/
00046 
00047 
00048 /*
00049  *      S o l u t i o n A n a l y s i s
00050  */
00051 SolutionAnalysis::SolutionAnalysis( )
00052 {
00053 
00054 }
00055 
00056 
00057 /*
00058  *      S o l u t i o n A n a l y s i s
00059  */
00060 SolutionAnalysis::SolutionAnalysis( const SolutionAnalysis& rhs )
00061 {
00062 
00063 }
00064 
00065 
00066 /*
00067  *      ~ S o l u t i o n A n a l y s i s
00068  */
00069 SolutionAnalysis::~SolutionAnalysis( )
00070 {
00071 
00072 }
00073 
00074 
00075 /*
00076  *      o p e r a t o r =
00077  */
00078 SolutionAnalysis& SolutionAnalysis::operator=( const SolutionAnalysis& rhs )
00079 {
00080         if ( this != &rhs )
00081         {
00082 
00083         }
00084 
00085         return *this;
00086 }
00087 
00088 
00089 
00090 /*
00091  *      g e t M a x K K T v i o l a t i o n
00092  */
00093 returnValue SolutionAnalysis::getMaxKKTviolation( QProblemB* qp, real_t& maxKKTviolation ) const
00094 {
00095         int i;
00096         int nV = qp->getNV( );
00097 
00098         real_t *tmp = new real_t[nV];
00099         maxKKTviolation = 0.0;
00100 
00101 
00102         /* 1) Check for Hx + g - y*A' = 0  (here: A = Id). */
00103         for( i=0; i<nV; ++i )
00104                 tmp[i] = qp->g[i];
00105 
00106         switch ( qp->getHessianType( ) )
00107         {
00108                 case HST_ZERO:
00109                         /*tmp += qp->eps * qp->x[i]; */
00110                         break;
00111 
00112                 case HST_IDENTITY:
00113                         for( i=0; i<nV; ++i )
00114                                 tmp[i] += qp->x[i];
00115                         break;
00116 
00117                 default:
00118                         qp->H->times(1, 1.0, qp->x, nV, 1.0, tmp, nV);
00119                         break;
00120         }
00121 
00122         for( i=0; i<nV; ++i )
00123         {
00124                 tmp[i] -= qp->y[i];
00125 
00126                 if ( fabs( tmp[i] ) > maxKKTviolation )
00127                         maxKKTviolation = fabs( tmp[i] );
00128         }
00129         delete[] tmp;
00130 
00131         /* 2) Check for lb <= x <= ub. */
00132         for( i=0; i<nV; ++i )
00133         {
00134                 if ( qp->lb[i] - qp->x[i] > maxKKTviolation )
00135                         maxKKTviolation = qp->lb[i] - qp->x[i];
00136 
00137                 if ( qp->x[i] - qp->ub[i] > maxKKTviolation )
00138                         maxKKTviolation = qp->x[i] - qp->ub[i];
00139         }
00140 
00141         /* 3) Check for correct sign of y and for complementary slackness. */
00142         for( i=0; i<nV; ++i )
00143         {
00144                 switch ( qp->bounds.getStatus( i ) )
00145                 {
00146                         case ST_LOWER:
00147                                 if ( -qp->y[i] > maxKKTviolation )
00148                                         maxKKTviolation = -qp->y[i];
00149                                 if ( fabs( ( qp->x[i] - qp->lb[i] ) * qp->y[i] ) > maxKKTviolation )
00150                                         maxKKTviolation = fabs( ( qp->x[i] - qp->lb[i] ) * qp->y[i] );
00151                                 break;
00152 
00153                         case ST_UPPER:
00154                                 if ( qp->y[i] > maxKKTviolation )
00155                                         maxKKTviolation = qp->y[i];
00156                                 if ( fabs( ( qp->ub[i] - qp->x[i] ) * qp->y[i] ) > maxKKTviolation )
00157                                         maxKKTviolation = fabs( ( qp->ub[i] - qp->x[i] ) * qp->y[i] );
00158                                 break;
00159 
00160                         default: /* inactive */
00161                         if ( fabs( qp->y[i] ) > maxKKTviolation )
00162                                         maxKKTviolation = fabs( qp->y[i] );
00163                                 break;
00164                 }
00165         }
00166 
00167 
00168         return SUCCESSFUL_RETURN;
00169 }
00170 
00171 
00172 /*
00173  *      g e t M a x K K T v i o l a t i o n
00174  */
00175 returnValue SolutionAnalysis::getMaxKKTviolation( QProblem* qp, real_t& maxKKTviolation ) const
00176 {
00177         int i;
00178         int nV = qp->getNV( );
00179         int nC = qp->getNC( );
00180 
00181         real_t *tmp = new real_t[nV];
00182         maxKKTviolation = 0.0;
00183 
00184 
00185         /* 1) check for Hx + g - [yFX yAC]*[Id A]' = 0. */
00186         for( i=0; i<nV; ++i )
00187                 tmp[i] = qp->g[i];
00188 
00189         switch ( qp->getHessianType( ) )
00190         {
00191                 case HST_ZERO:
00192                                 /*tmp += qp->eps * qp->x[i]; */
00193                         break;
00194 
00195                 case HST_IDENTITY:
00196                         for( i=0; i<nV; ++i )
00197                                 tmp[i] += qp->x[i];
00198                         break;
00199 
00200                 default:
00201                         qp->H->times(1, 1.0, qp->x, nV, 1.0, tmp, nV);
00202                         break;
00203         }
00204 
00205         qp->A->transTimes(1, -1.0, qp->y + nV, nC, 1.0, tmp, nV);
00206 
00207         for( i=0; i<nV; ++i )
00208         {
00209                 tmp[i] -= qp->y[i];
00210 
00211                 if ( fabs( tmp[i] ) > maxKKTviolation )
00212                         maxKKTviolation = fabs( tmp[i] );
00213         }
00214 
00215         /* 2) Check for [lb lbA] <= [Id A]*x <= [ub ubA]. */
00216         /* lbA <= Ax <= ubA */
00217         real_t* Ax = new real_t[nC];
00218         qp->A->times(1, 1.0, qp->x, nV, 0.0, Ax, nC);
00219 
00220         for( i=0; i<nC; ++i )
00221         {
00222                 if ( qp->lbA[i] - Ax[i] > maxKKTviolation )
00223                         maxKKTviolation = qp->lbA[i] - Ax[i];
00224 
00225                 if ( Ax[i] - qp->ubA[i] > maxKKTviolation )
00226                         maxKKTviolation = Ax[i] - qp->ubA[i];
00227         }
00228 
00229         /* lb <= x <= ub */
00230         for( i=0; i<nV; ++i )
00231         {
00232                 if ( qp->lb[i] - qp->x[i] > maxKKTviolation )
00233                         maxKKTviolation = qp->lb[i] - qp->x[i];
00234 
00235                 if ( qp->x[i] - qp->ub[i] > maxKKTviolation )
00236                         maxKKTviolation = qp->x[i] - qp->ub[i];
00237         }
00238 
00239         /* 3) Check for correct sign of y and for complementary slackness. */
00240         /* bounds */
00241         for( i=0; i<nV; ++i )
00242         {
00243                 switch ( qp->bounds.getStatus( i ) )
00244                 {
00245                         case ST_LOWER:
00246                                 if ( -qp->y[i] > maxKKTviolation )
00247                                         maxKKTviolation = -qp->y[i];
00248                                 if ( fabs( ( qp->x[i] - qp->lb[i] ) * qp->y[i] ) > maxKKTviolation )
00249                                         maxKKTviolation = fabs( ( qp->x[i] - qp->lb[i] ) * qp->y[i] );
00250                                 break;
00251 
00252                         case ST_UPPER:
00253                                 if ( qp->y[i] > maxKKTviolation )
00254                                         maxKKTviolation = qp->y[i];
00255                                 if ( fabs( ( qp->ub[i] - qp->x[i] ) * qp->y[i] ) > maxKKTviolation )
00256                                         maxKKTviolation = fabs( ( qp->ub[i] - qp->x[i] ) * qp->y[i] );
00257                                 break;
00258 
00259                         default: /* inactive */
00260                         if ( fabs( qp->y[i] ) > maxKKTviolation )
00261                                         maxKKTviolation = fabs( qp->y[i] );
00262                                 break;
00263                 }
00264         }
00265 
00266         /* constraints */
00267         for( i=0; i<nC; ++i )
00268         {
00269                 switch ( qp->constraints.getStatus( i ) )
00270                 {
00271                         case ST_LOWER:
00272                                 if ( -qp->y[nV+i] > maxKKTviolation )
00273                                         maxKKTviolation = -qp->y[nV+i];
00274                                 if ( fabs( ( Ax[i] - qp->lbA[i] ) * qp->y[nV+i] ) > maxKKTviolation )
00275                                         maxKKTviolation = fabs( ( Ax[i] - qp->lbA[i] ) * qp->y[nV+i] );
00276                                 break;
00277 
00278                         case ST_UPPER:
00279                                 if ( qp->y[nV+i] > maxKKTviolation )
00280                                         maxKKTviolation = qp->y[nV+i];
00281                                 if ( fabs( ( qp->ubA[i] - Ax[i] ) * qp->y[nV+i] ) > maxKKTviolation )
00282                                         maxKKTviolation = fabs( ( qp->ubA[i] - Ax[i] ) * qp->y[nV+i] );
00283                                 break;
00284 
00285                         default: /* inactive */
00286                         if ( fabs( qp->y[nV+i] ) > maxKKTviolation )
00287                                         maxKKTviolation = fabs( qp->y[nV+i] );
00288                                 break;
00289                 }
00290         }
00291 
00292         delete[] tmp;
00293         delete[] Ax;
00294 
00295         return SUCCESSFUL_RETURN;
00296 }
00297 
00298 
00299 /*
00300  *      g e t M a x K K T v i o l a t i o n
00301  */
00302 returnValue SolutionAnalysis::getMaxKKTviolation( SQProblem* qp, real_t& maxKKTviolation ) const
00303 {
00304         return getMaxKKTviolation( (QProblem*)qp,maxKKTviolation );
00305 }
00306 
00307 
00308 
00309 /*
00310  *      g e t V a r i a n c e C o v a r i a n c e
00311  */
00312 returnValue SolutionAnalysis::getVarianceCovariance( QProblemB* qp, real_t* g_b_bA_VAR, real_t* Primal_Dual_VAR ) const
00313 {
00314         return THROWERROR( RET_NOT_YET_IMPLEMENTED );
00315 }
00316 
00317 
00318 /*
00319  *      g e t V a r i a n c e C o v a r i a n c e
00320  */
00321 returnValue SolutionAnalysis::getVarianceCovariance( QProblem* qp, real_t* g_b_bA_VAR, real_t* Primal_Dual_VAR ) const
00322 {
00323 
00324   /* DEFINITION OF THE DIMENSIONS nV AND nC:
00325    * --------------------------------------- */
00326   int nV  = qp->getNV( );                      /* dimension of x / the bounds */
00327   int nC  = qp->getNC( );                      /* dimension of the constraints */
00328   int dim = 2*nV+nC;                           /* dimension of input and output */
00329                                                /* variance-covariance matrix */
00330   int run1, run2, run3;                        /* simple run variables (for loops). */
00331 
00332 
00333   /* ALLOCATION OF MEMORY:
00334    * --------------------- */
00335   real_t* delta_g_cov    = new real_t[nV];     /* a covariance-vector of g */
00336   real_t* delta_lb_cov   = new real_t[nV];     /* a covariance-vector of lb */
00337   real_t* delta_ub_cov   = new real_t[nV];     /* a covariance-vector of ub */
00338   real_t* delta_lbA_cov  = new real_t[nC];     /* a covariance-vector of lbA */
00339   real_t* delta_ubA_cov  = new real_t[nC];     /* a covariance-vector of ubA */
00340 
00341   returnValue returnvalue;                     /* the return value */
00342   BooleanType Delta_bC_isZero = BT_FALSE;      /* (just use FALSE here) */
00343   BooleanType Delta_bB_isZero = BT_FALSE;      /* (just use FALSE here) */
00344 
00345 
00346 
00347   /* ASK FOR THE NUMBER OF FREE AND FIXED VARIABLES:
00348    * (ASSUMES THAT ACTIVE SET IS CONSTANT FOR THE
00349    *  VARIANCE-COVARIANCE EVALUATION)
00350    * ----------------------------------------------- */
00351   int nFR, nFX, nAC;
00352 
00353   nFR = qp->getNFR( );
00354   nFX = qp->getNFX( );
00355   nAC = qp->getNAC( );
00356 
00357 
00358   /* ASK FOR THE CORRESPONDING INDEX ARRAYS:
00359    * --------------------------------------- */
00360   int *FR_idx, *FX_idx, *AC_idx;
00361 
00362   if ( qp->bounds.getFree( )->getNumberArray( &FR_idx ) != SUCCESSFUL_RETURN )
00363        return THROWERROR( RET_HOTSTART_FAILED );
00364 
00365   if ( qp->bounds.getFixed( )->getNumberArray( &FX_idx ) != SUCCESSFUL_RETURN )
00366        return THROWERROR( RET_HOTSTART_FAILED );
00367 
00368   if ( qp->constraints.getActive( )->getNumberArray( &AC_idx ) != SUCCESSFUL_RETURN )
00369        return THROWERROR( RET_HOTSTART_FAILED );
00370 
00371 
00372 
00373   /* INTRODUCE VARIABLES TO MEASURE THE REACTION OF THE QP-SOLUTION TO
00374    * THE VARIANCE-COVARIANCE DISTURBANCE:
00375    * ----------------------------------------------------------------- */
00376   real_t *delta_xFR = new real_t[nFR];
00377   real_t *delta_xFX = new real_t[nFX];
00378   real_t *delta_yAC = new real_t[nAC];
00379   real_t *delta_yFX = new real_t[nFX];
00380 
00381   real_t* K             = new real_t[dim*dim];  /* matrix to store */
00382                                                 /* an intermediate */
00383                                                 /* result. */
00384 
00385   /* SOME INITIALIZATIONS:
00386    * --------------------- */
00387   for( run1 = 0; run1 < dim*dim; run1++ ){
00388     K              [run1] = 0.0;
00389     Primal_Dual_VAR[run1] = 0.0;
00390   }
00391 
00392 
00393   /* ================================================================= */
00394 
00395   /* FIRST MATRIX MULTIPLICATION (OBTAINS THE INTERMEDIATE RESULT
00396    *  K := [ ("ACTIVE" KKT-MATRIX OF THE QP)^(-1) * g_b_bA_VAR ]^T )
00397    * THE EVALUATION OF THE INVERSE OF THE KKT-MATRIX OF THE QP
00398    * WITH RESPECT TO THE CURRENT ACTIVE SET
00399    * USES THE EXISTING CHOLESKY AND TQ-DECOMPOSITIONS. FOR DETAILS
00400    * cf. THE (protected) FUNCTION determineStepDirection. */
00401 
00402   for( run3 = 0; run3 < dim; run3++ ){
00403 
00404 
00405     for( run1 = 0; run1 < nV; run1++ ){
00406       delta_g_cov  [run1]   = g_b_bA_VAR[run3*dim+run1];
00407       delta_lb_cov [run1]   = g_b_bA_VAR[run3*dim+nV+run1];         /*  LINE-WISE LOADING OF THE INPUT */
00408       delta_ub_cov [run1]   = g_b_bA_VAR[run3*dim+nV+run1];         /*  VARIANCE-COVARIANCE            */
00409     }
00410     for( run1 = 0; run1 < nC; run1++ ){
00411       delta_lbA_cov [run1]  = g_b_bA_VAR[run3*dim+2*nV+run1];
00412       delta_ubA_cov [run1]  = g_b_bA_VAR[run3*dim+2*nV+run1];
00413     }
00414 
00415 
00416     /* EVALUATION OF THE STEP:
00417      * ------------------------------------------------------------------------------ */
00418 
00419     returnvalue = qp->determineStepDirection( delta_g_cov, delta_lbA_cov, delta_ubA_cov, delta_lb_cov, delta_ub_cov,
00420                                               Delta_bC_isZero, Delta_bB_isZero, delta_xFX,delta_xFR,
00421                                               delta_yAC,delta_yFX );
00422 
00423     /* ------------------------------------------------------------------------------ */
00424 
00425 
00426     /* STOP THE ALGORITHM IN THE CASE OF NO SUCCESFUL RETURN:
00427      * ------------------------------------------------------ */
00428     if ( returnvalue != SUCCESSFUL_RETURN ){
00429 
00430       delete[] delta_g_cov;
00431       delete[] delta_lb_cov;
00432       delete[] delta_ub_cov;
00433       delete[] delta_lbA_cov;
00434       delete[] delta_ubA_cov;
00435       delete[] delta_xFR;
00436       delete[] delta_xFX;
00437       delete[] delta_yAC;
00438       delete[] delta_yFX;
00439       delete[] K;
00440 
00441       THROWERROR( RET_STEPDIRECTION_DETERMINATION_FAILED );
00442       return returnvalue;
00443     }
00444 
00445 
00446 
00447     for( run1=0; run1<nFR; run1++ ){
00448       run2                  = FR_idx[run1];
00449       K[run3*dim+run2]      = delta_xFR[run1];
00450     }                                                               /*  LINE WISE                  */
00451     for( run1=0; run1<nFX; run1++ ){                                /*  STORAGE OF THE QP-REACTION */
00452       run2                  = FX_idx[run1];                         /*  (uses the index list)      */
00453       K[run3*dim+run2]      = delta_xFX[run1];
00454       K[run3*dim+nV+run2]   = delta_yFX[run1];
00455     }
00456     for( run1=0; run1<nAC; run1++ ){
00457       run2                  = AC_idx[run1];
00458       K[run3*dim+2*nV+run2] = delta_yAC[run1];
00459     }
00460 
00461   }
00462 
00463 
00464   /* ================================================================= */
00465 
00466   /* SECOND MATRIX MULTIPLICATION (OBTAINS THE FINAL RESULT
00467    * Primal_Dual_VAR := ("ACTIVE" KKT-MATRIX OF THE QP)^(-1) * K )
00468    * THE APPLICATION OF THE KKT-INVERSE IS AGAIN REALIZED
00469    * BY USING THE PROTECTED FUNCTION
00470    * determineStepDirection */
00471 
00472   for( run3 = 0; run3 < dim; run3++ ){
00473 
00474     for( run1 = 0; run1 < nV; run1++ ){
00475       delta_g_cov  [run1]   = K[run3+     run1*dim];
00476       delta_lb_cov [run1]   = K[run3+(nV+run1)*dim];                /*  ROW WISE LOADING OF THE */
00477       delta_ub_cov [run1]   = K[run3+(nV+run1)*dim];                /*  INTERMEDIATE RESULT K   */
00478     }
00479     for( run1 = 0; run1 < nC; run1++ ){
00480       delta_lbA_cov [run1]  = K[run3+(2*nV+run1)*dim];
00481       delta_ubA_cov [run1]  = K[run3+(2*nV+run1)*dim];
00482     }
00483 
00484 
00485     /* EVALUATION OF THE STEP:
00486      * ------------------------------------------------------------------------------ */
00487 
00488     returnvalue = qp->determineStepDirection( delta_g_cov, delta_lbA_cov, delta_ubA_cov, delta_lb_cov, delta_ub_cov,
00489                                               Delta_bC_isZero, Delta_bB_isZero, delta_xFX,delta_xFR,
00490                                               delta_yAC,delta_yFX);
00491 
00492 
00493     /* ------------------------------------------------------------------------------ */
00494 
00495 
00496     /* STOP THE ALGORITHM IN THE CASE OF NO SUCCESFUL RETURN:
00497      * ------------------------------------------------------ */
00498     if ( returnvalue != SUCCESSFUL_RETURN ){
00499 
00500       delete[] delta_g_cov;
00501       delete[] delta_lb_cov;
00502       delete[] delta_ub_cov;
00503       delete[] delta_lbA_cov;
00504       delete[] delta_ubA_cov;
00505       delete[] delta_xFR;
00506       delete[] delta_xFX;
00507       delete[] delta_yAC;
00508       delete[] delta_yFX;
00509       delete[] K;
00510 
00511       THROWERROR( RET_STEPDIRECTION_DETERMINATION_FAILED );
00512       return returnvalue;
00513     }
00514 
00515 
00516 
00517     for( run1=0; run1<nFR; run1++ ){
00518       run2                                = FR_idx[run1];
00519       Primal_Dual_VAR[run3+run2*dim]      = delta_xFR[run1];
00520     }
00521     for( run1=0; run1<nFX; run1++ ){                                 /*  ROW-WISE STORAGE */
00522       run2                  = FX_idx[run1];                          /*  OF THE RESULT.   */
00523       Primal_Dual_VAR[run3+run2*dim     ]   = delta_xFX[run1];
00524       Primal_Dual_VAR[run3+(nV+run2)*dim]   = delta_yFX[run1];
00525     }
00526     for( run1=0; run1<nAC; run1++ ){
00527       run2                                  = AC_idx[run1];
00528       Primal_Dual_VAR[run3+(2*nV+run2)*dim] = delta_yAC[run1];
00529     }
00530 
00531   }
00532 
00533 
00534   /* DEALOCATE MEMORY:
00535    * ----------------- */
00536 
00537   delete[] delta_g_cov;
00538   delete[] delta_lb_cov;
00539   delete[] delta_ub_cov;
00540   delete[] delta_lbA_cov;
00541   delete[] delta_ubA_cov;
00542   delete[] delta_xFR;
00543   delete[] delta_xFX;
00544   delete[] delta_yAC;
00545   delete[] delta_yFX;
00546   delete[] K;
00547 
00548   return SUCCESSFUL_RETURN;
00549 }
00550 
00551 
00552 /*
00553  *      g e t V a r i a n c e C o v a r i a n c e
00554  */
00555 returnValue SolutionAnalysis::getVarianceCovariance( SQProblem* qp, real_t* g_b_bA_VAR, real_t* Primal_Dual_VAR ) const
00556 {
00557         /* Call QProblem variant. */
00558         return getVarianceCovariance( (QProblem*)qp,g_b_bA_VAR,Primal_Dual_VAR );
00559 }
00560 
00561 
00562 END_NAMESPACE_QPOASES
00563 
00564 
00565 /*
00566  *      end of file
00567  */

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