35 #include <QProblemB.hpp> 42 printf(
"%s = [...\n", name);
43 for (i = 0; i < m; i++) {
44 for (j = 0; j < n; j++)
45 printf(
" % 9.4f", A[i*n+j]);
132 for( i=0; i<_nV; ++i )
133 for( j=0; j<_nV; ++j )
136 for( i=0; i<_nV; ++i )
139 for( i=0; i<_nV; ++i )
142 for( i=0; i<_nV; ++i )
148 for( i=0; i<_nV; ++i )
149 for( j=0; j<_nV; ++j )
152 for( i=0; i<_nV; ++i )
155 for( i=0; i<_nV; ++i )
191 for( i=0; i<_nV; ++i )
192 for( j=0; j<_nV; ++j )
195 for( i=0; i<_nV; ++i )
198 for( i=0; i<_nV; ++i )
201 for( i=0; i<_nV; ++i )
206 for( i=0; i<_nV; ++i )
207 for( j=0; j<_nV; ++j )
211 for( i=0; i<_nV; ++i )
214 for( i=0; i<_nV; ++i )
248 for( i=0; i<nV; ++i )
249 for( j=0; j<nV; ++j )
331 char messageString[80];
335 for( l=0; l<
nWSR; ++l )
342 sprintf( messageString,
"%d ...",l );
357 delta_g,delta_lb,delta_ub,
369 delta_g,delta_lb,delta_ub,
371 delta_xFX,delta_xFR,delta_yFX
395 delta_g,delta_lb,delta_ub,
396 delta_xFX,delta_xFR,delta_yFX,
464 sprintf( messageString,
"(nWSR = %d)",nWSR );
473 return returnvalueKKTcheck;
523 for( i=0; i<nV; ++i )
525 obj_tmp += _x[i]*
g[i];
527 for( j=0; j<nV; ++j )
528 obj_tmp += 0.5*_x[i]*
H[i*
NVMAX + j]*_x[j];
548 for( i=0; i<
getNV( ); ++i )
573 for( i=0; i<
getNV( ); ++i )
643 for ( i=0; i<nV; ++i )
647 for ( i=0; i<nV; ++i )
649 for ( j=0; j<i; ++j )
672 for( i=0; i<nV; ++i )
681 for( i=0; i<nV; ++i )
692 for( i=0; i<nV; ++i )
730 for( i=0; i<nV; ++i )
731 for( j=0; j<nV; ++j )
738 for( i=0; i<nFR; ++i )
752 for( i=0; i<nFR; ++i )
758 for( k=(i-1); k>=0; --k )
770 for( j=(i+1); j<nFR; ++j )
775 for( k=(i-1); k>=0; --k )
792 const Bounds*
const guessedBounds,
831 static Bounds auxiliaryBounds;
833 auxiliaryBounds.
init( nV );
850 for( i=0; i<nV; ++i )
852 g_original[i] =
g[i];
853 lb_original[i] =
lb[i];
854 ub_original[i] =
ub[i];
901 const Bounds*
const guessedBounds,
Bounds* auxiliaryBounds
909 if ( ( auxiliaryBounds == 0 ) || ( auxiliaryBounds == guessedBounds ) )
914 if ( guessedBounds != 0 )
918 for( i=0; i<nV; ++i )
934 if ( ( xOpt != 0 ) && ( yOpt == 0 ) )
937 for( i=0; i<nV; ++i )
967 if ( ( xOpt == 0 ) && ( yOpt != 0 ) )
970 for( i=0; i<nV; ++i )
972 if ( yOpt[i] >
ZERO )
979 if ( yOpt[i] < -
ZERO )
1003 if ( ( xOpt == 0 ) && ( yOpt == 0 ) )
1005 for( i=0; i<nV; ++i )
1037 if ( auxiliaryBounds != 0 )
1039 for( i=0; i<nV; ++i )
1064 for( i=0; i<nV; ++i )
1080 for( i=0; i<nV; ++i )
1109 for( i=0; i<nV; ++i )
1114 for( i=0; i<nV; ++i )
1121 for( i=0; i<nV; ++i )
1126 for( i=0; i<nV; ++i )
1144 for ( i=0; i<nV; ++i )
1150 for ( j=0; j<nV; ++j )
1168 for ( i=0; i<nV; ++i )
1173 if ( useRelaxation ==
BT_TRUE )
1196 if ( useRelaxation ==
BT_TRUE )
1209 if ( useRelaxation ==
BT_TRUE )
1261 for( i=number_idx+1; i<nFR; ++i )
1265 for( j=(1+i); j<nFR; ++j )
1270 for( i=0; i<nFR-1; ++i )
1271 for( j=number_idx+1; j<nFR; ++j )
1274 for( i=0; i<nFR; ++i )
1275 R[i*
NVMAX + nFR-1] = 0.0;
1324 for( i=0; i<nFR; ++i )
1327 rhs[i] =
H[number*
NVMAX + ii];
1333 for( i=0; i<nFR; ++i )
1337 for( i=0; i<nFR; ++i )
1379 if ( removingBound ==
BT_TRUE )
1391 for( i=(nR-1); i>=0; --i )
1394 for( j=(i+1); j<nR; ++j )
1395 sum -=
R[i*
NVMAX + j] * a[j];
1398 a[i] = sum /
R[i*
NVMAX + i];
1406 for( i=0; i<nR; ++i )
1410 for( j=0; j<i; ++j )
1411 sum -=
R[j*
NVMAX + i] * a[j];
1414 a[i] = sum /
R[i*
NVMAX + i];
1439 for( i=0; i<nV; ++i )
1440 delta_g[i] = g_new[i] -
g[i];
1444 for( i=0; i<nV; ++i )
1445 delta_lb[i] = lb_new[i] -
lb[i];
1450 for( i=0; i<nV; ++i )
1456 for( i=0; i<nV; ++i )
1457 delta_ub[i] = ub_new[i] -
ub[i];
1462 for( i=0; i<nV; ++i )
1469 for ( i=0; i<nFX; ++i )
1494 for( i=0; i<
getNV( ); ++i )
1495 if ( (
lb[i] >
ub[i] -
BOUNDTOL ) && ( delta_lb[i] > delta_ub[i] +
EPS ) )
1516 for( i=0; i<nV; ++i )
1517 for( j=0; j<nV; ++j )
1518 H[i*
NVMAX + j] = _H[i*nV + j];
1524 for( i=0; i<nV; ++i )
1530 for( i=0; i<nV; ++i )
1536 for( i=0; i<nV; ++i )
1543 for( i=0; i<nV; ++i )
1549 for( i=0; i<nV; ++i )
1571 const real_t*
const delta_g,
const real_t*
const delta_lb,
const real_t*
const delta_ub,
1584 for( i=0; i<nFR; ++i )
1585 HMX_delta_xFX[i] = 0.0;
1591 for( i=0; i<nFX; ++i )
1596 delta_xFX[i] = delta_lb[ii];
1598 delta_xFX[i] = delta_ub[ii];
1613 for( i=0; i<nFR; ++i )
1616 for( j=0; j<nFX; ++j )
1619 HMX_delta_xFX[i] +=
H[ii*
NVMAX + jj] * delta_xFX[j];
1624 if ( Delta_bB_isZero ==
BT_TRUE )
1626 for( j=0; j<nFR; ++j )
1629 delta_xFRz_RHS[j] = delta_g[jj];
1634 for( j=0; j<nFR; ++j )
1637 delta_xFRz_RHS[j] = delta_g[jj] + HMX_delta_xFX[j];
1641 for( i=0; i<nFR; ++i )
1642 delta_xFR[i] = -delta_xFRz_RHS[i];
1655 for( i=0; i<nFX; ++i )
1660 for( j=0; j<nFR; ++j )
1663 delta_yFX[i] +=
H[ii*
NVMAX + jj] * delta_xFR[j];
1668 for( j=0; j<nFX; ++j )
1671 delta_yFX[i] +=
H[ii*
NVMAX + jj] * delta_xFX[j];
1675 delta_yFX[i] += delta_g[ii];
1687 const real_t*
const delta_lb,
const real_t*
const delta_ub,
1706 for( i=0; i<nFX; ++i )
1715 if ( ( delta_yFX[i] < -
ZERO ) && (
y[ii] >= 0.0 ) )
1717 tau_tmp =
y[ii] / ( -delta_yFX[i] );
1718 if ( tau_tmp < tau_new )
1720 if ( tau_tmp >= 0.0 )
1732 if ( ( delta_yFX[i] >
ZERO ) && (
y[ii] <= 0.0 ) )
1734 tau_tmp =
y[ii] / ( -delta_yFX[i] );
1735 if ( tau_tmp < tau_new )
1737 if ( tau_tmp >= 0.0 )
1755 for( i=0; i<nFR; ++i )
1761 if ( delta_lb[ii] > delta_xFR[i] )
1763 if (
x[ii] >
lb[ii] )
1764 tau_tmp = (
x[ii] -
lb[ii] ) / ( delta_lb[ii] - delta_xFR[i] );
1768 if ( tau_tmp < tau_new )
1770 if ( tau_tmp >= 0.0 )
1785 for( i=0; i<nFR; ++i )
1791 if ( delta_ub[ii] < delta_xFR[i] )
1793 if (
x[ii] <
ub[ii] )
1794 tau_tmp = (
x[ii] -
ub[ii] ) / ( delta_ub[ii] - delta_xFR[i] );
1798 if ( tau_tmp < tau_new )
1800 if ( tau_tmp >= 0.0 )
1819 char messageString[80];
1822 sprintf( messageString,
"Stepsize is %.6e!",
tau );
1824 sprintf( messageString,
"Stepsize is %.6e! (BC_idx = %d, BC_status = %d)",
tau,BC_idx,BC_status );
1838 const real_t*
const delta_g,
const real_t*
const delta_lb,
const real_t*
const delta_ub,
1864 for( i=0; i<nFR; ++i )
1867 x[ii] +=
tau*delta_xFR[i];
1870 for( i=0; i<nFX; ++i )
1873 x[ii] +=
tau*delta_xFX[i];
1874 y[ii] +=
tau*delta_yFX[i];
1878 for( i=0; i<nV; ++i )
1880 g[i] +=
tau*delta_g[i];
1881 lb[i] +=
tau*delta_lb[i];
1882 ub[i] +=
tau*delta_ub[i];
1889 char messageString[80];
1890 sprintf( messageString,
"Stepsize is %.6e",
tau );
1898 char messageString[80];
1909 switch ( BC_status )
1919 sprintf( messageString,
"bound no. %d.", BC_idx );
1934 sprintf( messageString,
"lower bound no. %d.", BC_idx );
1936 sprintf( messageString,
"upper bound no. %d.", BC_idx );
1958 char myPrintfString[160];
1961 if ( iteration < 0 )
1970 if ( iteration == 0 )
1972 sprintf( myPrintfString,
"\n############## qpOASES -- QP NO.%4.1d ###############\n",
count );
1975 sprintf( myPrintfString,
" Iter | StepLength | Info | nFX \n" );
1982 sprintf( myPrintfString,
" %4.1d | %1.5e | QP SOLVED | %4.1d \n", iteration,
tau,
getNFX( ) );
1990 sprintf( info,
"REM BND" );
1992 sprintf( info,
"ADD BND" );
1994 sprintf( myPrintfString,
" %4.1d | %1.5e | %s%4.1d | %4.1d \n", iteration,
tau,info,BC_idx,
getNFX( ) );
2010 #ifdef __PERFORM_KKT_TEST__ 2016 real_t maxKKTviolation = 0.0;
2020 for( i=0; i<nV; ++i )
2024 for( j=0; j<nV; ++j )
2025 tmp +=
H[i*nV + j] *
x[j];
2029 if (
getAbs( tmp ) > maxKKTviolation )
2030 maxKKTviolation =
getAbs( tmp );
2034 for( i=0; i<nV; ++i )
2036 if (
lb[i] -
x[i] > maxKKTviolation )
2037 maxKKTviolation =
lb[i] -
x[i];
2039 if ( x[i] -
ub[i] > maxKKTviolation )
2040 maxKKTviolation = x[i] -
ub[i];
2044 for( i=0; i<nV; ++i )
2049 if ( -
y[i] > maxKKTviolation )
2050 maxKKTviolation = -
y[i];
2051 if (
getAbs( (
x[i] -
lb[i] ) *
y[i] ) > maxKKTviolation )
2052 maxKKTviolation =
getAbs( (
x[i] -
lb[i] ) *
y[i] );
2056 if (
y[i] > maxKKTviolation )
2057 maxKKTviolation =
y[i];
2058 if (
getAbs( (
ub[i] -
x[i] ) *
y[i] ) > maxKKTviolation )
2059 maxKKTviolation =
getAbs( (
ub[i] -
x[i] ) *
y[i] );
2063 if (
getAbs(
y[i] ) > maxKKTviolation )
2064 maxKKTviolation =
getAbs(
y[i] );
returnValue getPrimalSolution(real_t *const xOpt) const
returnValue setupCholeskyDecomposition()
void setNoLower(BooleanType _status)
returnValue setType(int i, SubjectToType value)
IntermediateState sqrt(const Expression &arg)
BooleanType isInfeasible() const
BooleanType isNoLower() const
Implements the online active set strategy for box-constrained QPs.
returnValue getNumberArray(int *const numberarray) const
returnValue moveFixedToFree(int _number)
BooleanType isUnbounded() const
returnValue checkForIdentityHessian()
void computeGivens(real_t xold, real_t yold, real_t &xnew, real_t &ynew, real_t &c, real_t &s) const
BEGIN_NAMESPACE_ACADO const double EPS
#define THROWINFO(retval)
returnValue obtainAuxiliaryWorkingSet(const real_t *const xOpt, const real_t *const yOpt, const Bounds *const guessedBounds, Bounds *auxiliaryBounds) const
returnValue throwInfo(returnValue Inumber, const char *additionaltext, const char *functionname, const char *filename, const unsigned long linenumber, VisibilityStatus localVisibilityStatus)
const real_t CRITICALACCURACY
Allows to pass back messages to the calling function.
returnValue setupAuxiliaryWorkingSet(const Bounds *const auxiliaryBounds, BooleanType setupAfresh)
#define THROWERROR(retval)
returnValue setupAuxiliaryQPsolution(const real_t *const xOpt, const real_t *const yOpt)
returnValue backsolveR(const real_t *const b, BooleanType transposed, real_t *const a)
returnValue 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=0, real_t *const cputime=0)
returnValue throwWarning(returnValue Wnumber, const char *additionaltext, const char *functionname, const char *filename, const unsigned long linenumber, VisibilityStatus localVisibilityStatus)
returnValue setupAuxiliaryQPgradient()
returnValue myPrintf(const char *s)
returnValue setupBound(int _number, SubjectToStatus _status)
returnValue setNUV(int n)
returnValue setNBV(int n)
SubjectToStatus getStatus(int i) const
returnValue 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)
returnValue getDualSolution(real_t *const yOpt) const
returnValue addBound(int number, SubjectToStatus B_status, BooleanType updateCholesky)
returnValue 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)
returnValue setupQPdata(const real_t *const _H, const real_t *const _g, const real_t *const _lb, const real_t *const _ub)
returnValue setNFV(int n)
returnValue setupSubjectToType()
returnValue setupAllFree()
void setInfoVisibilityStatus(VisibilityStatus _infoVisibility)
returnValue printIteration(int iteration, int BC_idx, SubjectToStatus BC_status)
BooleanType areBoundsConsistent(const real_t *const delta_lb, const real_t *const delta_ub) const
returnValue 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)
returnValue setupAuxiliaryQPbounds(BooleanType useRelaxation)
void rhs(const real_t *x, real_t *f)
void printmatrix(char *name, double *A, int m, int n)
SubjectToType getType(int i) const
BooleanType isNoUpper() const
#define HST_POSDEF_NULLSPACE
QProblemStatus getStatus() const
void setErrorVisibilityStatus(VisibilityStatus _errorVisibility)
const real_t DESIREDACCURACY
returnValue 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)
returnValue removeBound(int number, BooleanType updateCholesky)
void setNoUpper(BooleanType _status)
QProblemB & operator=(const QProblemB &rhs)
returnValue checkKKTconditions()
int getIndex(int givennumber) const
void setWarningVisibilityStatus(VisibilityStatus _warningVisibility)
Manages working sets of bounds (= box constraints).
returnValue 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)
void applyGivens(real_t c, real_t s, real_t xold, real_t yold, real_t &xnew, real_t &ynew) const
MessageHandling * getGlobalMessageHandler()
returnValue solveInitialQP(const real_t *const xOpt, const real_t *const yOpt, const Bounds *const guessedBounds, int &nWSR, real_t *const cputime)
const double BOUNDRELAXATION
returnValue moveFreeToFixed(int _number, SubjectToStatus _status)
returnValue setPrintLevel(PrintLevel _printlevel)