sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
openpilot is an open source driver assistance system. openpilot performs the functions of Automated Lane Centering and Adaptive Cruise Control for over 200 supported car makes and models.
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.
2738 Commits
113 Branches
85 Tags
6.2 GiB
Tag: Branch: Tree: 2a935a2010
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from '2a935a2010'
${ noResults }
openpilot_comma/phonelibs/qpoases/SRC/QProblemB.cpp

2152 lines
48 KiB
Raw Normal View History Unescape

bring over phonelibs minus frida-gum and qsml
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/QProblemB.cpp
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*
* Implementation of the QProblemB class which is able to use the newly
* developed online active set strategy for parametric quadratic programming.
*/
#include <QProblemB.hpp>
#include <stdio.h>
void printmatrix(char *name, double *A, int m, int n) {
int i, j;
printf("%s = [...\n", name);
for (i = 0; i < m; i++) {
for (j = 0; j < n; j++)
printf(" % 9.4f", A[i*n+j]);
printf(",\n");
}
printf("];\n");
}
/*****************************************************************************
* P U B L I C *
*****************************************************************************/
/*
* Q P r o b l e m B
*/
QProblemB::QProblemB( )
{
/* reset global message handler */
getGlobalMessageHandler( )->reset( );
hasHessian = BT_FALSE;
bounds.init( 0 );
hasCholesky = BT_FALSE;
tau = 0.0;
hessianType = HST_POSDEF_NULLSPACE; /* Hessian is assumed to be positive definite by default */
infeasible = BT_FALSE;
unbounded = BT_FALSE;
status = QPS_NOTINITIALISED;
#ifdef PC_DEBUG
printlevel = PL_MEDIUM;
setPrintLevel( PL_MEDIUM );
#else
printlevel = QPOASES_PRINTLEVEL;
#endif
count = 0;
}
/*
* Q P r o b l e m B
*/
QProblemB::QProblemB( int _nV )
{
/* consistency check */
if ( _nV <= 0 )
{
_nV = 1;
THROWERROR( RET_INVALID_ARGUMENTS );
}
hasHessian = BT_FALSE;
/* reset global message handler */
getGlobalMessageHandler( )->reset( );
bounds.init( _nV );
hasCholesky = BT_FALSE;
tau = 0.0;
hessianType = HST_POSDEF_NULLSPACE; /* Hessian is assumed to be positive definite by default */
infeasible = BT_FALSE;
unbounded = BT_FALSE;
status = QPS_NOTINITIALISED;
#ifdef PC_DEBUG
printlevel = PL_MEDIUM;
setPrintLevel( PL_MEDIUM );
#else
printlevel = QPOASES_PRINTLEVEL;
#endif
count = 0;
}
/*
* Q P r o b l e m B
*/
QProblemB::QProblemB( const QProblemB& rhs )
{
int i, j;
int _nV = rhs.bounds.getNV( );
for( i=0; i<_nV; ++i )
for( j=0; j<_nV; ++j )
H[i*NVMAX + j] = rhs.H[i*NVMAX + j];
hasHessian = rhs.hasHessian;
for( i=0; i<_nV; ++i )
g[i] = rhs.g[i];
for( i=0; i<_nV; ++i )
lb[i] = rhs.lb[i];
for( i=0; i<_nV; ++i )
ub[i] = rhs.ub[i];
bounds = rhs.bounds;
for( i=0; i<_nV; ++i )
for( j=0; j<_nV; ++j )
R[i*NVMAX + j] = rhs.R[i*NVMAX + j];
hasCholesky = rhs.hasCholesky;
for( i=0; i<_nV; ++i )
x[i] = rhs.x[i];
for( i=0; i<_nV; ++i )
y[i] = rhs.y[i];
tau = rhs.tau;
hessianType = rhs.hessianType;
infeasible = rhs.infeasible;
unbounded = rhs.unbounded;
status = rhs.status;
printlevel = rhs.printlevel;
count = rhs.count;
}
/*
* ~ Q P r o b l e m B
*/
QProblemB::~QProblemB( )
{
}
/*
* o p e r a t o r =
*/
QProblemB& QProblemB::operator=( const QProblemB& rhs )
{
int i, j;
if ( this != &rhs )
{
int _nV = rhs.bounds.getNV( );
for( i=0; i<_nV; ++i )
for( j=0; j<_nV; ++j )
H[i*NVMAX + j] = rhs.H[i*NVMAX + j];
hasHessian = rhs.hasHessian;
for( i=0; i<_nV; ++i )
g[i] = rhs.g[i];
for( i=0; i<_nV; ++i )
lb[i] = rhs.lb[i];
for( i=0; i<_nV; ++i )
ub[i] = rhs.ub[i];
bounds = rhs.bounds;
for( i=0; i<_nV; ++i )
for( j=0; j<_nV; ++j )
R[i*NVMAX + j] = rhs.R[i*NVMAX + j];
hasCholesky = rhs.hasCholesky;
for( i=0; i<_nV; ++i )
x[i] = rhs.x[i];
for( i=0; i<_nV; ++i )
y[i] = rhs.y[i];
tau = rhs.tau;
hessianType = rhs.hessianType;
infeasible = rhs.infeasible;
unbounded = rhs.unbounded;
status = rhs.status;
printlevel = rhs.printlevel;
setPrintLevel( rhs.printlevel );
count = rhs.count;
}
return *this;
}
/*
* r e s e t
*/
returnValue QProblemB::reset( )
{
int i, j;
int nV = getNV( );
/** 0) Reset has Hessian flag. */
hasHessian = BT_FALSE;
/* 1) Reset bounds. */
bounds.init( nV );
/* 2) Reset Cholesky decomposition. */
for( i=0; i<nV; ++i )
for( j=0; j<nV; ++j )
R[i*NVMAX + j] = 0.0;
hasCholesky = BT_FALSE;
/* 3) Reset steplength and status flags. */
tau = 0.0;
hessianType = HST_POSDEF_NULLSPACE; /* Hessian is assumed to be positive definite by default */
infeasible = BT_FALSE;
unbounded = BT_FALSE;
status = QPS_NOTINITIALISED;
return SUCCESSFUL_RETURN;
}
/*
* i n i t
*/
returnValue QProblemB::init( const real_t* const _H, const real_t* const _g,
const real_t* const _lb, const real_t* const _ub,
int& nWSR, const real_t* const yOpt, real_t* const cputime
)
{
/* 1) Setup QP data. */
if (setupQPdata(_H, 0, _g, _lb, _ub) != SUCCESSFUL_RETURN)
return THROWERROR( RET_INVALID_ARGUMENTS );
/* 2) Call to main initialisation routine (without any additional information). */
return solveInitialQP(0, yOpt, 0, nWSR, cputime);
}
returnValue QProblemB::init( const real_t* const _H, const real_t* const _R, const real_t* const _g,
const real_t* const _lb, const real_t* const _ub,
int& nWSR, const real_t* const yOpt, real_t* const cputime
)
{
/* 1) Setup QP data. */
if (setupQPdata(_H, _R, _g, _lb, _ub) != SUCCESSFUL_RETURN)
return THROWERROR( RET_INVALID_ARGUMENTS );
/* 2) Call to main initialisation routine (without any additional information). */
return solveInitialQP(0, yOpt, 0, nWSR, cputime);
}
/*
* h o t s t a r t
*/
returnValue QProblemB::hotstart( const real_t* const g_new, const real_t* const lb_new, const real_t* const ub_new,
int& nWSR, real_t* const cputime
)
{
int l;
/* consistency check */
if ( ( getStatus( ) == QPS_NOTINITIALISED ) ||
( getStatus( ) == QPS_PREPARINGAUXILIARYQP ) ||
( getStatus( ) == QPS_PERFORMINGHOMOTOPY ) )
{
return THROWERROR( RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED );
}
/* start runtime measurement */
real_t starttime = 0.0;
if ( cputime != 0 )
starttime = getCPUtime( );
/* I) PREPARATIONS */
/* 1) Reset status flags and increase QP counter. */
infeasible = BT_FALSE;
unbounded = BT_FALSE;
++count;
/* 2) Allocate delta vectors of gradient and bounds. */
returnValue returnvalue;
BooleanType Delta_bB_isZero;
int FR_idx[NVMAX];
int FX_idx[NVMAX];
real_t delta_g[NVMAX];
real_t delta_lb[NVMAX];
real_t delta_ub[NVMAX];
real_t delta_xFR[NVMAX];
real_t delta_xFX[NVMAX];
real_t delta_yFX[NVMAX];
int BC_idx;
SubjectToStatus BC_status;
#ifdef PC_DEBUG
char messageString[80];
#endif
/* II) MAIN HOMOTOPY LOOP */
for( l=0; l<nWSR; ++l )
{
status = QPS_PERFORMINGHOMOTOPY;
if ( printlevel == PL_HIGH )
{
#ifdef PC_DEBUG
sprintf( messageString,"%d ...",l );
getGlobalMessageHandler( )->throwInfo( RET_ITERATION_STARTED,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
#endif
}
/* 1) Setup index arrays. */
if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_HOTSTART_FAILED );
if ( bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_HOTSTART_FAILED );
/* 2) Initialize shift direction of the gradient and the bounds. */
returnvalue = hotstart_determineDataShift( FX_idx,
g_new,lb_new,ub_new,
delta_g,delta_lb,delta_ub,
Delta_bB_isZero
);
if ( returnvalue != SUCCESSFUL_RETURN )
{
nWSR = l;
THROWERROR( RET_SHIFT_DETERMINATION_FAILED );
return returnvalue;
}
/* 3) Determination of step direction of X and Y. */
returnvalue = hotstart_determineStepDirection( FR_idx,FX_idx,
delta_g,delta_lb,delta_ub,
Delta_bB_isZero,
delta_xFX,delta_xFR,delta_yFX
);
if ( returnvalue != SUCCESSFUL_RETURN )
{
nWSR = l;
THROWERROR( RET_STEPDIRECTION_DETERMINATION_FAILED );
return returnvalue;
}
/* 4) Determination of step length TAU. */
returnvalue = hotstart_determineStepLength( FR_idx,FX_idx,
delta_lb,delta_ub,
delta_xFR,delta_yFX,
BC_idx,BC_status );
if ( returnvalue != SUCCESSFUL_RETURN )
{
nWSR = l;
THROWERROR( RET_STEPLENGTH_DETERMINATION_FAILED );
return returnvalue;
}
/* 5) Realization of the homotopy step. */
returnvalue = hotstart_performStep( FR_idx,FX_idx,
delta_g,delta_lb,delta_ub,
delta_xFX,delta_xFR,delta_yFX,
BC_idx,BC_status
);
if ( returnvalue != SUCCESSFUL_RETURN )
{
nWSR = l;
/* stop runtime measurement */
if ( cputime != 0 )
*cputime = getCPUtime( ) - starttime;
/* optimal solution found? */
if ( returnvalue == RET_OPTIMAL_SOLUTION_FOUND )
{
status = QPS_SOLVED;
if ( printlevel == PL_HIGH )
THROWINFO( RET_OPTIMAL_SOLUTION_FOUND );
#ifdef PC_DEBUG
if ( printIteration( l,BC_idx,BC_status ) != SUCCESSFUL_RETURN )
THROWERROR( RET_PRINT_ITERATION_FAILED ); /* do not pass this as return value! */
#endif
/* check KKT optimality conditions */
return checkKKTconditions( );
}
else
{
/* checks for infeasibility... */
if ( infeasible == BT_TRUE )
{
status = QPS_HOMOTOPYQPSOLVED;
return THROWERROR( RET_HOTSTART_STOPPED_INFEASIBILITY );
}
/* ...unboundedness... */
if ( unbounded == BT_TRUE ) /* not necessary since objective function convex! */
return THROWERROR( RET_HOTSTART_STOPPED_UNBOUNDEDNESS );
/* ... and throw unspecific error otherwise */
THROWERROR( RET_HOMOTOPY_STEP_FAILED );
return returnvalue;
}
}
/* 6) Output information of successful QP iteration. */
status = QPS_HOMOTOPYQPSOLVED;
#ifdef PC_DEBUG
if ( printIteration( l,BC_idx,BC_status ) != SUCCESSFUL_RETURN )
THROWERROR( RET_PRINT_ITERATION_FAILED ); /* do not pass this as return value! */
#endif
}
/* stop runtime measurement */
if ( cputime != 0 )
*cputime = getCPUtime( ) - starttime;
/* if programm gets to here, output information that QP could not be solved
* within the given maximum numbers of working set changes */
if ( printlevel == PL_HIGH )
{
#ifdef PC_DEBUG
sprintf( messageString,"(nWSR = %d)",nWSR );
return getGlobalMessageHandler( )->throwWarning( RET_MAX_NWSR_REACHED,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
#endif
}
/* Finally check KKT optimality conditions. */
returnValue returnvalueKKTcheck = checkKKTconditions( );
if ( returnvalueKKTcheck != SUCCESSFUL_RETURN )
return returnvalueKKTcheck;
else
return RET_MAX_NWSR_REACHED;
}
/*
* g e t N Z
*/
int QProblemB::getNZ( )
{
/* if no constraints are present: nZ=nFR */
return bounds.getFree( )->getLength( );
}
/*
* g e t O b j V a l
*/
real_t QProblemB::getObjVal( ) const
{
real_t objVal;
/* calculated optimal objective function value
* only if current QP has been solved */
if ( ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
( getStatus( ) == QPS_HOMOTOPYQPSOLVED ) ||
( getStatus( ) == QPS_SOLVED ) )
{
objVal = getObjVal( x );
}
else
{
objVal = INFTY;
}
return objVal;
}
/*
* g e t O b j V a l
*/
real_t QProblemB::getObjVal( const real_t* const _x ) const
{
int i, j;
int nV = getNV( );
real_t obj_tmp = 0.0;
for( i=0; i<nV; ++i )
{
obj_tmp += _x[i]*g[i];
for( j=0; j<nV; ++j )
obj_tmp += 0.5*_x[i]*H[i*NVMAX + j]*_x[j];
}
return obj_tmp;
}
/*
* g e t P r i m a l S o l u t i o n
*/
returnValue QProblemB::getPrimalSolution( real_t* const xOpt ) const
{
int i;
/* return optimal primal solution vector
* only if current QP has been solved */
if ( ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
( getStatus( ) == QPS_HOMOTOPYQPSOLVED ) ||
( getStatus( ) == QPS_SOLVED ) )
{
for( i=0; i<getNV( ); ++i )
xOpt[i] = x[i];
return SUCCESSFUL_RETURN;
}
else
{
return RET_QP_NOT_SOLVED;
}
}
/*
* g e t D u a l S o l u t i o n
*/
returnValue QProblemB::getDualSolution( real_t* const yOpt ) const
{
int i;
/* return optimal dual solution vector
* only if current QP has been solved */
if ( ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
( getStatus( ) == QPS_HOMOTOPYQPSOLVED ) ||
( getStatus( ) == QPS_SOLVED ) )
{
for( i=0; i<getNV( ); ++i )
yOpt[i] = y[i];
return SUCCESSFUL_RETURN;
}
else
{
return RET_QP_NOT_SOLVED;
}
}
/*
* s e t P r i n t L e v e l
*/
returnValue QProblemB::setPrintLevel( PrintLevel _printlevel )
{
#ifndef __MATLAB__
if ( ( printlevel >= PL_MEDIUM ) && ( printlevel != _printlevel ) )
THROWINFO( RET_PRINTLEVEL_CHANGED );
#endif
printlevel = _printlevel;
/* update message handler preferences */
switch ( printlevel )
{
case PL_NONE:
getGlobalMessageHandler( )->setErrorVisibilityStatus( VS_HIDDEN );
getGlobalMessageHandler( )->setWarningVisibilityStatus( VS_HIDDEN );
getGlobalMessageHandler( )->setInfoVisibilityStatus( VS_HIDDEN );
break;
case PL_LOW:
getGlobalMessageHandler( )->setErrorVisibilityStatus( VS_VISIBLE );
getGlobalMessageHandler( )->setWarningVisibilityStatus( VS_HIDDEN );
getGlobalMessageHandler( )->setInfoVisibilityStatus( VS_HIDDEN );
break;
default: /* PL_MEDIUM, PL_HIGH */
getGlobalMessageHandler( )->setErrorVisibilityStatus( VS_VISIBLE );
getGlobalMessageHandler( )->setWarningVisibilityStatus( VS_VISIBLE );
getGlobalMessageHandler( )->setInfoVisibilityStatus( VS_VISIBLE );
break;
}
return SUCCESSFUL_RETURN;
}
/*****************************************************************************
* P R O T E C T E D *
*****************************************************************************/
/*
* c h e c k F o r I d e n t i t y H e s s i a n
*/
returnValue QProblemB::checkForIdentityHessian( )
{
int i, j;
int nV = getNV( );
/* nothing to do as status flag remains unaltered
* if Hessian differs from identity matrix */
if ( hessianType == HST_IDENTITY )
return SUCCESSFUL_RETURN;
/* 1) If Hessian differs from identity matrix,
* return without changing the internal HessianType. */
for ( i=0; i<nV; ++i )
if ( getAbs( H[i*NVMAX + i] - 1.0 ) > EPS )
return SUCCESSFUL_RETURN;
for ( i=0; i<nV; ++i )
{
for ( j=0; j<i; ++j )
if ( ( getAbs( H[i*NVMAX + j] ) > EPS ) || ( getAbs( H[j*NVMAX + i] ) > EPS ) )
return SUCCESSFUL_RETURN;
}
/* 2) If this point is reached, Hessian equals the idetity matrix. */
hessianType = HST_IDENTITY;
return SUCCESSFUL_RETURN;
}
/*
* s e t u p S u b j e c t T o T y p e
*/
returnValue QProblemB::setupSubjectToType( )
{
int i;
int nV = getNV( );
/* 1) Check if lower bounds are present. */
bounds.setNoLower( BT_TRUE );
for( i=0; i<nV; ++i )
if ( lb[i] > -INFTY )
{
bounds.setNoLower( BT_FALSE );
break;
}
/* 2) Check if upper bounds are present. */
bounds.setNoUpper( BT_TRUE );
for( i=0; i<nV; ++i )
if ( ub[i] < INFTY )
{
bounds.setNoUpper( BT_FALSE );
break;
}
/* 3) Determine implicitly fixed and unbounded variables. */
int nFV = 0;
int nUV = 0;
for( i=0; i<nV; ++i )
if ( ( lb[i] < -INFTY + BOUNDTOL ) && ( ub[i] > INFTY - BOUNDTOL ) )
{
bounds.setType( i,ST_UNBOUNDED );
++nUV;
}
else
{
if ( lb[i] > ub[i] - BOUNDTOL )
{
bounds.setType( i,ST_EQUALITY );
++nFV;
}
else
{
bounds.setType( i,ST_BOUNDED );
}
}
/* 4) Set dimensions of bounds structure. */
bounds.setNFV( nFV );
bounds.setNUV( nUV );
bounds.setNBV( nV - nFV - nUV );
return SUCCESSFUL_RETURN;
}
/*
* c h o l e s k y D e c o m p o s i t i o n
*/
returnValue QProblemB::setupCholeskyDecomposition( )
{
int i, j, k, ii, jj;
int nV = getNV( );
int nFR = getNFR( );
/* If Hessian flag is false, it means that H & R already contain Cholesky
* factorization -- provided from outside. */
if (hasHessian == BT_FALSE)
return SUCCESSFUL_RETURN;
/* 1) Initialises R with all zeros. */
for( i=0; i<nV; ++i )
for( j=0; j<nV; ++j )
R[i*NVMAX + j] = 0.0;
/* 2) Calculate Cholesky decomposition of H (projected to free variables). */
if ( hessianType == HST_IDENTITY )
{
/* if Hessian is identity, so is its Cholesky factor. */
for( i=0; i<nFR; ++i )
R[i*NVMAX + i] = 1.0;
}
else
{
if ( nFR > 0 )
{
int FR_idx[NVMAX];
if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_INDEXLIST_CORRUPTED );
/* R'*R = H */
real_t sum;
real_t inv;
for( i=0; i<nFR; ++i )
{
/* j == i */
ii = FR_idx[i];
sum = H[ii*NVMAX + ii];
for( k=(i-1); k>=0; --k )
sum -= R[k*NVMAX + i] * R[k*NVMAX + i];
if ( sum > 0.0 )
{
R[i*NVMAX + i] = sqrt( sum );
inv = 1.0 / R[i*NVMAX + i];
}
else
{
hessianType = HST_SEMIDEF;
return THROWERROR( RET_HESSIAN_NOT_SPD );
}
/* j > i */
for( j=(i+1); j<nFR; ++j )
{
jj = FR_idx[j];
sum = H[jj*NVMAX + ii];
for( k=(i-1); k>=0; --k )
sum -= R[k*NVMAX + i] * R[k*NVMAX + j];
R[i*NVMAX + j] = sum * inv;
}
}
}
}
return SUCCESSFUL_RETURN;
}
/*
* s o l v e I n i t i a l Q P
*/
returnValue QProblemB::solveInitialQP( const real_t* const xOpt, const real_t* const yOpt,
const Bounds* const guessedBounds,
int& nWSR, real_t* const cputime
)
{
int i, nFR;
int nV = getNV( );
/* start runtime measurement */
real_t starttime = 0.0;
if ( cputime != 0 )
starttime = getCPUtime( );
status = QPS_NOTINITIALISED;
/* I) ANALYSE QP DATA: */
/* 1) Check if Hessian happens to be the identity matrix. */
if ( checkForIdentityHessian( ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_INIT_FAILED );
/* 2) Setup type of bounds (i.e. unbounded, implicitly fixed etc.). */
if ( setupSubjectToType( ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_INIT_FAILED );
status = QPS_PREPARINGAUXILIARYQP;
/* II) SETUP AUXILIARY QP WITH GIVEN OPTIMAL SOLUTION: */
/* 1) Setup bounds data structure. */
if ( bounds.setupAllFree( ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_INIT_FAILED );
/* 2) Setup optimal primal/dual solution for auxiliary QP. */
if ( setupAuxiliaryQPsolution( xOpt,yOpt ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_INIT_FAILED );
/* 3) Obtain linear independent working set for auxiliary QP. */
static Bounds auxiliaryBounds;
auxiliaryBounds.init( nV );
if ( obtainAuxiliaryWorkingSet( xOpt,yOpt,guessedBounds, &auxiliaryBounds ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_INIT_FAILED );
/* 4) Setup working set of auxiliary QP and setup cholesky decomposition. */
if ( setupAuxiliaryWorkingSet( &auxiliaryBounds,BT_TRUE ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_INIT_FAILED );
nFR = getNFR();
/* At the moment we can only provide a Cholesky of the Hessian if
* the solver is cold-started. */
if (hasCholesky == BT_FALSE || nFR != nV)
if (setupCholeskyDecomposition() != SUCCESSFUL_RETURN)
return THROWERROR( RET_INIT_FAILED_CHOLESKY );
/* 5) Store original QP formulation... */
real_t g_original[NVMAX];
real_t lb_original[NVMAX];
real_t ub_original[NVMAX];
for( i=0; i<nV; ++i )
g_original[i] = g[i];
for( i=0; i<nV; ++i )
lb_original[i] = lb[i];
for( i=0; i<nV; ++i )
ub_original[i] = ub[i];
/* ... and setup QP data of an auxiliary QP having an optimal solution
* as specified by the user (or xOpt = yOpt = 0, by default). */
if ( setupAuxiliaryQPgradient( ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_INIT_FAILED );
if ( setupAuxiliaryQPbounds( BT_TRUE ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_INIT_FAILED );
status = QPS_AUXILIARYQPSOLVED;
/* III) SOLVE ACTUAL INITIAL QP: */
/* Use hotstart method to find the solution of the original initial QP,... */
returnValue returnvalue = hotstart( g_original,lb_original,ub_original, nWSR,0 );
/* ... check for infeasibility and unboundedness... */
if ( isInfeasible( ) == BT_TRUE )
return THROWERROR( RET_INIT_FAILED_INFEASIBILITY );
if ( isUnbounded( ) == BT_TRUE )
return THROWERROR( RET_INIT_FAILED_UNBOUNDEDNESS );
/* ... and internal errors. */
if ( ( returnvalue != SUCCESSFUL_RETURN ) && ( returnvalue != RET_MAX_NWSR_REACHED ) &&
( returnvalue != RET_INACCURATE_SOLUTION ) && ( returnvalue != RET_NO_SOLUTION ) )
return THROWERROR( RET_INIT_FAILED_HOTSTART );
/* stop runtime measurement */
if ( cputime != 0 )
*cputime = getCPUtime( ) - starttime;
if ( printlevel == PL_HIGH )
THROWINFO( RET_INIT_SUCCESSFUL );
return returnvalue;
}
/*
* o b t a i n A u x i l i a r y W o r k i n g S e t
*/
returnValue QProblemB::obtainAuxiliaryWorkingSet( const real_t* const xOpt, const real_t* const yOpt,
const Bounds* const guessedBounds, Bounds* auxiliaryBounds
) const
{
int i = 0;
int nV = getNV( );
/* 1) Ensure that desiredBounds is allocated (and different from guessedBounds). */
if ( ( auxiliaryBounds == 0 ) || ( auxiliaryBounds == guessedBounds ) )
return THROWERROR( RET_INVALID_ARGUMENTS );
/* 2) Setup working set for auxiliary initial QP. */
if ( guessedBounds != 0 )
{
/* If an initial working set is specific, use it!
* Moreover, add all implictly fixed variables if specified. */
for( i=0; i<nV; ++i )
{
if ( bounds.getType( i ) == ST_EQUALITY )
{
if ( auxiliaryBounds->setupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
}
else
{
if ( auxiliaryBounds->setupBound( i,guessedBounds->getStatus( i ) ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
}
}
}
else /* No initial working set specified. */
{
if ( ( xOpt != 0 ) && ( yOpt == 0 ) )
{
/* Obtain initial working set by "clipping". */
for( i=0; i<nV; ++i )
{
if ( xOpt[i] <= lb[i] + BOUNDTOL )
{
if ( auxiliaryBounds->setupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
continue;
}
if ( xOpt[i] >= ub[i] - BOUNDTOL )
{
if ( auxiliaryBounds->setupBound( i,ST_UPPER ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
continue;
}
/* Moreover, add all implictly fixed variables if specified. */
if ( bounds.getType( i ) == ST_EQUALITY )
{
if ( auxiliaryBounds->setupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
}
else
{
if ( auxiliaryBounds->setupBound( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
}
}
}
if ( ( xOpt == 0 ) && ( yOpt != 0 ) )
{
/* Obtain initial working set in accordance to sign of dual solution vector. */
for( i=0; i<nV; ++i )
{
if ( yOpt[i] > ZERO )
{
if ( auxiliaryBounds->setupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
continue;
}
if ( yOpt[i] < -ZERO )
{
if ( auxiliaryBounds->setupBound( i,ST_UPPER ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
continue;
}
/* Moreover, add all implictly fixed variables if specified. */
if ( bounds.getType( i ) == ST_EQUALITY )
{
if ( auxiliaryBounds->setupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
}
else
{
if ( auxiliaryBounds->setupBound( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
}
}
}
/* If xOpt and yOpt are null pointer and no initial working is specified,
* start with empty working set (or implicitly fixed bounds only)
* for auxiliary QP. */
if ( ( xOpt == 0 ) && ( yOpt == 0 ) )
{
for( i=0; i<nV; ++i )
{
/* Only add all implictly fixed variables if specified. */
if ( bounds.getType( i ) == ST_EQUALITY )
{
if ( auxiliaryBounds->setupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
}
else
{
if ( auxiliaryBounds->setupBound( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
}
}
}
}
return SUCCESSFUL_RETURN;
}
/*
* s e t u p A u x i l i a r y W o r k i n g S e t
*/
returnValue QProblemB::setupAuxiliaryWorkingSet( const Bounds* const auxiliaryBounds,
BooleanType setupAfresh
)
{
int i;
int nV = getNV( );
/* consistency checks */
if ( auxiliaryBounds != 0 )
{
for( i=0; i<nV; ++i )
if ( ( bounds.getStatus( i ) == ST_UNDEFINED ) || ( auxiliaryBounds->getStatus( i ) == ST_UNDEFINED ) )
return THROWERROR( RET_UNKNOWN_BUG );
}
else
{
return THROWERROR( RET_INVALID_ARGUMENTS );
}
/* I) SETUP CHOLESKY FLAG:
* Cholesky decomposition shall only be updated if working set
* shall be updated (i.e. NOT setup afresh!) */
BooleanType updateCholesky;
if ( setupAfresh == BT_TRUE )
updateCholesky = BT_FALSE;
else
updateCholesky = BT_TRUE;
/* II) REMOVE FORMERLY ACTIVE BOUNDS (IF NECESSARY): */
if ( setupAfresh == BT_FALSE )
{
/* Remove all active bounds that shall be inactive AND
* all active bounds that are active at the wrong bound. */
for( i=0; i<nV; ++i )
{
if ( ( bounds.getStatus( i ) == ST_LOWER ) && ( auxiliaryBounds->getStatus( i ) != ST_LOWER ) )
if ( removeBound( i,updateCholesky ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
if ( ( bounds.getStatus( i ) == ST_UPPER ) && ( auxiliaryBounds->getStatus( i ) != ST_UPPER ) )
if ( removeBound( i,updateCholesky ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
}
}
/* III) ADD NEWLY ACTIVE BOUNDS: */
/* Add all inactive bounds that shall be active AND
* all formerly active bounds that have been active at the wrong bound. */
for( i=0; i<nV; ++i )
{
if ( ( bounds.getStatus( i ) == ST_INACTIVE ) && ( auxiliaryBounds->getStatus( i ) != ST_INACTIVE ) )
{
if ( addBound( i,auxiliaryBounds->getStatus( i ),updateCholesky ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
}
}
return SUCCESSFUL_RETURN;
}
/*
* s e t u p A u x i l i a r y Q P s o l u t i o n
*/
returnValue QProblemB::setupAuxiliaryQPsolution( const real_t* const xOpt, const real_t* const yOpt
)
{
int i;
int nV = getNV( );
/* Setup primal/dual solution vectors for auxiliary initial QP:
* if a null pointer is passed, a zero vector is assigned;
* old solution vector is kept if pointer to internal solution vector is passed. */
if ( xOpt != 0 )
{
if ( xOpt != x )
for( i=0; i<nV; ++i )
x[i] = xOpt[i];
}
else
{
for( i=0; i<nV; ++i )
x[i] = 0.0;
}
if ( yOpt != 0 )
{
if ( yOpt != y )
for( i=0; i<nV; ++i )
y[i] = yOpt[i];
}
else
{
for( i=0; i<nV; ++i )
y[i] = 0.0;
}
return SUCCESSFUL_RETURN;
}
/*
* s e t u p A u x i l i a r y Q P g r a d i e n t
*/
returnValue QProblemB::setupAuxiliaryQPgradient( )
{
int i, j;
int nV = getNV( );
/* Setup gradient vector: g = -H*x + y'*Id. */
for ( i=0; i<nV; ++i )
{
/* y'*Id */
g[i] = y[i];
/* -H*x */
for ( j=0; j<nV; ++j )
g[i] -= H[i*NVMAX + j] * x[j];
}
return SUCCESSFUL_RETURN;
}
/*
* s e t u p A u x i l i a r y Q P b o u n d s
*/
returnValue QProblemB::setupAuxiliaryQPbounds( BooleanType useRelaxation )
{
int i;
int nV = getNV( );
/* Setup bound vectors. */
for ( i=0; i<nV; ++i )
{
switch ( bounds.getStatus( i ) )
{
case ST_INACTIVE:
if ( useRelaxation == BT_TRUE )
{
if ( bounds.getType( i ) == ST_EQUALITY )
{
lb[i] = x[i];
ub[i] = x[i];
}
else
{
lb[i] = x[i] - BOUNDRELAXATION;
ub[i] = x[i] + BOUNDRELAXATION;
}
}
break;
case ST_LOWER:
lb[i] = x[i];
if ( bounds.getType( i ) == ST_EQUALITY )
{
ub[i] = x[i];
}
else
{
if ( useRelaxation == BT_TRUE )
ub[i] = x[i] + BOUNDRELAXATION;
}
break;
case ST_UPPER:
ub[i] = x[i];
if ( bounds.getType( i ) == ST_EQUALITY )
{
lb[i] = x[i];
}
else
{
if ( useRelaxation == BT_TRUE )
lb[i] = x[i] - BOUNDRELAXATION;
}
break;
default:
return THROWERROR( RET_UNKNOWN_BUG );
}
}
return SUCCESSFUL_RETURN;
}
/*
* a d d B o u n d
*/
returnValue QProblemB::addBound( int number, SubjectToStatus B_status,
BooleanType updateCholesky
)
{
int i, j;
int nFR = getNFR( );
/* consistency check */
if ( ( getStatus( ) == QPS_NOTINITIALISED ) ||
( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
( getStatus( ) == QPS_HOMOTOPYQPSOLVED ) ||
( getStatus( ) == QPS_SOLVED ) )
{
return THROWERROR( RET_UNKNOWN_BUG );
}
/* Perform cholesky updates only if QProblemB has been initialised! */
if ( ( getStatus( ) == QPS_PREPARINGAUXILIARYQP ) || ( updateCholesky == BT_FALSE ) )
{
/* UPDATE INDICES */
if ( bounds.moveFreeToFixed( number,B_status ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_ADDBOUND_FAILED );
return SUCCESSFUL_RETURN;
}
/* I) PERFORM CHOLESKY UPDATE: */
/* 1) Index of variable to be added within the list of free variables. */
int number_idx = bounds.getFree( )->getIndex( number );
real_t c, s;
/* 2) Use row-wise Givens rotations to restore upper triangular form of R. */
for( i=number_idx+1; i<nFR; ++i )
{
computeGivens( R[(i-1)*NVMAX + i],R[i*NVMAX + i], R[(i-1)*NVMAX + i],R[i*NVMAX + i],c,s );
for( j=(1+i); j<nFR; ++j ) /* last column of R is thrown away */
applyGivens( c,s,R[(i-1)*NVMAX + j],R[i*NVMAX + j], R[(i-1)*NVMAX + j],R[i*NVMAX + j] );
}
/* 3) Delete <number_idx>th column and ... */
for( i=0; i<nFR-1; ++i )
for( j=number_idx+1; j<nFR; ++j )
R[i*NVMAX + j-1] = R[i*NVMAX + j];
/* ... last column of R. */
for( i=0; i<nFR; ++i )
R[i*NVMAX + nFR-1] = 0.0;
/* II) UPDATE INDICES */
if ( bounds.moveFreeToFixed( number,B_status ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_ADDBOUND_FAILED );
return SUCCESSFUL_RETURN;
}
returnValue QProblemB::removeBound( int number,
BooleanType updateCholesky
)
{
int i, ii;
int nFR = getNFR( );
/* consistency check */
if ( ( getStatus( ) == QPS_NOTINITIALISED ) ||
( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
( getStatus( ) == QPS_HOMOTOPYQPSOLVED ) ||
( getStatus( ) == QPS_SOLVED ) )
{
return THROWERROR( RET_UNKNOWN_BUG );
}
/* I) UPDATE INDICES */
if ( bounds.moveFixedToFree( number ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_REMOVEBOUND_FAILED );
/* Perform cholesky updates only if QProblemB has been initialised! */
if ( ( getStatus( ) == QPS_PREPARINGAUXILIARYQP ) || ( updateCholesky == BT_FALSE ) )
return SUCCESSFUL_RETURN;
/* II) PERFORM CHOLESKY UPDATE */
int FR_idx[NVMAX];
if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_REMOVEBOUND_FAILED );
/* 1) Calculate new column of cholesky decomposition. */
real_t rhs[NVMAX];
real_t r[NVMAX];
real_t r0 = H[number*NVMAX + number];
for( i=0; i<nFR; ++i )
{
ii = FR_idx[i];
rhs[i] = H[number*NVMAX + ii];
}
if ( backsolveR( rhs,BT_TRUE,BT_TRUE,r ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_REMOVEBOUND_FAILED );
for( i=0; i<nFR; ++i )
r0 -= r[i]*r[i];
/* 2) Store new column into R. */
for( i=0; i<nFR; ++i )
R[i*NVMAX + nFR] = r[i];
if ( r0 > 0.0 )
R[nFR*NVMAX + nFR] = sqrt( r0 );
else
{
hessianType = HST_SEMIDEF;
return THROWERROR( RET_HESSIAN_NOT_SPD );
}
return SUCCESSFUL_RETURN;
}
/*
* b a c k s o l v e R (CODE DUPLICATED IN QProblem CLASS!!!)
*/
returnValue QProblemB::backsolveR( const real_t* const b, BooleanType transposed,
real_t* const a
)
{
/* Call standard backsolve procedure (i.e. removingBound == BT_FALSE). */
return backsolveR( b,transposed,BT_FALSE,a );
}
/*
* b a c k s o l v e R (CODE DUPLICATED IN QProblem CLASS!!!)
*/
returnValue QProblemB::backsolveR( const real_t* const b, BooleanType transposed,
BooleanType removingBound,
real_t* const a
)
{
int i, j;
int nR = getNZ( );
real_t sum;
/* if backsolve is called while removing a bound, reduce nZ by one. */
if ( removingBound == BT_TRUE )
--nR;
/* nothing to do */
if ( nR <= 0 )
return SUCCESSFUL_RETURN;
/* Solve Ra = b, where R might be transposed. */
if ( transposed == BT_FALSE )
{
/* solve Ra = b */
for( i=(nR-1); i>=0; --i )
{
sum = b[i];
for( j=(i+1); j<nR; ++j )
sum -= R[i*NVMAX + j] * a[j];
if ( getAbs( R[i*NVMAX + i] ) > ZERO )
a[i] = sum / R[i*NVMAX + i];
else
return THROWERROR( RET_DIV_BY_ZERO );
}
}
else
{
/* solve R^T*a = b */
for( i=0; i<nR; ++i )
{
sum = b[i];
for( j=0; j<i; ++j )
sum -= R[j*NVMAX + i] * a[j];
if ( getAbs( R[i*NVMAX + i] ) > ZERO )
a[i] = sum / R[i*NVMAX + i];
else
return THROWERROR( RET_DIV_BY_ZERO );
}
}
return SUCCESSFUL_RETURN;
}
/*
* h o t s t a r t _ d e t e r m i n e D a t a S h i f t
*/
returnValue QProblemB::hotstart_determineDataShift( const int* const FX_idx,
const real_t* const g_new, const real_t* const lb_new, const real_t* const ub_new,
real_t* const delta_g, real_t* const delta_lb, real_t* const delta_ub,
BooleanType& Delta_bB_isZero
)
{
int i, ii;
int nV = getNV( );
int nFX = getNFX( );
/* 1) Calculate shift directions. */
for( i=0; i<nV; ++i )
delta_g[i] = g_new[i] - g[i];
if ( lb_new != 0 )
{
for( i=0; i<nV; ++i )
delta_lb[i] = lb_new[i] - lb[i];
}
else
{
/* if no lower bounds exist, assume the new lower bounds to be -infinity */
for( i=0; i<nV; ++i )
delta_lb[i] = -INFTY - lb[i];
}
if ( ub_new != 0 )
{
for( i=0; i<nV; ++i )
delta_ub[i] = ub_new[i] - ub[i];
}
else
{
/* if no upper bounds exist, assume the new upper bounds to be infinity */
for( i=0; i<nV; ++i )
delta_ub[i] = INFTY - ub[i];
}
/* 2) Determine if active bounds are to be shifted. */
Delta_bB_isZero = BT_TRUE;
for ( i=0; i<nFX; ++i )
{
ii = FX_idx[i];
if ( ( getAbs( delta_lb[ii] ) > EPS ) || ( getAbs( delta_ub[ii] ) > EPS ) )
{
Delta_bB_isZero = BT_FALSE;
break;
}
}
return SUCCESSFUL_RETURN;
}
/*
* a r e B o u n d s C o n s i s t e n t
*/
BooleanType QProblemB::areBoundsConsistent( const real_t* const delta_lb, const real_t* const delta_ub
) const
{
int i;
/* Check if delta_lb[i] is greater than delta_ub[i]
* for a component i whose bounds are already (numerically) equal. */
for( i=0; i<getNV( ); ++i )
if ( ( lb[i] > ub[i] - BOUNDTOL ) && ( delta_lb[i] > delta_ub[i] + EPS ) )
return BT_FALSE;
return BT_TRUE;
}
/*
* s e t u p Q P d a t a
*/
returnValue QProblemB::setupQPdata( const real_t* const _H, const real_t* const _R, const real_t* const _g,
const real_t* const _lb, const real_t* const _ub
)
{
int i, j;
int nV = getNV( );
/* 1) Setup Hessian matrix and it's Cholesky factorization. */
if (_H != 0)
{
for( i=0; i<nV; ++i )
for( j=0; j<nV; ++j )
H[i*NVMAX + j] = _H[i*nV + j];
hasHessian = BT_TRUE;
}
else
hasHessian = BT_FALSE;
if (_R != 0)
{
for( i=0; i<nV; ++i )
for( j=0; j<nV; ++j )
R[i*NVMAX + j] = _R[i*nV + j];
hasCholesky = BT_TRUE;
/* If Hessian is not provided, store it's factorization in H, and that guy
* is going to be used for H * x products (R^T * R * x in this case). */
if (hasHessian == BT_FALSE)
for( i=0; i<nV; ++i )
for( j=0; j<nV; ++j )
H[i*NVMAX + j] = _R[i*nV + j];
}
else
hasCholesky = BT_FALSE;
if (hasHessian == BT_FALSE && hasCholesky == BT_FALSE)
return THROWERROR( RET_INVALID_ARGUMENTS );
/* 2) Setup gradient vector. */
if ( _g == 0 )
return THROWERROR( RET_INVALID_ARGUMENTS );
for( i=0; i<nV; ++i )
g[i] = _g[i];
/* 3) Setup lower bounds vector. */
if ( _lb != 0 )
{
for( i=0; i<nV; ++i )
lb[i] = _lb[i];
}
else
{
/* if no lower bounds are specified, set them to -infinity */
for( i=0; i<nV; ++i )
lb[i] = -INFTY;
}
/* 4) Setup upper bounds vector. */
if ( _ub != 0 )
{
for( i=0; i<nV; ++i )
ub[i] = _ub[i];
}
else
{
/* if no upper bounds are specified, set them to infinity */
for( i=0; i<nV; ++i )
ub[i] = INFTY;
}
//printmatrix( "H",H,nV,nV );
//printmatrix( "R",R,nV,nV );
//printmatrix( "g",g,1,nV );
//printmatrix( "lb",lb,1,nV );
//printmatrix( "ub",ub,1,nV );
return SUCCESSFUL_RETURN;
}
/*****************************************************************************
* P R I V A T E *
*****************************************************************************/
/*
* h o t s t a r t _ d e t e r m i n e S t e p D i r e c t i o n
*/
returnValue QProblemB::hotstart_determineStepDirection( const int* const FR_idx, const int* const FX_idx,
const real_t* const delta_g, const real_t* const delta_lb, const real_t* const delta_ub,
BooleanType Delta_bB_isZero,
real_t* const delta_xFX, real_t* const delta_xFR,
real_t* const delta_yFX
)
{
int i, j, ii, jj;
int nFR = getNFR( );
int nFX = getNFX( );
/* initialise auxiliary vectors */
real_t HMX_delta_xFX[NVMAX];
for( i=0; i<nFR; ++i )
HMX_delta_xFX[i] = 0.0;
/* I) DETERMINE delta_xFX */
if ( nFX > 0 )
{
for( i=0; i<nFX; ++i )
{
ii = FX_idx[i];
if ( bounds.getStatus( ii ) == ST_LOWER )
delta_xFX[i] = delta_lb[ii];
else
delta_xFX[i] = delta_ub[ii];
}
}
/* II) DETERMINE delta_xFR */
if ( nFR > 0 )
{
/* auxiliary variables */
real_t delta_xFRz_TMP[NVMAX];
real_t delta_xFRz_RHS[NVMAX];
/* Determine delta_xFRz. */
if ( Delta_bB_isZero == BT_FALSE )
{
for( i=0; i<nFR; ++i )
{
ii = FR_idx[i];
for( j=0; j<nFX; ++j )
{
jj = FX_idx[j];
HMX_delta_xFX[i] += H[ii*NVMAX + jj] * delta_xFX[j];
}
}
}
if ( Delta_bB_isZero == BT_TRUE )
{
for( j=0; j<nFR; ++j )
{
jj = FR_idx[j];
delta_xFRz_RHS[j] = delta_g[jj];
}
}
else
{
for( j=0; j<nFR; ++j )
{
jj = FR_idx[j];
delta_xFRz_RHS[j] = delta_g[jj] + HMX_delta_xFX[j]; /* *ZFR */
}
}
for( i=0; i<nFR; ++i )
delta_xFR[i] = -delta_xFRz_RHS[i];
if ( backsolveR( delta_xFR,BT_TRUE,delta_xFRz_TMP ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_STEPDIRECTION_FAILED_CHOLESKY );
if ( backsolveR( delta_xFRz_TMP,BT_FALSE,delta_xFR ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_STEPDIRECTION_FAILED_CHOLESKY );
}
/* III) DETERMINE delta_yFX */
if ( nFX > 0 )
{
for( i=0; i<nFX; ++i )
{
ii = FX_idx[i];
delta_yFX[i] = 0.0;
for( j=0; j<nFR; ++j )
{
jj = FR_idx[j];
delta_yFX[i] += H[ii*NVMAX + jj] * delta_xFR[j];
}
if ( Delta_bB_isZero == BT_FALSE )
{
for( j=0; j<nFX; ++j )
{
jj = FX_idx[j];
delta_yFX[i] += H[ii*NVMAX + jj] * delta_xFX[j];
}
}
delta_yFX[i] += delta_g[ii];
}
}
return SUCCESSFUL_RETURN;
}
/*
* h o t s t a r t _ d e t e r m i n e S t e p L e n g t h
*/
returnValue QProblemB::hotstart_determineStepLength( const int* const FR_idx, const int* const FX_idx,
const real_t* const delta_lb, const real_t* const delta_ub,
const real_t* const delta_xFR,
const real_t* const delta_yFX,
int& BC_idx, SubjectToStatus& BC_status
)
{
int i, ii;
int nFR = getNFR( );
int nFX = getNFX( );
real_t tau_tmp;
real_t tau_new = 1.0;
BC_idx = 0;
BC_status = ST_UNDEFINED;
/* I) DETERMINE MAXIMUM DUAL STEPLENGTH, i.e. ensure that
* active dual bounds remain valid (ignoring implicitly fixed variables): */
for( i=0; i<nFX; ++i )
{
ii = FX_idx[i];
if ( bounds.getType( ii ) != ST_EQUALITY )
{
if ( bounds.getStatus( ii ) == ST_LOWER )
{
/* 1) Active lower bounds. */
if ( ( delta_yFX[i] < -ZERO ) && ( y[ii] >= 0.0 ) )
{
tau_tmp = y[ii] / ( -delta_yFX[i] );
if ( tau_tmp < tau_new )
{
if ( tau_tmp >= 0.0 )
{
tau_new = tau_tmp;
BC_idx = ii;
BC_status = ST_INACTIVE;
}
}
}
}
else
{
/* 2) Active upper bounds. */
if ( ( delta_yFX[i] > ZERO ) && ( y[ii] <= 0.0 ) )
{
tau_tmp = y[ii] / ( -delta_yFX[i] );
if ( tau_tmp < tau_new )
{
if ( tau_tmp >= 0.0 )
{
tau_new = tau_tmp;
BC_idx = ii;
BC_status = ST_INACTIVE;
}
}
}
}
}
}
/* II) DETERMINE MAXIMUM PRIMAL STEPLENGTH, i.e. ensure that
* inactive bounds remain valid (ignoring unbounded variables). */
/* 1) Inactive lower bounds. */
if ( bounds.isNoLower( ) == BT_FALSE )
{
for( i=0; i<nFR; ++i )
{
ii = FR_idx[i];
if ( bounds.getType( ii ) != ST_UNBOUNDED )
{
if ( delta_lb[ii] > delta_xFR[i] )
{
if ( x[ii] > lb[ii] )
tau_tmp = ( x[ii] - lb[ii] ) / ( delta_lb[ii] - delta_xFR[i] );
else
tau_tmp = 0.0;
if ( tau_tmp < tau_new )
{
if ( tau_tmp >= 0.0 )
{
tau_new = tau_tmp;
BC_idx = ii;
BC_status = ST_LOWER;
}
}
}
}
}
}
/* 2) Inactive upper bounds. */
if ( bounds.isNoUpper( ) == BT_FALSE )
{
for( i=0; i<nFR; ++i )
{
ii = FR_idx[i];
if ( bounds.getType( ii ) != ST_UNBOUNDED )
{
if ( delta_ub[ii] < delta_xFR[i] )
{
if ( x[ii] < ub[ii] )
tau_tmp = ( x[ii] - ub[ii] ) / ( delta_ub[ii] - delta_xFR[i] );
else
tau_tmp = 0.0;
if ( tau_tmp < tau_new )
{
if ( tau_tmp >= 0.0 )
{
tau_new = tau_tmp;
BC_idx = ii;
BC_status = ST_UPPER;
}
}
}
}
}
}
/* III) SET MAXIMUM HOMOTOPY STEPLENGTH */
tau = tau_new;
if ( printlevel == PL_HIGH )
{
#ifdef PC_DEBUG
char messageString[80];
if ( BC_status == ST_UNDEFINED )
sprintf( messageString,"Stepsize is %.6e!",tau );
else
sprintf( messageString,"Stepsize is %.6e! (BC_idx = %d, BC_status = %d)",tau,BC_idx,BC_status );
getGlobalMessageHandler( )->throwInfo( RET_STEPSIZE_NONPOSITIVE,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
#endif
}
return SUCCESSFUL_RETURN;
}
/*
* h o t s t a r t _ p e r f o r m S t e p
*/
returnValue QProblemB::hotstart_performStep( const int* const FR_idx, const int* const FX_idx,
const real_t* const delta_g, const real_t* const delta_lb, const real_t* const delta_ub,
const real_t* const delta_xFX, const real_t* const delta_xFR,
const real_t* const delta_yFX,
int BC_idx, SubjectToStatus BC_status
)
{
int i, ii;
int nV = getNV( );
int nFR = getNFR( );
int nFX = getNFX( );
/* I) CHECK BOUNDS' CONSISTENCY */
if ( areBoundsConsistent( delta_lb,delta_ub ) == BT_FALSE )
{
infeasible = BT_TRUE;
tau = 0.0;
return THROWERROR( RET_QP_INFEASIBLE );
}
/* II) GO TO ACTIVE SET CHANGE */
if ( tau > ZERO )
{
/* 1) Perform step in primal und dual space. */
for( i=0; i<nFR; ++i )
{
ii = FR_idx[i];
x[ii] += tau*delta_xFR[i];
}
for( i=0; i<nFX; ++i )
{
ii = FX_idx[i];
x[ii] += tau*delta_xFX[i];
y[ii] += tau*delta_yFX[i];
}
/* 2) Shift QP data. */
for( i=0; i<nV; ++i )
{
g[i] += tau*delta_g[i];
lb[i] += tau*delta_lb[i];
ub[i] += tau*delta_ub[i];
}
}
else
{
/* print a stepsize warning if stepsize is zero */
#ifdef PC_DEBUG
char messageString[80];
sprintf( messageString,"Stepsize is %.6e",tau );
getGlobalMessageHandler( )->throwWarning( RET_STEPSIZE,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
#endif
}
/* setup output preferences */
#ifdef PC_DEBUG
char messageString[80];
VisibilityStatus visibilityStatus;
if ( printlevel == PL_HIGH )
visibilityStatus = VS_VISIBLE;
else
visibilityStatus = VS_HIDDEN;
#endif
/* III) UPDATE ACTIVE SET */
switch ( BC_status )
{
/* Optimal solution found as no working set change detected. */
case ST_UNDEFINED:
return RET_OPTIMAL_SOLUTION_FOUND;
/* Remove one variable from active set. */
case ST_INACTIVE:
#ifdef PC_DEBUG
sprintf( messageString,"bound no. %d.", BC_idx );
getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus );
#endif
if ( removeBound( BC_idx,BT_TRUE ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );
y[BC_idx] = 0.0;
break;
/* Add one variable to active set. */
default:
#ifdef PC_DEBUG
if ( BC_status == ST_LOWER )
sprintf( messageString,"lower bound no. %d.", BC_idx );
else
sprintf( messageString,"upper bound no. %d.", BC_idx );
getGlobalMessageHandler( )->throwInfo( RET_ADD_TO_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus );
#endif
if ( addBound( BC_idx,BC_status,BT_TRUE ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_ADD_TO_ACTIVESET_FAILED );
break;
}
return SUCCESSFUL_RETURN;
}
#ifdef PC_DEBUG /* Define print functions only for debugging! */
/*
* p r i n t I t e r a t i o n
*/
returnValue QProblemB::printIteration( int iteration,
int BC_idx, SubjectToStatus BC_status
)
{
char myPrintfString[160];
/* consistency check */
if ( iteration < 0 )
return THROWERROR( RET_INVALID_ARGUMENTS );
/* nothing to do */
if ( printlevel != PL_MEDIUM )
return SUCCESSFUL_RETURN;
/* 1) Print header at first iteration. */
if ( iteration == 0 )
{
sprintf( myPrintfString,"\n############## qpOASES -- QP NO.%4.1d ###############\n", count );
myPrintf( myPrintfString );
sprintf( myPrintfString," Iter | StepLength | Info | nFX \n" );
myPrintf( myPrintfString );
}
/* 2) Print iteration line. */
if ( BC_status == ST_UNDEFINED )
{
sprintf( myPrintfString," %4.1d | %1.5e | QP SOLVED | %4.1d \n", iteration,tau,getNFX( ) );
myPrintf( myPrintfString );
}
else
{
char info[8];
if ( BC_status == ST_INACTIVE )
sprintf( info,"REM BND" );
else
sprintf( info,"ADD BND" );
sprintf( myPrintfString," %4.1d | %1.5e | %s%4.1d | %4.1d \n", iteration,tau,info,BC_idx,getNFX( ) );
myPrintf( myPrintfString );
}
return SUCCESSFUL_RETURN;
}
#endif /* PC_DEBUG */
/*
* c h e c k K K T c o n d i t i o n s
*/
returnValue QProblemB::checkKKTconditions( )
{
#ifdef __PERFORM_KKT_TEST__
int i, j;
int nV = getNV( );
real_t tmp;
real_t maxKKTviolation = 0.0;
/* 1) Check for Hx + g - y*A' = 0 (here: A = Id). */
for( i=0; i<nV; ++i )
{
tmp = g[i];
for( j=0; j<nV; ++j )
tmp += H[i*nV + j] * x[j];
tmp -= y[i];
if ( getAbs( tmp ) > maxKKTviolation )
maxKKTviolation = getAbs( tmp );
}
/* 2) Check for lb <= x <= ub. */
for( i=0; i<nV; ++i )
{
if ( lb[i] - x[i] > maxKKTviolation )
maxKKTviolation = lb[i] - x[i];
if ( x[i] - ub[i] > maxKKTviolation )
maxKKTviolation = x[i] - ub[i];
}
/* 3) Check for correct sign of y and for complementary slackness. */
for( i=0; i<nV; ++i )
{
switch ( bounds.getStatus( i ) )
{
case ST_LOWER:
if ( -y[i] > maxKKTviolation )
maxKKTviolation = -y[i];
if ( getAbs( ( x[i] - lb[i] ) * y[i] ) > maxKKTviolation )
maxKKTviolation = getAbs( ( x[i] - lb[i] ) * y[i] );
break;
case ST_UPPER:
if ( y[i] > maxKKTviolation )
maxKKTviolation = y[i];
if ( getAbs( ( ub[i] - x[i] ) * y[i] ) > maxKKTviolation )
maxKKTviolation = getAbs( ( ub[i] - x[i] ) * y[i] );
break;
default: /* inactive */
if ( getAbs( y[i] ) > maxKKTviolation )
maxKKTviolation = getAbs( y[i] );
break;
}
}
if ( maxKKTviolation > CRITICALACCURACY )
return RET_NO_SOLUTION;
if ( maxKKTviolation > DESIREDACCURACY )
return RET_INACCURATE_SOLUTION;
#endif /* __PERFORM_KKT_TEST__ */
return SUCCESSFUL_RETURN;
}
/*
* end of file
*/