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-2008 by Hans Joachim Ferreau et al. All rights reserved.
00006  *
00007  *      qpOASES is free software; you can redistribute it and/or
00008  *      modify it under the terms of the GNU Lesser General Public
00009  *      License as published by the Free Software Foundation; either
00010  *      version 2.1 of the License, or (at your option) any later version.
00011  *
00012  *      qpOASES is distributed in the hope that it will be useful,
00013  *      but WITHOUT ANY WARRANTY; without even the implied warranty of
00014  *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00015  *      Lesser General Public License for more details.
00016  *
00017  *      You should have received a copy of the GNU Lesser General Public
00018  *      License along with qpOASES; if not, write to the Free Software
00019  *      Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
00020  *
00021  */
00022 
00023 
00033 #include <EXTRAS/SolutionAnalysis.hpp>
00034 
00035 /*
00036  *      S o l u t i o n A n a l y s i s
00037  */
00038 SolutionAnalysis::SolutionAnalysis( )
00039 {
00040         
00041 }
00042 
00043 /*
00044  *      S o l u t i o n A n a l y s i s
00045  */
00046 SolutionAnalysis::SolutionAnalysis( const SolutionAnalysis& rhs )
00047 {
00048         
00049 }
00050 
00051 /*
00052  *      ~ S o l u t i o n A n a l y s i s
00053  */
00054 SolutionAnalysis::~SolutionAnalysis( )
00055 {
00056         
00057 }
00058 
00059 /*
00060  *      o p e r a t o r =
00061  */
00062 SolutionAnalysis& SolutionAnalysis::operator=( const SolutionAnalysis& rhs )
00063 {
00064         if ( this != &rhs )
00065         {
00066                 
00067         }
00068         
00069         return *this;
00070 }
00071 
00072 /*
00073  * g e t H e s s i a n I n v e r s e
00074  */
00075 returnValue SolutionAnalysis::getHessianInverse( QProblem* qp, real_t* hessianInverse )
00076 {
00077         returnValue returnvalue; /* the return value */
00078         BooleanType Delta_bC_isZero = BT_FALSE; /* (just use FALSE here) */
00079         BooleanType Delta_bB_isZero = BT_FALSE; /* (just use FALSE here) */
00080         
00081         register int run1, run2, run3;
00082         
00083         register int nFR, nFX;
00084         
00085         /* Ask for the number of free and fixed variables, assumes that active set
00086          * is constant for the covariance evaluation */
00087         nFR = qp->getNFR( );
00088         nFX = qp->getNFX( );
00089         
00090         /* Ask for the corresponding index arrays: */
00091         if ( qp->bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
00092                 return THROWERROR( RET_HOTSTART_FAILED );
00093         
00094         if ( qp->bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN )
00095                 return THROWERROR( RET_HOTSTART_FAILED );
00096         
00097         if ( qp->constraints.getActive( )->getNumberArray( AC_idx ) != SUCCESSFUL_RETURN )
00098                 return THROWERROR( RET_HOTSTART_FAILED );
00099         
00100         /* Initialization: */
00101         for( run1 = 0; run1 < NVMAX; run1++ )
00102                 delta_g_cov[ run1 ] = 0.0;
00103         
00104         for( run1 = 0; run1 < NVMAX; run1++ )
00105                 delta_lb_cov[ run1 ] = 0.0;
00106         
00107         for( run1 = 0; run1 < NVMAX; run1++ )
00108                 delta_ub_cov[ run1 ] = 0.0;
00109         
00110         for( run1 = 0; run1 < NCMAX; run1++ )
00111                 delta_lbA_cov[ run1 ] = 0.0;
00112         
00113         for( run1 = 0; run1 < NCMAX; run1++ )
00114                 delta_ubA_cov[ run1 ] = 0.0;
00115         
00116         /* The following loop solves the following:
00117          *
00118          * KKT * x =
00119          *   [delta_g_cov', delta_lbA_cov', delta_ubA_cov', delta_lb_cov', delta_ub_cov]'
00120          *
00121          * for the first NVMAX (negative) elementary vectors in order to get
00122          * transposed inverse of the Hessian. Assuming that the Hessian is
00123          * symmetric, the function will return transposed inverse, instead of the
00124          * true inverse.
00125          *
00126          * Note, that we use negative elementary vectors due because internal
00127          * implementation of the function hotstart_determineStepDirection requires
00128          * so.
00129          *
00130          * */
00131         
00132         for( run3 = 0; run3 < NVMAX; run3++ )
00133         {
00134                 /* Line wise loading of the corresponding (negative) elementary vector: */
00135                 delta_g_cov[ run3 ] = -1.0;
00136                 
00137                 /* Evaluation of the step: */
00138                 returnvalue = qp->hotstart_determineStepDirection(
00139                         FR_idx, FX_idx, AC_idx,
00140                         delta_g_cov, delta_lbA_cov, delta_ubA_cov, delta_lb_cov, delta_ub_cov,
00141                         Delta_bC_isZero, Delta_bB_isZero,
00142                         delta_xFX, delta_xFR, delta_yAC, delta_yFX
00143                         );
00144                 if ( returnvalue != SUCCESSFUL_RETURN )
00145                 {
00146                         return returnvalue;
00147                 }
00148                 
00149                 /* Line wise storage of the QP reaction: */
00150                 for( run1 = 0; run1 < nFR; run1++ )
00151                 {
00152                         run2 = FR_idx[ run1 ];
00153                         
00154                         hessianInverse[run3 * NVMAX + run2] = delta_xFR[ run1 ];
00155                 } 
00156                 
00157                 for( run1 = 0; run1 < nFX; run1++ )
00158                 { 
00159                         run2 = FX_idx[ run1 ];
00160                         
00161                         hessianInverse[run3 * NVMAX + run2] = delta_xFX[ run1 ];
00162                 }
00163                 
00164                 /* Prepare for the next iteration */
00165                 delta_g_cov[ run3 ] = 0.0;
00166         }
00167         
00168         // TODO: Perform the transpose of the inverse of the Hessian matrix
00169         
00170         return SUCCESSFUL_RETURN; 
00171 }
00172 
00173 /*
00174  * g e t H e s s i a n I n v e r s e
00175  */
00176 returnValue SolutionAnalysis::getHessianInverse( QProblemB* qp, real_t* hessianInverse )
00177 {
00178         returnValue returnvalue; /* the return value */
00179         BooleanType Delta_bB_isZero = BT_FALSE; /* (just use FALSE here) */
00180         
00181         register int run1, run2, run3;
00182         
00183         register int nFR, nFX;
00184         
00185         /* Ask for the number of free and fixed variables, assumes that active set
00186          * is constant for the covariance evaluation */
00187         nFR = qp->getNFR( );
00188         nFX = qp->getNFX( );
00189         
00190         /* Ask for the corresponding index arrays: */
00191         if ( qp->bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
00192                 return THROWERROR( RET_HOTSTART_FAILED );
00193         
00194         if ( qp->bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN )
00195                 return THROWERROR( RET_HOTSTART_FAILED );
00196         
00197         /* Initialization: */
00198         for( run1 = 0; run1 < NVMAX; run1++ )
00199                 delta_g_cov[ run1 ] = 0.0;
00200         
00201         for( run1 = 0; run1 < NVMAX; run1++ )
00202                 delta_lb_cov[ run1 ] = 0.0;
00203         
00204         for( run1 = 0; run1 < NVMAX; run1++ )
00205                 delta_ub_cov[ run1 ] = 0.0;
00206         
00207         /* The following loop solves the following:
00208          *
00209          * KKT * x =
00210          *   [delta_g_cov', delta_lb_cov', delta_ub_cov']'
00211          *
00212          * for the first NVMAX (negative) elementary vectors in order to get
00213          * transposed inverse of the Hessian. Assuming that the Hessian is
00214          * symmetric, the function will return transposed inverse, instead of the
00215          * true inverse.
00216          *
00217          * Note, that we use negative elementary vectors due because internal
00218          * implementation of the function hotstart_determineStepDirection requires
00219          * so.
00220          *
00221          * */
00222         
00223         for( run3 = 0; run3 < NVMAX; run3++ )
00224         {
00225                 /* Line wise loading of the corresponding (negative) elementary vector: */
00226                 delta_g_cov[ run3 ] = -1.0;
00227                 
00228                 /* Evaluation of the step: */
00229                 returnvalue = qp->hotstart_determineStepDirection(
00230                         FR_idx, FX_idx,
00231                         delta_g_cov, delta_lb_cov, delta_ub_cov,
00232                         Delta_bB_isZero,
00233                         delta_xFX, delta_xFR, delta_yFX
00234                         );
00235                 if ( returnvalue != SUCCESSFUL_RETURN )
00236                 {
00237                         return returnvalue;
00238                 }
00239                                 
00240                 /* Line wise storage of the QP reaction: */
00241                 for( run1 = 0; run1 < nFR; run1++ )
00242                 {
00243                         run2 = FR_idx[ run1 ];
00244                         
00245                         hessianInverse[run3 * NVMAX + run2] = delta_xFR[ run1 ];
00246                 } 
00247                 
00248                 for( run1 = 0; run1 < nFX; run1++ )
00249                 { 
00250                         run2 = FX_idx[ run1 ];
00251                         
00252                         hessianInverse[run3 * NVMAX + run2] = delta_xFX[ run1 ];
00253                 }
00254                 
00255                 /* Prepare for the next iteration */
00256                 delta_g_cov[ run3 ] = 0.0;
00257         }
00258         
00259         // TODO: Perform the transpose of the inverse of the Hessian matrix
00260         
00261         return SUCCESSFUL_RETURN; 
00262 }
00263 
00264 /*
00265  * g e t V a r i a n c e C o v a r i a n c e
00266  */
00267 
00268 #if QPOASES_USE_OLD_VERSION
00269 
00270 returnValue SolutionAnalysis::getVarianceCovariance( QProblem* qp, real_t* g_b_bA_VAR, real_t* Primal_Dual_VAR )
00271 {
00272         int run1, run2, run3; /* simple run variables (for loops). */
00273         
00274         returnValue returnvalue; /* the return value */
00275         BooleanType Delta_bC_isZero = BT_FALSE; /* (just use FALSE here) */
00276         BooleanType Delta_bB_isZero = BT_FALSE; /* (just use FALSE here) */
00277         
00278         /* ASK FOR THE NUMBER OF FREE AND FIXED VARIABLES:
00279          * (ASSUMES THAT ACTIVE SET IS CONSTANT FOR THE
00280          *  VARIANCE-COVARIANCE EVALUATION)
00281          * ----------------------------------------------- */
00282         int nFR, nFX, nAC;
00283         
00284         nFR = qp->getNFR( );
00285         nFX = qp->getNFX( );
00286         nAC = qp->getNAC( );
00287         
00288         if ( qp->bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
00289                 return THROWERROR( RET_HOTSTART_FAILED );
00290         
00291         if ( qp->bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN )
00292                 return THROWERROR( RET_HOTSTART_FAILED );
00293         
00294         if ( qp->constraints.getActive( )->getNumberArray( AC_idx ) != SUCCESSFUL_RETURN )
00295                 return THROWERROR( RET_HOTSTART_FAILED );
00296         
00297         /* SOME INITIALIZATIONS:
00298          * --------------------- */
00299         for( run1 = 0; run1 < KKT_DIM * KKT_DIM; run1++ )
00300         {
00301                 K [run1] = 0.0;
00302                 Primal_Dual_VAR[run1] = 0.0;
00303         }
00304         
00305         /* ================================================================= */
00306         
00307         /* FIRST MATRIX MULTIPLICATION (OBTAINS THE INTERMEDIATE RESULT
00308          *  K := [ ("ACTIVE" KKT-MATRIX OF THE QP)^(-1) * g_b_bA_VAR ]^T )
00309          * THE EVALUATION OF THE INVERSE OF THE KKT-MATRIX OF THE QP
00310          * WITH RESPECT TO THE CURRENT ACTIVE SET
00311          * USES THE EXISTING CHOLESKY AND TQ-DECOMPOSITIONS. FOR DETAILS
00312          * cf. THE (protected) FUNCTION determineStepDirection. */
00313         
00314         for( run3 = 0; run3 < KKT_DIM; run3++ )
00315         {
00316                 
00317                 for( run1 = 0; run1 < NVMAX; run1++ )
00318                 {
00319                         delta_g_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+run1];
00320                         delta_lb_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+NVMAX+run1]; /*  LINE-WISE LOADING OF THE INPUT */
00321                         delta_ub_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+NVMAX+run1]; /*  VARIANCE-COVARIANCE            */
00322                 }
00323                 for( run1 = 0; run1 < NCMAX; run1++ )
00324                 {
00325                         delta_lbA_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+2*NVMAX+run1];
00326                         delta_ubA_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+2*NVMAX+run1];
00327                 }
00328                 
00329                 /* EVALUATION OF THE STEP:
00330                  * ------------------------------------------------------------------------------ */
00331                 
00332                 returnvalue = qp->hotstart_determineStepDirection(
00333                         FR_idx, FX_idx, AC_idx,
00334                         delta_g_cov, delta_lbA_cov, delta_ubA_cov, delta_lb_cov, delta_ub_cov,
00335                         Delta_bC_isZero, Delta_bB_isZero, delta_xFX,delta_xFR,
00336                         delta_yAC,delta_yFX );
00337                 
00338                 /* ------------------------------------------------------------------------------ */
00339                 
00340                 /* STOP THE ALGORITHM IN THE CASE OF NO SUCCESFUL RETURN:
00341                  * ------------------------------------------------------ */
00342                 if ( returnvalue != SUCCESSFUL_RETURN )
00343                 {
00344                         return returnvalue;
00345                 }
00346                 
00347                 /*  LINE WISE                  */
00348                 /*  STORAGE OF THE QP-REACTION */
00349                 /*  (uses the index list)      */
00350                 
00351                 for( run1=0; run1<nFR; run1++ )
00352                 {
00353                         run2 = FR_idx[run1];
00354                         K[run3*KKT_DIM+run2] = delta_xFR[run1];
00355                 } 
00356                 for( run1=0; run1<nFX; run1++ )
00357                 { 
00358                         run2 = FX_idx[run1]; 
00359                         K[run3*KKT_DIM+run2] = delta_xFX[run1];
00360                         K[run3*KKT_DIM+NVMAX+run2] = delta_yFX[run1];
00361                 }
00362                 for( run1=0; run1<nAC; run1++ )
00363                 {
00364                         run2 = AC_idx[run1];
00365                         K[run3*KKT_DIM+2*NVMAX+run2] = delta_yAC[run1];
00366                 }
00367         }
00368         
00369         /* ================================================================= */
00370         
00371         /* SECOND MATRIX MULTIPLICATION (OBTAINS THE FINAL RESULT
00372          * Primal_Dual_VAR := ("ACTIVE" KKT-MATRIX OF THE QP)^(-1) * K )
00373          * THE APPLICATION OF THE KKT-INVERSE IS AGAIN REALIZED
00374          * BY USING THE PROTECTED FUNCTION
00375          * determineStepDirection */
00376         
00377         for( run3 = 0; run3 < KKT_DIM; run3++ )
00378         {
00379                 
00380                 for( run1 = 0; run1 < NVMAX; run1++ )
00381                 {
00382                         delta_g_cov [run1] = K[run3+ run1*KKT_DIM];
00383                         delta_lb_cov [run1] = K[run3+(NVMAX+run1)*KKT_DIM]; /*  ROW WISE LOADING OF THE */
00384                         delta_ub_cov [run1] = K[run3+(NVMAX+run1)*KKT_DIM]; /*  INTERMEDIATE RESULT K   */
00385                 }
00386                 for( run1 = 0; run1 < NCMAX; run1++ )
00387                 {
00388                         delta_lbA_cov [run1] = K[run3+(2*NVMAX+run1)*KKT_DIM];
00389                         delta_ubA_cov [run1] = K[run3+(2*NVMAX+run1)*KKT_DIM];
00390                 }
00391                 
00392                 /* EVALUATION OF THE STEP:
00393                  * ------------------------------------------------------------------------------ */
00394                 
00395                 returnvalue = qp->hotstart_determineStepDirection(
00396                         FR_idx, FX_idx, AC_idx,
00397                         delta_g_cov, delta_lbA_cov, delta_ubA_cov, delta_lb_cov, delta_ub_cov,
00398                         Delta_bC_isZero, Delta_bB_isZero, delta_xFX,delta_xFR,
00399                         delta_yAC,delta_yFX );
00400                 
00401                 /* ------------------------------------------------------------------------------ */
00402                 
00403                 /* STOP THE ALGORITHM IN THE CASE OF NO SUCCESFUL RETURN:
00404                  * ------------------------------------------------------ */
00405                 if ( returnvalue != SUCCESSFUL_RETURN )
00406                 {
00407                         return returnvalue;
00408                 }
00409                 
00410                 /*  ROW-WISE STORAGE */
00411                 /*  OF THE RESULT.   */
00412                 
00413                 for( run1=0; run1<nFR; run1++ )
00414                 {
00415                         run2 = FR_idx[run1];
00416                         Primal_Dual_VAR[run3+run2*KKT_DIM] = delta_xFR[run1];
00417                 }
00418                 for( run1=0; run1<nFX; run1++ )
00419                 { 
00420                         run2 = FX_idx[run1]; 
00421                         Primal_Dual_VAR[run3+run2*KKT_DIM ] = delta_xFX[run1];
00422                         Primal_Dual_VAR[run3+(NVMAX+run2)*KKT_DIM] = delta_yFX[run1];
00423                 }
00424                 for( run1=0; run1<nAC; run1++ )
00425                 {
00426                         run2 = AC_idx[run1];
00427                         Primal_Dual_VAR[run3+(2*NVMAX+run2)*KKT_DIM] = delta_yAC[run1];
00428                 }
00429         }
00430         
00431         return SUCCESSFUL_RETURN;
00432 }
00433 
00434 #endif


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