external_packages/qpOASES-3.0beta/src/extras/SolutionAnalysis.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of qpOASES.
3  *
4  * qpOASES -- An Implementation of the Online Active Set Strategy.
5  * Copyright (C) 2007-2011 by Hans Joachim Ferreau, Andreas Potschka,
6  * Christian Kirches et al. All rights reserved.
7  *
8  * qpOASES is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU Lesser General Public
10  * License as published by the Free Software Foundation; either
11  * version 2.1 of the License, or (at your option) any later version.
12  *
13  * qpOASES is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
16  * See the GNU Lesser General Public License for more details.
17  *
18  * You should have received a copy of the GNU Lesser General Public
19  * License along with qpOASES; if not, write to the Free Software
20  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
21  *
22  */
23 
24 
37 #include <qpOASES/extras/SolutionAnalysis.hpp>
38 
39 
41 
42 
43 /*****************************************************************************
44  * P U B L I C *
45  *****************************************************************************/
46 
47 
48 /*
49  * S o l u t i o n A n a l y s i s
50  */
52 {
53 
54 }
55 
56 
57 /*
58  * S o l u t i o n A n a l y s i s
59  */
61 {
62 
63 }
64 
65 
66 /*
67  * ~ S o l u t i o n A n a l y s i s
68  */
70 {
71 
72 }
73 
74 
75 /*
76  * o p e r a t o r =
77  */
79 {
80  if ( this != &rhs )
81  {
82 
83  }
84 
85  return *this;
86 }
87 
88 
89 
90 /*
91  * g e t M a x K K T v i o l a t i o n
92  */
94 {
95  int i;
96  int nV = qp->getNV( );
97 
98  real_t *tmp = new real_t[nV];
99  maxKKTviolation = 0.0;
100 
101 
102  /* 1) Check for Hx + g - y*A' = 0 (here: A = Id). */
103  for( i=0; i<nV; ++i )
104  tmp[i] = qp->g[i];
105 
106  switch ( qp->getHessianType( ) )
107  {
108  case HST_ZERO:
109  /*tmp += qp->eps * qp->x[i]; */
110  break;
111 
112  case HST_IDENTITY:
113  for( i=0; i<nV; ++i )
114  tmp[i] += qp->x[i];
115  break;
116 
117  default:
118  qp->H->times(1, 1.0, qp->x, nV, 1.0, tmp, nV);
119  break;
120  }
121 
122  for( i=0; i<nV; ++i )
123  {
124  tmp[i] -= qp->y[i];
125 
126  if ( fabs( tmp[i] ) > maxKKTviolation )
127  maxKKTviolation = fabs( tmp[i] );
128  }
129  delete[] tmp;
130 
131  /* 2) Check for lb <= x <= ub. */
132  for( i=0; i<nV; ++i )
133  {
134  if ( qp->lb[i] - qp->x[i] > maxKKTviolation )
135  maxKKTviolation = qp->lb[i] - qp->x[i];
136 
137  if ( qp->x[i] - qp->ub[i] > maxKKTviolation )
138  maxKKTviolation = qp->x[i] - qp->ub[i];
139  }
140 
141  /* 3) Check for correct sign of y and for complementary slackness. */
142  for( i=0; i<nV; ++i )
143  {
144  switch ( qp->bounds.getStatus( i ) )
145  {
146  case ST_LOWER:
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] );
151  break;
152 
153  case ST_UPPER:
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] );
158  break;
159 
160  default: /* inactive */
161  if ( fabs( qp->y[i] ) > maxKKTviolation )
162  maxKKTviolation = fabs( qp->y[i] );
163  break;
164  }
165  }
166 
167 
168  return SUCCESSFUL_RETURN;
169 }
170 
171 
172 /*
173  * g e t M a x K K T v i o l a t i o n
174  */
176 {
177  int i;
178  int nV = qp->getNV( );
179  int nC = qp->getNC( );
180 
181  real_t *tmp = new real_t[nV];
182  maxKKTviolation = 0.0;
183 
184 
185  /* 1) check for Hx + g - [yFX yAC]*[Id A]' = 0. */
186  for( i=0; i<nV; ++i )
187  tmp[i] = qp->g[i];
188 
189  switch ( qp->getHessianType( ) )
190  {
191  case HST_ZERO:
192  /*tmp += qp->eps * qp->x[i]; */
193  break;
194 
195  case HST_IDENTITY:
196  for( i=0; i<nV; ++i )
197  tmp[i] += qp->x[i];
198  break;
199 
200  default:
201  qp->H->times(1, 1.0, qp->x, nV, 1.0, tmp, nV);
202  break;
203  }
204 
205  qp->A->transTimes(1, -1.0, qp->y + nV, nC, 1.0, tmp, nV);
206 
207  for( i=0; i<nV; ++i )
208  {
209  tmp[i] -= qp->y[i];
210 
211  if ( fabs( tmp[i] ) > maxKKTviolation )
212  maxKKTviolation = fabs( tmp[i] );
213  }
214 
215  /* 2) Check for [lb lbA] <= [Id A]*x <= [ub ubA]. */
216  /* lbA <= Ax <= ubA */
217  real_t* Ax = new real_t[nC];
218  qp->A->times(1, 1.0, qp->x, nV, 0.0, Ax, nC);
219 
220  for( i=0; i<nC; ++i )
221  {
222  if ( qp->lbA[i] - Ax[i] > maxKKTviolation )
223  maxKKTviolation = qp->lbA[i] - Ax[i];
224 
225  if ( Ax[i] - qp->ubA[i] > maxKKTviolation )
226  maxKKTviolation = Ax[i] - qp->ubA[i];
227  }
228 
229  /* lb <= x <= ub */
230  for( i=0; i<nV; ++i )
231  {
232  if ( qp->lb[i] - qp->x[i] > maxKKTviolation )
233  maxKKTviolation = qp->lb[i] - qp->x[i];
234 
235  if ( qp->x[i] - qp->ub[i] > maxKKTviolation )
236  maxKKTviolation = qp->x[i] - qp->ub[i];
237  }
238 
239  /* 3) Check for correct sign of y and for complementary slackness. */
240  /* bounds */
241  for( i=0; i<nV; ++i )
242  {
243  switch ( qp->bounds.getStatus( i ) )
244  {
245  case ST_LOWER:
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] );
250  break;
251 
252  case ST_UPPER:
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] );
257  break;
258 
259  default: /* inactive */
260  if ( fabs( qp->y[i] ) > maxKKTviolation )
261  maxKKTviolation = fabs( qp->y[i] );
262  break;
263  }
264  }
265 
266  /* constraints */
267  for( i=0; i<nC; ++i )
268  {
269  switch ( qp->constraints.getStatus( i ) )
270  {
271  case ST_LOWER:
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] );
276  break;
277 
278  case ST_UPPER:
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] );
283  break;
284 
285  default: /* inactive */
286  if ( fabs( qp->y[nV+i] ) > maxKKTviolation )
287  maxKKTviolation = fabs( qp->y[nV+i] );
288  break;
289  }
290  }
291 
292  delete[] tmp;
293  delete[] Ax;
294 
295  return SUCCESSFUL_RETURN;
296 }
297 
298 
299 /*
300  * g e t M a x K K T v i o l a t i o n
301  */
303 {
304  return getMaxKKTviolation( (QProblem*)qp,maxKKTviolation );
305 }
306 
307 
308 
309 /*
310  * g e t V a r i a n c e C o v a r i a n c e
311  */
313 {
315 }
316 
317 
318 /*
319  * g e t V a r i a n c e C o v a r i a n c e
320  */
322 {
323 
324  /* DEFINITION OF THE DIMENSIONS nV AND nC:
325  * --------------------------------------- */
326  int nV = qp->getNV( ); /* dimension of x / the bounds */
327  int nC = qp->getNC( ); /* dimension of the constraints */
328  int dim = 2*nV+nC; /* dimension of input and output */
329  /* variance-covariance matrix */
330  int run1, run2, run3; /* simple run variables (for loops). */
331 
332 
333  /* ALLOCATION OF MEMORY:
334  * --------------------- */
335  real_t* delta_g_cov = new real_t[nV]; /* a covariance-vector of g */
336  real_t* delta_lb_cov = new real_t[nV]; /* a covariance-vector of lb */
337  real_t* delta_ub_cov = new real_t[nV]; /* a covariance-vector of ub */
338  real_t* delta_lbA_cov = new real_t[nC]; /* a covariance-vector of lbA */
339  real_t* delta_ubA_cov = new real_t[nC]; /* a covariance-vector of ubA */
340 
341  returnValue returnvalue; /* the return value */
342  BooleanType Delta_bC_isZero = BT_FALSE; /* (just use FALSE here) */
343  BooleanType Delta_bB_isZero = BT_FALSE; /* (just use FALSE here) */
344 
345 
346 
347  /* ASK FOR THE NUMBER OF FREE AND FIXED VARIABLES:
348  * (ASSUMES THAT ACTIVE SET IS CONSTANT FOR THE
349  * VARIANCE-COVARIANCE EVALUATION)
350  * ----------------------------------------------- */
351  int nFR, nFX, nAC;
352 
353  nFR = qp->getNFR( );
354  nFX = qp->getNFX( );
355  nAC = qp->getNAC( );
356 
357 
358  /* ASK FOR THE CORRESPONDING INDEX ARRAYS:
359  * --------------------------------------- */
360  int *FR_idx, *FX_idx, *AC_idx;
361 
362  if ( qp->bounds.getFree( )->getNumberArray( &FR_idx ) != SUCCESSFUL_RETURN )
364 
365  if ( qp->bounds.getFixed( )->getNumberArray( &FX_idx ) != SUCCESSFUL_RETURN )
367 
368  if ( qp->constraints.getActive( )->getNumberArray( &AC_idx ) != SUCCESSFUL_RETURN )
370 
371 
372 
373  /* INTRODUCE VARIABLES TO MEASURE THE REACTION OF THE QP-SOLUTION TO
374  * THE VARIANCE-COVARIANCE DISTURBANCE:
375  * ----------------------------------------------------------------- */
376  real_t *delta_xFR = new real_t[nFR];
377  real_t *delta_xFX = new real_t[nFX];
378  real_t *delta_yAC = new real_t[nAC];
379  real_t *delta_yFX = new real_t[nFX];
380 
381  real_t* K = new real_t[dim*dim]; /* matrix to store */
382  /* an intermediate */
383  /* result. */
384 
385  /* SOME INITIALIZATIONS:
386  * --------------------- */
387  for( run1 = 0; run1 < dim*dim; run1++ ){
388  K [run1] = 0.0;
389  Primal_Dual_VAR[run1] = 0.0;
390  }
391 
392 
393  /* ================================================================= */
394 
395  /* FIRST MATRIX MULTIPLICATION (OBTAINS THE INTERMEDIATE RESULT
396  * K := [ ("ACTIVE" KKT-MATRIX OF THE QP)^(-1) * g_b_bA_VAR ]^T )
397  * THE EVALUATION OF THE INVERSE OF THE KKT-MATRIX OF THE QP
398  * WITH RESPECT TO THE CURRENT ACTIVE SET
399  * USES THE EXISTING CHOLESKY AND TQ-DECOMPOSITIONS. FOR DETAILS
400  * cf. THE (protected) FUNCTION determineStepDirection. */
401 
402  for( run3 = 0; run3 < dim; run3++ ){
403 
404 
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]; /* LINE-WISE LOADING OF THE INPUT */
408  delta_ub_cov [run1] = g_b_bA_VAR[run3*dim+nV+run1]; /* VARIANCE-COVARIANCE */
409  }
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];
413  }
414 
415 
416  /* EVALUATION OF THE STEP:
417  * ------------------------------------------------------------------------------ */
418 
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 );
422 
423  /* ------------------------------------------------------------------------------ */
424 
425 
426  /* STOP THE ALGORITHM IN THE CASE OF NO SUCCESFUL RETURN:
427  * ------------------------------------------------------ */
428  if ( returnvalue != SUCCESSFUL_RETURN ){
429 
430  delete[] delta_g_cov;
431  delete[] delta_lb_cov;
432  delete[] delta_ub_cov;
433  delete[] delta_lbA_cov;
434  delete[] delta_ubA_cov;
435  delete[] delta_xFR;
436  delete[] delta_xFX;
437  delete[] delta_yAC;
438  delete[] delta_yFX;
439  delete[] K;
440 
442  return returnvalue;
443  }
444 
445 
446 
447  for( run1=0; run1<nFR; run1++ ){
448  run2 = FR_idx[run1];
449  K[run3*dim+run2] = delta_xFR[run1];
450  } /* LINE WISE */
451  for( run1=0; run1<nFX; run1++ ){ /* STORAGE OF THE QP-REACTION */
452  run2 = FX_idx[run1]; /* (uses the index list) */
453  K[run3*dim+run2] = delta_xFX[run1];
454  K[run3*dim+nV+run2] = delta_yFX[run1];
455  }
456  for( run1=0; run1<nAC; run1++ ){
457  run2 = AC_idx[run1];
458  K[run3*dim+2*nV+run2] = delta_yAC[run1];
459  }
460 
461  }
462 
463 
464  /* ================================================================= */
465 
466  /* SECOND MATRIX MULTIPLICATION (OBTAINS THE FINAL RESULT
467  * Primal_Dual_VAR := ("ACTIVE" KKT-MATRIX OF THE QP)^(-1) * K )
468  * THE APPLICATION OF THE KKT-INVERSE IS AGAIN REALIZED
469  * BY USING THE PROTECTED FUNCTION
470  * determineStepDirection */
471 
472  for( run3 = 0; run3 < dim; run3++ ){
473 
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]; /* ROW WISE LOADING OF THE */
477  delta_ub_cov [run1] = K[run3+(nV+run1)*dim]; /* INTERMEDIATE RESULT K */
478  }
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];
482  }
483 
484 
485  /* EVALUATION OF THE STEP:
486  * ------------------------------------------------------------------------------ */
487 
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);
491 
492 
493  /* ------------------------------------------------------------------------------ */
494 
495 
496  /* STOP THE ALGORITHM IN THE CASE OF NO SUCCESFUL RETURN:
497  * ------------------------------------------------------ */
498  if ( returnvalue != SUCCESSFUL_RETURN ){
499 
500  delete[] delta_g_cov;
501  delete[] delta_lb_cov;
502  delete[] delta_ub_cov;
503  delete[] delta_lbA_cov;
504  delete[] delta_ubA_cov;
505  delete[] delta_xFR;
506  delete[] delta_xFX;
507  delete[] delta_yAC;
508  delete[] delta_yFX;
509  delete[] K;
510 
512  return returnvalue;
513  }
514 
515 
516 
517  for( run1=0; run1<nFR; run1++ ){
518  run2 = FR_idx[run1];
519  Primal_Dual_VAR[run3+run2*dim] = delta_xFR[run1];
520  }
521  for( run1=0; run1<nFX; run1++ ){ /* ROW-WISE STORAGE */
522  run2 = FX_idx[run1]; /* OF THE RESULT. */
523  Primal_Dual_VAR[run3+run2*dim ] = delta_xFX[run1];
524  Primal_Dual_VAR[run3+(nV+run2)*dim] = delta_yFX[run1];
525  }
526  for( run1=0; run1<nAC; run1++ ){
527  run2 = AC_idx[run1];
528  Primal_Dual_VAR[run3+(2*nV+run2)*dim] = delta_yAC[run1];
529  }
530 
531  }
532 
533 
534  /* DEALOCATE MEMORY:
535  * ----------------- */
536 
537  delete[] delta_g_cov;
538  delete[] delta_lb_cov;
539  delete[] delta_ub_cov;
540  delete[] delta_lbA_cov;
541  delete[] delta_ubA_cov;
542  delete[] delta_xFR;
543  delete[] delta_xFX;
544  delete[] delta_yAC;
545  delete[] delta_yFX;
546  delete[] K;
547 
548  return SUCCESSFUL_RETURN;
549 }
550 
551 
552 /*
553  * g e t V a r i a n c e C o v a r i a n c e
554  */
556 {
557  /* Call QProblem variant. */
558  return getVarianceCovariance( (QProblem*)qp,g_b_bA_VAR,Primal_Dual_VAR );
559 }
560 
561 
563 
564 
565 /*
566  * end of file
567  */
real_t g[NVMAX]
Definition: QProblem.h:68
HessianType getHessianType() const
#define ST_LOWER
Indexlist * getFree()
Implements the online active set strategy for box-constrained QPs.
int getNFX()
Implements the online active set strategy for QPs with varying matrices.
Allows to pass back messages to the calling function.
int getNV() const
#define HST_IDENTITY
int getNAC()
SubjectToStatus getStatus(int i) const
Bounds bounds
Definition: QProblem.h:72
real_t x[NVMAX]
Definition: QProblem.h:77
int getNFR()
returnValue getMaxKKTviolation(QProblemB *qp, real_t &maxKKTviolation) const
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)
void rhs(const real_t *x, real_t *f)
int getNC() const
real_t y[NVMAX+NCMAX]
Definition: QProblem.h:78
returnValue getVarianceCovariance(QProblemB *qp, real_t *g_b_bA_VAR, real_t *Primal_Dual_VAR) const
#define HST_ZERO
returnValue times(int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD) const
Indexlist * getActive()
DenseMatrix * H
Definition: QProblem.h:65
real_t ub[NVMAX]
Definition: QProblem.h:70
real_t lb[NVMAX]
Definition: QProblem.h:69
#define BT_FALSE
Definition: acado_types.hpp:49
#define ST_UPPER
double real_t
Definition: AD_test.c:10
Implements the online active set strategy for QPs with general constraints.
Indexlist * getFixed()


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Mon Jun 10 2019 12:35:06