external_packages/qpoases/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-2008 by Hans Joachim Ferreau et al. All rights reserved.
6  *
7  * qpOASES is free software; you can redistribute it and/or
8  * modify it under the terms of the GNU Lesser General Public
9  * License as published by the Free Software Foundation; either
10  * version 2.1 of the License, or (at your option) any later version.
11  *
12  * qpOASES is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15  * Lesser General Public License for more details.
16  *
17  * You should have received a copy of the GNU Lesser General Public
18  * License along with qpOASES; if not, write to the Free Software
19  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
20  *
21  */
22 
23 
33 #include <EXTRAS/SolutionAnalysis.hpp>
34 
35 /*
36  * S o l u t i o n A n a l y s i s
37  */
38 SolutionAnalysis::SolutionAnalysis( )
39 {
40 
41 }
42 
43 /*
44  * S o l u t i o n A n a l y s i s
45  */
46 SolutionAnalysis::SolutionAnalysis( const SolutionAnalysis& rhs )
47 {
48 
49 }
50 
51 /*
52  * ~ S o l u t i o n A n a l y s i s
53  */
54 SolutionAnalysis::~SolutionAnalysis( )
55 {
56 
57 }
58 
59 /*
60  * o p e r a t o r =
61  */
62 SolutionAnalysis& SolutionAnalysis::operator=( const SolutionAnalysis& rhs )
63 {
64  if ( this != &rhs )
65  {
66 
67  }
68 
69  return *this;
70 }
71 
72 /*
73  * g e t H e s s i a n I n v e r s e
74  */
75 returnValue SolutionAnalysis::getHessianInverse( QProblem* qp, real_t* hessianInverse )
76 {
77  returnValue returnvalue; /* the return value */
78  BooleanType Delta_bC_isZero = BT_FALSE; /* (just use FALSE here) */
79  BooleanType Delta_bB_isZero = BT_FALSE; /* (just use FALSE here) */
80 
81  register int run1, run2, run3;
82 
83  register int nFR, nFX;
84 
85  /* Ask for the number of free and fixed variables, assumes that active set
86  * is constant for the covariance evaluation */
87  nFR = qp->getNFR( );
88  nFX = qp->getNFX( );
89 
90  /* Ask for the corresponding index arrays: */
91  if ( qp->bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
92  return THROWERROR( RET_HOTSTART_FAILED );
93 
94  if ( qp->bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN )
95  return THROWERROR( RET_HOTSTART_FAILED );
96 
97  if ( qp->constraints.getActive( )->getNumberArray( AC_idx ) != SUCCESSFUL_RETURN )
98  return THROWERROR( RET_HOTSTART_FAILED );
99 
100  /* Initialization: */
101  for( run1 = 0; run1 < NVMAX; run1++ )
102  delta_g_cov[ run1 ] = 0.0;
103 
104  for( run1 = 0; run1 < NVMAX; run1++ )
105  delta_lb_cov[ run1 ] = 0.0;
106 
107  for( run1 = 0; run1 < NVMAX; run1++ )
108  delta_ub_cov[ run1 ] = 0.0;
109 
110  for( run1 = 0; run1 < NCMAX; run1++ )
111  delta_lbA_cov[ run1 ] = 0.0;
112 
113  for( run1 = 0; run1 < NCMAX; run1++ )
114  delta_ubA_cov[ run1 ] = 0.0;
115 
116  /* The following loop solves the following:
117  *
118  * KKT * x =
119  * [delta_g_cov', delta_lbA_cov', delta_ubA_cov', delta_lb_cov', delta_ub_cov]'
120  *
121  * for the first NVMAX (negative) elementary vectors in order to get
122  * transposed inverse of the Hessian. Assuming that the Hessian is
123  * symmetric, the function will return transposed inverse, instead of the
124  * true inverse.
125  *
126  * Note, that we use negative elementary vectors due because internal
127  * implementation of the function hotstart_determineStepDirection requires
128  * so.
129  *
130  * */
131 
132  for( run3 = 0; run3 < NVMAX; run3++ )
133  {
134  /* Line wise loading of the corresponding (negative) elementary vector: */
135  delta_g_cov[ run3 ] = -1.0;
136 
137  /* Evaluation of the step: */
138  returnvalue = qp->hotstart_determineStepDirection(
139  FR_idx, FX_idx, AC_idx,
140  delta_g_cov, delta_lbA_cov, delta_ubA_cov, delta_lb_cov, delta_ub_cov,
141  Delta_bC_isZero, Delta_bB_isZero,
142  delta_xFX, delta_xFR, delta_yAC, delta_yFX
143  );
144  if ( returnvalue != SUCCESSFUL_RETURN )
145  {
146  return returnvalue;
147  }
148 
149  /* Line wise storage of the QP reaction: */
150  for( run1 = 0; run1 < nFR; run1++ )
151  {
152  run2 = FR_idx[ run1 ];
153 
154  hessianInverse[run3 * NVMAX + run2] = delta_xFR[ run1 ];
155  }
156 
157  for( run1 = 0; run1 < nFX; run1++ )
158  {
159  run2 = FX_idx[ run1 ];
160 
161  hessianInverse[run3 * NVMAX + run2] = delta_xFX[ run1 ];
162  }
163 
164  /* Prepare for the next iteration */
165  delta_g_cov[ run3 ] = 0.0;
166  }
167 
168  // TODO: Perform the transpose of the inverse of the Hessian matrix
169 
170  return SUCCESSFUL_RETURN;
171 }
172 
173 /*
174  * g e t H e s s i a n I n v e r s e
175  */
176 returnValue SolutionAnalysis::getHessianInverse( QProblemB* qp, real_t* hessianInverse )
177 {
178  returnValue returnvalue; /* the return value */
179  BooleanType Delta_bB_isZero = BT_FALSE; /* (just use FALSE here) */
180 
181  register int run1, run2, run3;
182 
183  register int nFR, nFX;
184 
185  /* Ask for the number of free and fixed variables, assumes that active set
186  * is constant for the covariance evaluation */
187  nFR = qp->getNFR( );
188  nFX = qp->getNFX( );
189 
190  /* Ask for the corresponding index arrays: */
191  if ( qp->bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
192  return THROWERROR( RET_HOTSTART_FAILED );
193 
194  if ( qp->bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN )
195  return THROWERROR( RET_HOTSTART_FAILED );
196 
197  /* Initialization: */
198  for( run1 = 0; run1 < NVMAX; run1++ )
199  delta_g_cov[ run1 ] = 0.0;
200 
201  for( run1 = 0; run1 < NVMAX; run1++ )
202  delta_lb_cov[ run1 ] = 0.0;
203 
204  for( run1 = 0; run1 < NVMAX; run1++ )
205  delta_ub_cov[ run1 ] = 0.0;
206 
207  /* The following loop solves the following:
208  *
209  * KKT * x =
210  * [delta_g_cov', delta_lb_cov', delta_ub_cov']'
211  *
212  * for the first NVMAX (negative) elementary vectors in order to get
213  * transposed inverse of the Hessian. Assuming that the Hessian is
214  * symmetric, the function will return transposed inverse, instead of the
215  * true inverse.
216  *
217  * Note, that we use negative elementary vectors due because internal
218  * implementation of the function hotstart_determineStepDirection requires
219  * so.
220  *
221  * */
222 
223  for( run3 = 0; run3 < NVMAX; run3++ )
224  {
225  /* Line wise loading of the corresponding (negative) elementary vector: */
226  delta_g_cov[ run3 ] = -1.0;
227 
228  /* Evaluation of the step: */
229  returnvalue = qp->hotstart_determineStepDirection(
230  FR_idx, FX_idx,
231  delta_g_cov, delta_lb_cov, delta_ub_cov,
232  Delta_bB_isZero,
233  delta_xFX, delta_xFR, delta_yFX
234  );
235  if ( returnvalue != SUCCESSFUL_RETURN )
236  {
237  return returnvalue;
238  }
239 
240  /* Line wise storage of the QP reaction: */
241  for( run1 = 0; run1 < nFR; run1++ )
242  {
243  run2 = FR_idx[ run1 ];
244 
245  hessianInverse[run3 * NVMAX + run2] = delta_xFR[ run1 ];
246  }
247 
248  for( run1 = 0; run1 < nFX; run1++ )
249  {
250  run2 = FX_idx[ run1 ];
251 
252  hessianInverse[run3 * NVMAX + run2] = delta_xFX[ run1 ];
253  }
254 
255  /* Prepare for the next iteration */
256  delta_g_cov[ run3 ] = 0.0;
257  }
258 
259  // TODO: Perform the transpose of the inverse of the Hessian matrix
260 
261  return SUCCESSFUL_RETURN;
262 }
263 
264 /*
265  * g e t V a r i a n c e C o v a r i a n c e
266  */
267 
268 #if QPOASES_USE_OLD_VERSION
269 
270 returnValue SolutionAnalysis::getVarianceCovariance( QProblem* qp, real_t* g_b_bA_VAR, real_t* Primal_Dual_VAR )
271 {
272  int run1, run2, run3; /* simple run variables (for loops). */
273 
274  returnValue returnvalue; /* the return value */
275  BooleanType Delta_bC_isZero = BT_FALSE; /* (just use FALSE here) */
276  BooleanType Delta_bB_isZero = BT_FALSE; /* (just use FALSE here) */
277 
278  /* ASK FOR THE NUMBER OF FREE AND FIXED VARIABLES:
279  * (ASSUMES THAT ACTIVE SET IS CONSTANT FOR THE
280  * VARIANCE-COVARIANCE EVALUATION)
281  * ----------------------------------------------- */
282  int nFR, nFX, nAC;
283 
284  nFR = qp->getNFR( );
285  nFX = qp->getNFX( );
286  nAC = qp->getNAC( );
287 
288  if ( qp->bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
289  return THROWERROR( RET_HOTSTART_FAILED );
290 
291  if ( qp->bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN )
292  return THROWERROR( RET_HOTSTART_FAILED );
293 
294  if ( qp->constraints.getActive( )->getNumberArray( AC_idx ) != SUCCESSFUL_RETURN )
295  return THROWERROR( RET_HOTSTART_FAILED );
296 
297  /* SOME INITIALIZATIONS:
298  * --------------------- */
299  for( run1 = 0; run1 < KKT_DIM * KKT_DIM; run1++ )
300  {
301  K [run1] = 0.0;
302  Primal_Dual_VAR[run1] = 0.0;
303  }
304 
305  /* ================================================================= */
306 
307  /* FIRST MATRIX MULTIPLICATION (OBTAINS THE INTERMEDIATE RESULT
308  * K := [ ("ACTIVE" KKT-MATRIX OF THE QP)^(-1) * g_b_bA_VAR ]^T )
309  * THE EVALUATION OF THE INVERSE OF THE KKT-MATRIX OF THE QP
310  * WITH RESPECT TO THE CURRENT ACTIVE SET
311  * USES THE EXISTING CHOLESKY AND TQ-DECOMPOSITIONS. FOR DETAILS
312  * cf. THE (protected) FUNCTION determineStepDirection. */
313 
314  for( run3 = 0; run3 < KKT_DIM; run3++ )
315  {
316 
317  for( run1 = 0; run1 < NVMAX; run1++ )
318  {
319  delta_g_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+run1];
320  delta_lb_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+NVMAX+run1]; /* LINE-WISE LOADING OF THE INPUT */
321  delta_ub_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+NVMAX+run1]; /* VARIANCE-COVARIANCE */
322  }
323  for( run1 = 0; run1 < NCMAX; run1++ )
324  {
325  delta_lbA_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+2*NVMAX+run1];
326  delta_ubA_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+2*NVMAX+run1];
327  }
328 
329  /* EVALUATION OF THE STEP:
330  * ------------------------------------------------------------------------------ */
331 
332  returnvalue = qp->hotstart_determineStepDirection(
333  FR_idx, FX_idx, AC_idx,
334  delta_g_cov, delta_lbA_cov, delta_ubA_cov, delta_lb_cov, delta_ub_cov,
335  Delta_bC_isZero, Delta_bB_isZero, delta_xFX,delta_xFR,
336  delta_yAC,delta_yFX );
337 
338  /* ------------------------------------------------------------------------------ */
339 
340  /* STOP THE ALGORITHM IN THE CASE OF NO SUCCESFUL RETURN:
341  * ------------------------------------------------------ */
342  if ( returnvalue != SUCCESSFUL_RETURN )
343  {
344  return returnvalue;
345  }
346 
347  /* LINE WISE */
348  /* STORAGE OF THE QP-REACTION */
349  /* (uses the index list) */
350 
351  for( run1=0; run1<nFR; run1++ )
352  {
353  run2 = FR_idx[run1];
354  K[run3*KKT_DIM+run2] = delta_xFR[run1];
355  }
356  for( run1=0; run1<nFX; run1++ )
357  {
358  run2 = FX_idx[run1];
359  K[run3*KKT_DIM+run2] = delta_xFX[run1];
360  K[run3*KKT_DIM+NVMAX+run2] = delta_yFX[run1];
361  }
362  for( run1=0; run1<nAC; run1++ )
363  {
364  run2 = AC_idx[run1];
365  K[run3*KKT_DIM+2*NVMAX+run2] = delta_yAC[run1];
366  }
367  }
368 
369  /* ================================================================= */
370 
371  /* SECOND MATRIX MULTIPLICATION (OBTAINS THE FINAL RESULT
372  * Primal_Dual_VAR := ("ACTIVE" KKT-MATRIX OF THE QP)^(-1) * K )
373  * THE APPLICATION OF THE KKT-INVERSE IS AGAIN REALIZED
374  * BY USING THE PROTECTED FUNCTION
375  * determineStepDirection */
376 
377  for( run3 = 0; run3 < KKT_DIM; run3++ )
378  {
379 
380  for( run1 = 0; run1 < NVMAX; run1++ )
381  {
382  delta_g_cov [run1] = K[run3+ run1*KKT_DIM];
383  delta_lb_cov [run1] = K[run3+(NVMAX+run1)*KKT_DIM]; /* ROW WISE LOADING OF THE */
384  delta_ub_cov [run1] = K[run3+(NVMAX+run1)*KKT_DIM]; /* INTERMEDIATE RESULT K */
385  }
386  for( run1 = 0; run1 < NCMAX; run1++ )
387  {
388  delta_lbA_cov [run1] = K[run3+(2*NVMAX+run1)*KKT_DIM];
389  delta_ubA_cov [run1] = K[run3+(2*NVMAX+run1)*KKT_DIM];
390  }
391 
392  /* EVALUATION OF THE STEP:
393  * ------------------------------------------------------------------------------ */
394 
395  returnvalue = qp->hotstart_determineStepDirection(
396  FR_idx, FX_idx, AC_idx,
397  delta_g_cov, delta_lbA_cov, delta_ubA_cov, delta_lb_cov, delta_ub_cov,
398  Delta_bC_isZero, Delta_bB_isZero, delta_xFX,delta_xFR,
399  delta_yAC,delta_yFX );
400 
401  /* ------------------------------------------------------------------------------ */
402 
403  /* STOP THE ALGORITHM IN THE CASE OF NO SUCCESFUL RETURN:
404  * ------------------------------------------------------ */
405  if ( returnvalue != SUCCESSFUL_RETURN )
406  {
407  return returnvalue;
408  }
409 
410  /* ROW-WISE STORAGE */
411  /* OF THE RESULT. */
412 
413  for( run1=0; run1<nFR; run1++ )
414  {
415  run2 = FR_idx[run1];
416  Primal_Dual_VAR[run3+run2*KKT_DIM] = delta_xFR[run1];
417  }
418  for( run1=0; run1<nFX; run1++ )
419  {
420  run2 = FX_idx[run1];
421  Primal_Dual_VAR[run3+run2*KKT_DIM ] = delta_xFX[run1];
422  Primal_Dual_VAR[run3+(NVMAX+run2)*KKT_DIM] = delta_yFX[run1];
423  }
424  for( run1=0; run1<nAC; run1++ )
425  {
426  run2 = AC_idx[run1];
427  Primal_Dual_VAR[run3+(2*NVMAX+run2)*KKT_DIM] = delta_yAC[run1];
428  }
429  }
430 
431  return SUCCESSFUL_RETURN;
432 }
433 
434 #endif
QProblem::hotstart_determineStepDirection
returnValue hotstart_determineStepDirection(const int *const FR_idx, const int *const FX_idx, const int *const AC_idx, 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)
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/SRC/QProblem.cpp:2697
Bounds::getFree
Indexlist * getFree()
QProblemB
Implements the online active set strategy for box-constrained QPs.
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/QProblemB.hpp:55
SUCCESSFUL_RETURN
Definition: acado_types.hpp:836
QProblemB::getNFX
int getNFX()
NCMAX
#define NCMAX
Definition: qpoases3/include/qpOASES_e/Constants.h:74
returnValue
Allows to pass back messages to the calling function.
Definition: acado_types.hpp:1139
NVMAX
#define NVMAX
Definition: qpoases3/include/qpOASES_e/Constants.h:70
THROWERROR
#define THROWERROR(retval)
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:391
QProblem::getNAC
int getNAC()
RET_HOTSTART_FAILED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:117
QProblem::bounds
Bounds bounds
Definition: QProblem.h:72
BooleanType
BooleanType
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/Types.hpp:44
QProblemB::getNFR
int getNFR()
rhs
void rhs(const real_t *x, real_t *f)
Definition: examples/code_generation/simulation/externModel_export/model.c:5
SolutionAnalysis::getVarianceCovariance
returnValue getVarianceCovariance(QProblemB *qp, real_t *g_b_bA_VAR, real_t *Primal_Dual_VAR) const
Definition: external_packages/qpOASES-3.0beta/src/extras/SolutionAnalysis.cpp:312
Constraints::getActive
Indexlist * getActive()
BT_FALSE
#define BT_FALSE
Definition: acado_types.hpp:49
QProblemB::hotstart_determineStepDirection
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)
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/SRC/QProblemB.cpp:1570
real_t
double real_t
Definition: AD_test.c:10
QProblem
Implements the online active set strategy for QPs with general constraints.
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/QProblem.hpp:53
Bounds::getFixed
Indexlist * getFixed()

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