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.
2151 lines
50 KiB
2151 lines
50 KiB
/*
|
|
* 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
|
|
*/
|
|
|