You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
435 lines
12 KiB
435 lines
12 KiB
6 years ago
|
/*
|
||
|
|
* This file is part of qpOASES.
|
||
|
|
*
|
||
|
|
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||
|
|
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||
|
|
*
|
||
|
|
* qpOASES is free software; you can redistribute it and/or
|
||
|
|
* modify it under the terms of the GNU Lesser General Public
|
||
|
|
* License as published by the Free Software Foundation; either
|
||
|
|
* version 2.1 of the License, or (at your option) any later version.
|
||
|
|
*
|
||
|
|
* qpOASES is distributed in the hope that it will be useful,
|
||
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||
|
|
* Lesser General Public License for more details.
|
||
|
|
*
|
||
|
|
* You should have received a copy of the GNU Lesser General Public
|
||
|
|
* License along with qpOASES; if not, write to the Free Software
|
||
|
|
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||
|
|
*
|
||
|
|
*/
|
||
|
|
|
||
|
|
|
||
|
|
/**
|
||
|
|
* \file SRC/EXTRAS/SolutionAnalysis.cpp
|
||
|
|
* \author Milan Vukov, Boris Houska, Hans Joachim Ferreau
|
||
|
|
* \version 1.3embedded
|
||
|
|
* \date 2012
|
||
|
|
*
|
||
|
|
* Solution analysis class, based on a class in the standard version of the qpOASES
|
||
|
|
*/
|
||
|
|
|
||
|
|
#include <EXTRAS/SolutionAnalysis.hpp>
|
||
|
|
|
||
|
|
/*
|
||
|
|
* S o l u t i o n A n a l y s i s
|
||
|
|
*/
|
||
|
|
SolutionAnalysis::SolutionAnalysis( )
|
||
|
|
{
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
* S o l u t i o n A n a l y s i s
|
||
|
|
*/
|
||
|
|
SolutionAnalysis::SolutionAnalysis( const SolutionAnalysis& rhs )
|
||
|
|
{
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
* ~ S o l u t i o n A n a l y s i s
|
||
|
|
*/
|
||
|
|
SolutionAnalysis::~SolutionAnalysis( )
|
||
|
|
{
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
* o p e r a t o r =
|
||
|
|
*/
|
||
|
|
SolutionAnalysis& SolutionAnalysis::operator=( const SolutionAnalysis& rhs )
|
||
|
|
{
|
||
|
|
if ( this != &rhs )
|
||
|
|
{
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
return *this;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
* g e t H e s s i a n I n v e r s e
|
||
|
|
*/
|
||
|
|
returnValue SolutionAnalysis::getHessianInverse( QProblem* qp, real_t* hessianInverse )
|
||
|
|
{
|
||
|
|
returnValue returnvalue; /* the return value */
|
||
|
|
BooleanType Delta_bC_isZero = BT_FALSE; /* (just use FALSE here) */
|
||
|
|
BooleanType Delta_bB_isZero = BT_FALSE; /* (just use FALSE here) */
|
||
|
|
|
||
|
|
register int run1, run2, run3;
|
||
|
|
|
||
|
|
register int nFR, nFX;
|
||
|
|
|
||
|
|
/* Ask for the number of free and fixed variables, assumes that active set
|
||
|
|
* is constant for the covariance evaluation */
|
||
|
|
nFR = qp->getNFR( );
|
||
|
|
nFX = qp->getNFX( );
|
||
|
|
|
||
|
|
/* Ask for the corresponding index arrays: */
|
||
|
|
if ( qp->bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
|
||
|
|
return THROWERROR( RET_HOTSTART_FAILED );
|
||
|
|
|
||
|
|
if ( qp->bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN )
|
||
|
|
return THROWERROR( RET_HOTSTART_FAILED );
|
||
|
|
|
||
|
|
if ( qp->constraints.getActive( )->getNumberArray( AC_idx ) != SUCCESSFUL_RETURN )
|
||
|
|
return THROWERROR( RET_HOTSTART_FAILED );
|
||
|
|
|
||
|
|
/* Initialization: */
|
||
|
|
for( run1 = 0; run1 < NVMAX; run1++ )
|
||
|
|
delta_g_cov[ run1 ] = 0.0;
|
||
|
|
|
||
|
|
for( run1 = 0; run1 < NVMAX; run1++ )
|
||
|
|
delta_lb_cov[ run1 ] = 0.0;
|
||
|
|
|
||
|
|
for( run1 = 0; run1 < NVMAX; run1++ )
|
||
|
|
delta_ub_cov[ run1 ] = 0.0;
|
||
|
|
|
||
|
|
for( run1 = 0; run1 < NCMAX; run1++ )
|
||
|
|
delta_lbA_cov[ run1 ] = 0.0;
|
||
|
|
|
||
|
|
for( run1 = 0; run1 < NCMAX; run1++ )
|
||
|
|
delta_ubA_cov[ run1 ] = 0.0;
|
||
|
|
|
||
|
|
/* The following loop solves the following:
|
||
|
|
*
|
||
|
|
* KKT * x =
|
||
|
|
* [delta_g_cov', delta_lbA_cov', delta_ubA_cov', delta_lb_cov', delta_ub_cov]'
|
||
|
|
*
|
||
|
|
* for the first NVMAX (negative) elementary vectors in order to get
|
||
|
|
* transposed inverse of the Hessian. Assuming that the Hessian is
|
||
|
|
* symmetric, the function will return transposed inverse, instead of the
|
||
|
|
* true inverse.
|
||
|
|
*
|
||
|
|
* Note, that we use negative elementary vectors due because internal
|
||
|
|
* implementation of the function hotstart_determineStepDirection requires
|
||
|
|
* so.
|
||
|
|
*
|
||
|
|
* */
|
||
|
|
|
||
|
|
for( run3 = 0; run3 < NVMAX; run3++ )
|
||
|
|
{
|
||
|
|
/* Line wise loading of the corresponding (negative) elementary vector: */
|
||
|
|
delta_g_cov[ run3 ] = -1.0;
|
||
|
|
|
||
|
|
/* Evaluation of the step: */
|
||
|
|
returnvalue = qp->hotstart_determineStepDirection(
|
||
|
|
FR_idx, FX_idx, AC_idx,
|
||
|
|
delta_g_cov, delta_lbA_cov, delta_ubA_cov, delta_lb_cov, delta_ub_cov,
|
||
|
|
Delta_bC_isZero, Delta_bB_isZero,
|
||
|
|
delta_xFX, delta_xFR, delta_yAC, delta_yFX
|
||
|
|
);
|
||
|
|
if ( returnvalue != SUCCESSFUL_RETURN )
|
||
|
|
{
|
||
|
|
return returnvalue;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Line wise storage of the QP reaction: */
|
||
|
|
for( run1 = 0; run1 < nFR; run1++ )
|
||
|
|
{
|
||
|
|
run2 = FR_idx[ run1 ];
|
||
|
|
|
||
|
|
hessianInverse[run3 * NVMAX + run2] = delta_xFR[ run1 ];
|
||
|
|
}
|
||
|
|
|
||
|
|
for( run1 = 0; run1 < nFX; run1++ )
|
||
|
|
{
|
||
|
|
run2 = FX_idx[ run1 ];
|
||
|
|
|
||
|
|
hessianInverse[run3 * NVMAX + run2] = delta_xFX[ run1 ];
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Prepare for the next iteration */
|
||
|
|
delta_g_cov[ run3 ] = 0.0;
|
||
|
|
}
|
||
|
|
|
||
|
|
// TODO: Perform the transpose of the inverse of the Hessian matrix
|
||
|
|
|
||
|
|
return SUCCESSFUL_RETURN;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
* g e t H e s s i a n I n v e r s e
|
||
|
|
*/
|
||
|
|
returnValue SolutionAnalysis::getHessianInverse( QProblemB* qp, real_t* hessianInverse )
|
||
|
|
{
|
||
|
|
returnValue returnvalue; /* the return value */
|
||
|
|
BooleanType Delta_bB_isZero = BT_FALSE; /* (just use FALSE here) */
|
||
|
|
|
||
|
|
register int run1, run2, run3;
|
||
|
|
|
||
|
|
register int nFR, nFX;
|
||
|
|
|
||
|
|
/* Ask for the number of free and fixed variables, assumes that active set
|
||
|
|
* is constant for the covariance evaluation */
|
||
|
|
nFR = qp->getNFR( );
|
||
|
|
nFX = qp->getNFX( );
|
||
|
|
|
||
|
|
/* Ask for the corresponding index arrays: */
|
||
|
|
if ( qp->bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
|
||
|
|
return THROWERROR( RET_HOTSTART_FAILED );
|
||
|
|
|
||
|
|
if ( qp->bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN )
|
||
|
|
return THROWERROR( RET_HOTSTART_FAILED );
|
||
|
|
|
||
|
|
/* Initialization: */
|
||
|
|
for( run1 = 0; run1 < NVMAX; run1++ )
|
||
|
|
delta_g_cov[ run1 ] = 0.0;
|
||
|
|
|
||
|
|
for( run1 = 0; run1 < NVMAX; run1++ )
|
||
|
|
delta_lb_cov[ run1 ] = 0.0;
|
||
|
|
|
||
|
|
for( run1 = 0; run1 < NVMAX; run1++ )
|
||
|
|
delta_ub_cov[ run1 ] = 0.0;
|
||
|
|
|
||
|
|
/* The following loop solves the following:
|
||
|
|
*
|
||
|
|
* KKT * x =
|
||
|
|
* [delta_g_cov', delta_lb_cov', delta_ub_cov']'
|
||
|
|
*
|
||
|
|
* for the first NVMAX (negative) elementary vectors in order to get
|
||
|
|
* transposed inverse of the Hessian. Assuming that the Hessian is
|
||
|
|
* symmetric, the function will return transposed inverse, instead of the
|
||
|
|
* true inverse.
|
||
|
|
*
|
||
|
|
* Note, that we use negative elementary vectors due because internal
|
||
|
|
* implementation of the function hotstart_determineStepDirection requires
|
||
|
|
* so.
|
||
|
|
*
|
||
|
|
* */
|
||
|
|
|
||
|
|
for( run3 = 0; run3 < NVMAX; run3++ )
|
||
|
|
{
|
||
|
|
/* Line wise loading of the corresponding (negative) elementary vector: */
|
||
|
|
delta_g_cov[ run3 ] = -1.0;
|
||
|
|
|
||
|
|
/* Evaluation of the step: */
|
||
|
|
returnvalue = qp->hotstart_determineStepDirection(
|
||
|
|
FR_idx, FX_idx,
|
||
|
|
delta_g_cov, delta_lb_cov, delta_ub_cov,
|
||
|
|
Delta_bB_isZero,
|
||
|
|
delta_xFX, delta_xFR, delta_yFX
|
||
|
|
);
|
||
|
|
if ( returnvalue != SUCCESSFUL_RETURN )
|
||
|
|
{
|
||
|
|
return returnvalue;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Line wise storage of the QP reaction: */
|
||
|
|
for( run1 = 0; run1 < nFR; run1++ )
|
||
|
|
{
|
||
|
|
run2 = FR_idx[ run1 ];
|
||
|
|
|
||
|
|
hessianInverse[run3 * NVMAX + run2] = delta_xFR[ run1 ];
|
||
|
|
}
|
||
|
|
|
||
|
|
for( run1 = 0; run1 < nFX; run1++ )
|
||
|
|
{
|
||
|
|
run2 = FX_idx[ run1 ];
|
||
|
|
|
||
|
|
hessianInverse[run3 * NVMAX + run2] = delta_xFX[ run1 ];
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Prepare for the next iteration */
|
||
|
|
delta_g_cov[ run3 ] = 0.0;
|
||
|
|
}
|
||
|
|
|
||
|
|
// TODO: Perform the transpose of the inverse of the Hessian matrix
|
||
|
|
|
||
|
|
return SUCCESSFUL_RETURN;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
* g e t V a r i a n c e C o v a r i a n c e
|
||
|
|
*/
|
||
|
|
|
||
|
|
#if QPOASES_USE_OLD_VERSION
|
||
|
|
|
||
|
|
returnValue SolutionAnalysis::getVarianceCovariance( QProblem* qp, real_t* g_b_bA_VAR, real_t* Primal_Dual_VAR )
|
||
|
|
{
|
||
|
|
int run1, run2, run3; /* simple run variables (for loops). */
|
||
|
|
|
||
|
|
returnValue returnvalue; /* the return value */
|
||
|
|
BooleanType Delta_bC_isZero = BT_FALSE; /* (just use FALSE here) */
|
||
|
|
BooleanType Delta_bB_isZero = BT_FALSE; /* (just use FALSE here) */
|
||
|
|
|
||
|
|
/* ASK FOR THE NUMBER OF FREE AND FIXED VARIABLES:
|
||
|
|
* (ASSUMES THAT ACTIVE SET IS CONSTANT FOR THE
|
||
|
|
* VARIANCE-COVARIANCE EVALUATION)
|
||
|
|
* ----------------------------------------------- */
|
||
|
|
int nFR, nFX, nAC;
|
||
|
|
|
||
|
|
nFR = qp->getNFR( );
|
||
|
|
nFX = qp->getNFX( );
|
||
|
|
nAC = qp->getNAC( );
|
||
|
|
|
||
|
|
if ( qp->bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
|
||
|
|
return THROWERROR( RET_HOTSTART_FAILED );
|
||
|
|
|
||
|
|
if ( qp->bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN )
|
||
|
|
return THROWERROR( RET_HOTSTART_FAILED );
|
||
|
|
|
||
|
|
if ( qp->constraints.getActive( )->getNumberArray( AC_idx ) != SUCCESSFUL_RETURN )
|
||
|
|
return THROWERROR( RET_HOTSTART_FAILED );
|
||
|
|
|
||
|
|
/* SOME INITIALIZATIONS:
|
||
|
|
* --------------------- */
|
||
|
|
for( run1 = 0; run1 < KKT_DIM * KKT_DIM; run1++ )
|
||
|
|
{
|
||
|
|
K [run1] = 0.0;
|
||
|
|
Primal_Dual_VAR[run1] = 0.0;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* ================================================================= */
|
||
|
|
|
||
|
|
/* FIRST MATRIX MULTIPLICATION (OBTAINS THE INTERMEDIATE RESULT
|
||
|
|
* K := [ ("ACTIVE" KKT-MATRIX OF THE QP)^(-1) * g_b_bA_VAR ]^T )
|
||
|
|
* THE EVALUATION OF THE INVERSE OF THE KKT-MATRIX OF THE QP
|
||
|
|
* WITH RESPECT TO THE CURRENT ACTIVE SET
|
||
|
|
* USES THE EXISTING CHOLESKY AND TQ-DECOMPOSITIONS. FOR DETAILS
|
||
|
|
* cf. THE (protected) FUNCTION determineStepDirection. */
|
||
|
|
|
||
|
|
for( run3 = 0; run3 < KKT_DIM; run3++ )
|
||
|
|
{
|
||
|
|
|
||
|
|
for( run1 = 0; run1 < NVMAX; run1++ )
|
||
|
|
{
|
||
|
|
delta_g_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+run1];
|
||
|
|
delta_lb_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+NVMAX+run1]; /* LINE-WISE LOADING OF THE INPUT */
|
||
|
|
delta_ub_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+NVMAX+run1]; /* VARIANCE-COVARIANCE */
|
||
|
|
}
|
||
|
|
for( run1 = 0; run1 < NCMAX; run1++ )
|
||
|
|
{
|
||
|
|
delta_lbA_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+2*NVMAX+run1];
|
||
|
|
delta_ubA_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+2*NVMAX+run1];
|
||
|
|
}
|
||
|
|
|
||
|
|
/* EVALUATION OF THE STEP:
|
||
|
|
* ------------------------------------------------------------------------------ */
|
||
|
|
|
||
|
|
returnvalue = qp->hotstart_determineStepDirection(
|
||
|
|
FR_idx, FX_idx, AC_idx,
|
||
|
|
delta_g_cov, delta_lbA_cov, delta_ubA_cov, delta_lb_cov, delta_ub_cov,
|
||
|
|
Delta_bC_isZero, Delta_bB_isZero, delta_xFX,delta_xFR,
|
||
|
|
delta_yAC,delta_yFX );
|
||
|
|
|
||
|
|
/* ------------------------------------------------------------------------------ */
|
||
|
|
|
||
|
|
/* STOP THE ALGORITHM IN THE CASE OF NO SUCCESFUL RETURN:
|
||
|
|
* ------------------------------------------------------ */
|
||
|
|
if ( returnvalue != SUCCESSFUL_RETURN )
|
||
|
|
{
|
||
|
|
return returnvalue;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* LINE WISE */
|
||
|
|
/* STORAGE OF THE QP-REACTION */
|
||
|
|
/* (uses the index list) */
|
||
|
|
|
||
|
|
for( run1=0; run1<nFR; run1++ )
|
||
|
|
{
|
||
|
|
run2 = FR_idx[run1];
|
||
|
|
K[run3*KKT_DIM+run2] = delta_xFR[run1];
|
||
|
|
}
|
||
|
|
for( run1=0; run1<nFX; run1++ )
|
||
|
|
{
|
||
|
|
run2 = FX_idx[run1];
|
||
|
|
K[run3*KKT_DIM+run2] = delta_xFX[run1];
|
||
|
|
K[run3*KKT_DIM+NVMAX+run2] = delta_yFX[run1];
|
||
|
|
}
|
||
|
|
for( run1=0; run1<nAC; run1++ )
|
||
|
|
{
|
||
|
|
run2 = AC_idx[run1];
|
||
|
|
K[run3*KKT_DIM+2*NVMAX+run2] = delta_yAC[run1];
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/* ================================================================= */
|
||
|
|
|
||
|
|
/* SECOND MATRIX MULTIPLICATION (OBTAINS THE FINAL RESULT
|
||
|
|
* Primal_Dual_VAR := ("ACTIVE" KKT-MATRIX OF THE QP)^(-1) * K )
|
||
|
|
* THE APPLICATION OF THE KKT-INVERSE IS AGAIN REALIZED
|
||
|
|
* BY USING THE PROTECTED FUNCTION
|
||
|
|
* determineStepDirection */
|
||
|
|
|
||
|
|
for( run3 = 0; run3 < KKT_DIM; run3++ )
|
||
|
|
{
|
||
|
|
|
||
|
|
for( run1 = 0; run1 < NVMAX; run1++ )
|
||
|
|
{
|
||
|
|
delta_g_cov [run1] = K[run3+ run1*KKT_DIM];
|
||
|
|
delta_lb_cov [run1] = K[run3+(NVMAX+run1)*KKT_DIM]; /* ROW WISE LOADING OF THE */
|
||
|
|
delta_ub_cov [run1] = K[run3+(NVMAX+run1)*KKT_DIM]; /* INTERMEDIATE RESULT K */
|
||
|
|
}
|
||
|
|
for( run1 = 0; run1 < NCMAX; run1++ )
|
||
|
|
{
|
||
|
|
delta_lbA_cov [run1] = K[run3+(2*NVMAX+run1)*KKT_DIM];
|
||
|
|
delta_ubA_cov [run1] = K[run3+(2*NVMAX+run1)*KKT_DIM];
|
||
|
|
}
|
||
|
|
|
||
|
|
/* EVALUATION OF THE STEP:
|
||
|
|
* ------------------------------------------------------------------------------ */
|
||
|
|
|
||
|
|
returnvalue = qp->hotstart_determineStepDirection(
|
||
|
|
FR_idx, FX_idx, AC_idx,
|
||
|
|
delta_g_cov, delta_lbA_cov, delta_ubA_cov, delta_lb_cov, delta_ub_cov,
|
||
|
|
Delta_bC_isZero, Delta_bB_isZero, delta_xFX,delta_xFR,
|
||
|
|
delta_yAC,delta_yFX );
|
||
|
|
|
||
|
|
/* ------------------------------------------------------------------------------ */
|
||
|
|
|
||
|
|
/* STOP THE ALGORITHM IN THE CASE OF NO SUCCESFUL RETURN:
|
||
|
|
* ------------------------------------------------------ */
|
||
|
|
if ( returnvalue != SUCCESSFUL_RETURN )
|
||
|
|
{
|
||
|
|
return returnvalue;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* ROW-WISE STORAGE */
|
||
|
|
/* OF THE RESULT. */
|
||
|
|
|
||
|
|
for( run1=0; run1<nFR; run1++ )
|
||
|
|
{
|
||
|
|
run2 = FR_idx[run1];
|
||
|
|
Primal_Dual_VAR[run3+run2*KKT_DIM] = delta_xFR[run1];
|
||
|
|
}
|
||
|
|
for( run1=0; run1<nFX; run1++ )
|
||
|
|
{
|
||
|
|
run2 = FX_idx[run1];
|
||
|
|
Primal_Dual_VAR[run3+run2*KKT_DIM ] = delta_xFX[run1];
|
||
|
|
Primal_Dual_VAR[run3+(NVMAX+run2)*KKT_DIM] = delta_yFX[run1];
|
||
|
|
}
|
||
|
|
for( run1=0; run1<nAC; run1++ )
|
||
|
|
{
|
||
|
|
run2 = AC_idx[run1];
|
||
|
|
Primal_Dual_VAR[run3+(2*NVMAX+run2)*KKT_DIM] = delta_yAC[run1];
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
return SUCCESSFUL_RETURN;
|
||
|
|
}
|
||
|
|
|
||
|
|
#endif
|