sadmen
/
jyoung
mirror of https://github.com/jyoung8607/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
open source driving agent
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.
1699 Commits
101 Branches
34 Tags
1.6 GiB
Tag: Branch: Tree: afdb4ce61e
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from 'afdb4ce61e'
${ noResults }
jyoung/phonelibs/qpoases/SRC/QProblemB.cpp

2152 lines
48 KiB
Raw Normal View History Unescape

bring over phonelibs minus frida-gum and qsml
5 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
*/