37 #include <qpOASES/extras/SolutionAnalysis.hpp> 96 int nV = qp->
getNV( );
99 maxKKTviolation = 0.0;
103 for( i=0; i<nV; ++i )
113 for( i=0; i<nV; ++i )
118 qp->
H->times(1, 1.0, qp->
x, nV, 1.0, tmp, nV);
122 for( i=0; i<nV; ++i )
126 if ( fabs( tmp[i] ) > maxKKTviolation )
127 maxKKTviolation = fabs( tmp[i] );
132 for( i=0; i<nV; ++i )
134 if ( qp->
lb[i] - qp->
x[i] > maxKKTviolation )
135 maxKKTviolation = qp->
lb[i] - qp->
x[i];
137 if ( qp->
x[i] - qp->
ub[i] > maxKKTviolation )
138 maxKKTviolation = qp->
x[i] - qp->
ub[i];
142 for( i=0; i<nV; ++i )
147 if ( -qp->
y[i] > maxKKTviolation )
148 maxKKTviolation = -qp->
y[i];
149 if ( fabs( ( qp->
x[i] - qp->
lb[i] ) * qp->
y[i] ) > maxKKTviolation )
150 maxKKTviolation = fabs( ( qp->
x[i] - qp->
lb[i] ) * qp->
y[i] );
154 if ( qp->
y[i] > maxKKTviolation )
155 maxKKTviolation = qp->
y[i];
156 if ( fabs( ( qp->
ub[i] - qp->
x[i] ) * qp->
y[i] ) > maxKKTviolation )
157 maxKKTviolation = fabs( ( qp->
ub[i] - qp->
x[i] ) * qp->
y[i] );
161 if ( fabs( qp->
y[i] ) > maxKKTviolation )
162 maxKKTviolation = fabs( qp->
y[i] );
178 int nV = qp->
getNV( );
179 int nC = qp->
getNC( );
182 maxKKTviolation = 0.0;
186 for( i=0; i<nV; ++i )
196 for( i=0; i<nV; ++i )
201 qp->
H->
times(1, 1.0, qp->
x, nV, 1.0, tmp, nV);
205 qp->
A->transTimes(1, -1.0, qp->
y + nV, nC, 1.0, tmp, nV);
207 for( i=0; i<nV; ++i )
211 if ( fabs( tmp[i] ) > maxKKTviolation )
212 maxKKTviolation = fabs( tmp[i] );
218 qp->
A->times(1, 1.0, qp->
x, nV, 0.0, Ax, nC);
220 for( i=0; i<nC; ++i )
222 if ( qp->
lbA[i] - Ax[i] > maxKKTviolation )
223 maxKKTviolation = qp->
lbA[i] - Ax[i];
225 if ( Ax[i] - qp->
ubA[i] > maxKKTviolation )
226 maxKKTviolation = Ax[i] - qp->
ubA[i];
230 for( i=0; i<nV; ++i )
232 if ( qp->
lb[i] - qp->
x[i] > maxKKTviolation )
233 maxKKTviolation = qp->
lb[i] - qp->
x[i];
235 if ( qp->
x[i] - qp->
ub[i] > maxKKTviolation )
236 maxKKTviolation = qp->
x[i] - qp->
ub[i];
241 for( i=0; i<nV; ++i )
246 if ( -qp->
y[i] > maxKKTviolation )
247 maxKKTviolation = -qp->
y[i];
248 if ( fabs( ( qp->
x[i] - qp->
lb[i] ) * qp->
y[i] ) > maxKKTviolation )
249 maxKKTviolation = fabs( ( qp->
x[i] - qp->
lb[i] ) * qp->
y[i] );
253 if ( qp->
y[i] > maxKKTviolation )
254 maxKKTviolation = qp->
y[i];
255 if ( fabs( ( qp->
ub[i] - qp->
x[i] ) * qp->
y[i] ) > maxKKTviolation )
256 maxKKTviolation = fabs( ( qp->
ub[i] - qp->
x[i] ) * qp->
y[i] );
260 if ( fabs( qp->
y[i] ) > maxKKTviolation )
261 maxKKTviolation = fabs( qp->
y[i] );
267 for( i=0; i<nC; ++i )
272 if ( -qp->
y[nV+i] > maxKKTviolation )
273 maxKKTviolation = -qp->
y[nV+i];
274 if ( fabs( ( Ax[i] - qp->
lbA[i] ) * qp->
y[nV+i] ) > maxKKTviolation )
275 maxKKTviolation = fabs( ( Ax[i] - qp->
lbA[i] ) * qp->
y[nV+i] );
279 if ( qp->
y[nV+i] > maxKKTviolation )
280 maxKKTviolation = qp->
y[nV+i];
281 if ( fabs( ( qp->
ubA[i] - Ax[i] ) * qp->
y[nV+i] ) > maxKKTviolation )
282 maxKKTviolation = fabs( ( qp->
ubA[i] - Ax[i] ) * qp->
y[nV+i] );
286 if ( fabs( qp->
y[nV+i] ) > maxKKTviolation )
287 maxKKTviolation = fabs( qp->
y[nV+i] );
326 int nV = qp->
getNV( );
327 int nC = qp->
getNC( );
330 int run1, run2, run3;
387 for( run1 = 0; run1 < dim*dim; run1++ ){
389 Primal_Dual_VAR[run1] = 0.0;
402 for( run3 = 0; run3 < dim; run3++ ){
405 for( run1 = 0; run1 < nV; run1++ ){
406 delta_g_cov [run1] = g_b_bA_VAR[run3*dim+run1];
407 delta_lb_cov [run1] = g_b_bA_VAR[run3*dim+nV+run1];
408 delta_ub_cov [run1] = g_b_bA_VAR[run3*dim+nV+run1];
410 for( run1 = 0; run1 < nC; run1++ ){
411 delta_lbA_cov [run1] = g_b_bA_VAR[run3*dim+2*nV+run1];
412 delta_ubA_cov [run1] = g_b_bA_VAR[run3*dim+2*nV+run1];
419 returnvalue = qp->
determineStepDirection( delta_g_cov, delta_lbA_cov, delta_ubA_cov, delta_lb_cov, delta_ub_cov,
420 Delta_bC_isZero, Delta_bB_isZero, delta_xFX,delta_xFR,
421 delta_yAC,delta_yFX );
447 for( run1=0; run1<nFR; run1++ ){
449 K[run3*dim+run2] = delta_xFR[run1];
451 for( run1=0; run1<nFX; run1++ ){
453 K[run3*dim+run2] = delta_xFX[run1];
454 K[run3*dim+nV+run2] = delta_yFX[run1];
456 for( run1=0; run1<nAC; run1++ ){
458 K[run3*dim+2*nV+run2] = delta_yAC[run1];
472 for( run3 = 0; run3 < dim; run3++ ){
474 for( run1 = 0; run1 < nV; run1++ ){
475 delta_g_cov [run1] = K[run3+ run1*dim];
476 delta_lb_cov [run1] = K[run3+(nV+run1)*dim];
477 delta_ub_cov [run1] = K[run3+(nV+run1)*dim];
479 for( run1 = 0; run1 < nC; run1++ ){
480 delta_lbA_cov [run1] = K[run3+(2*nV+run1)*dim];
481 delta_ubA_cov [run1] = K[run3+(2*nV+run1)*dim];
488 returnvalue = qp->
determineStepDirection( delta_g_cov, delta_lbA_cov, delta_ubA_cov, delta_lb_cov, delta_ub_cov,
489 Delta_bC_isZero, Delta_bB_isZero, delta_xFX,delta_xFR,
490 delta_yAC,delta_yFX);
517 for( run1=0; run1<nFR; run1++ ){
519 Primal_Dual_VAR[run3+run2*dim] = delta_xFR[run1];
521 for( run1=0; run1<nFX; run1++ ){
523 Primal_Dual_VAR[run3+run2*dim ] = delta_xFX[run1];
524 Primal_Dual_VAR[run3+(nV+run2)*dim] = delta_yFX[run1];
526 for( run1=0; run1<nAC; run1++ ){
528 Primal_Dual_VAR[run3+(2*nV+run2)*dim] = delta_yAC[run1];
HessianType getHessianType() const
Implements the online active set strategy for box-constrained QPs.
returnValue getNumberArray(int *const numberarray) const
SolutionAnalysis & operator=(const SolutionAnalysis &rhs)
Implements the online active set strategy for QPs with varying matrices.
Allows to pass back messages to the calling function.
#define THROWERROR(retval)
real_t delta_ubA_cov[NCMAX_ALLOC]
SubjectToStatus getStatus(int i) const
real_t delta_lbA_cov[NCMAX_ALLOC]
real_t A[NCMAX_ALLOC *NVMAX]
Provides additional tools for analysing QP solutions.
returnValue getMaxKKTviolation(QProblemB *qp, real_t &maxKKTviolation) const
real_t delta_ub_cov[NVMAX]
returnValue determineStepDirection(const real_t *const delta_g, const real_t *const delta_lbA, const real_t *const delta_ubA, const real_t *const delta_lb, const real_t *const delta_ub, BooleanType Delta_bC_isZero, BooleanType Delta_bB_isZero, real_t *const delta_xFX, real_t *const delta_xFR, real_t *const delta_yAC, real_t *const delta_yFX)
real_t delta_g_cov[NVMAX]
void rhs(const real_t *x, real_t *f)
#define END_NAMESPACE_QPOASES
real_t delta_lb_cov[NVMAX]
returnValue getVarianceCovariance(QProblemB *qp, real_t *g_b_bA_VAR, real_t *Primal_Dual_VAR) const
returnValue times(int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD) const
#define BEGIN_NAMESPACE_QPOASES
Implements the online active set strategy for QPs with general constraints.