QProblemB.c
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-2015 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 
36 #include <qpOASES_e/QProblemB.h>
37 
38 
39 BEGIN_NAMESPACE_QPOASES
40 
41 
42 /*
43  * Q P r o b l e m B
44  */
45 void QProblemBCON( QProblemB* _THIS,
46  int _nV, HessianType _hessianType )
47 {
48  int i;
49 
50  #ifdef __CODE_GENERATION__
51  Options_setToFast( &(_THIS->options) );
52  #else
53  Options_setToDefault( &(_THIS->options) );
54  #endif /* __CODE_GENERATION__ */
55 
56  /* print copyright notice */
57  if (_THIS->options.printLevel != PL_NONE)
58  qpOASES_printCopyrightNotice( );
59 
60  /* consistency check */
61  if ( ( _nV <= 0 ) || ( _nV > NVMAX ) )
62  {
63  _nV = 1;
64  THROWERROR( RET_INVALID_ARGUMENTS );
65  assert( 1 == 0 );
66  }
67 
68  /* reset global message handler */
69  MessageHandling_reset( qpOASES_getGlobalMessageHandler() );
70 
71  _THIS->H = &(_THIS->HH);
72 
73  for( i=0; i<_nV; ++i ) _THIS->g[i] = 0.0;
74  for( i=0; i<_nV; ++i ) _THIS->lb[i] = 0.0;
75  for( i=0; i<_nV; ++i ) _THIS->ub[i] = 0.0;
76 
77  for( i=0; i<_nV; ++i ) _THIS->x[i] = 0.0;
78  for( i=0; i<_nV; ++i ) _THIS->y[i] = 0.0;
79 
80  Bounds_init( &(_THIS->bounds),_nV );
81 
82  _THIS->haveCholesky = BT_FALSE;
83 
84  _THIS->tau = 0.0;
85 
86  _THIS->hessianType = _hessianType;
87  _THIS->regVal = 0.0;
88 
89  _THIS->infeasible = BT_FALSE;
90  _THIS->unbounded = BT_FALSE;
91 
92  _THIS->status = QPS_NOTINITIALISED;
93 
94  _THIS->count = 0;
95 
96  _THIS->ramp0 = _THIS->options.initialRamping;
97  _THIS->ramp1 = _THIS->options.finalRamping;
98  _THIS->rampOffset = 0;
99 
100  QProblemB_setPrintLevel( _THIS,_THIS->options.printLevel );
101 }
102 
103 
104 /*
105  * c o p y
106  */
107 void QProblemBCPY( QProblemB* FROM,
108  QProblemB* TO
109  )
110 {
111  unsigned int _nV = (unsigned int)QProblemB_getNV( FROM );
112 
113  TO->bounds = FROM->bounds;
114 
115  TO->HH = FROM->HH;
116  TO->H = &(TO->HH);
117 
118  QProblemB_setG( TO,FROM->g );
119  QProblemB_setLB( TO,FROM->lb );
120  QProblemB_setUB( TO,FROM->ub );
121 
122  memcpy( TO->R,FROM->R,NVMAX*NVMAX*sizeof(real_t) );
123 
124  TO->haveCholesky = FROM->haveCholesky;
125 
126  memcpy( TO->x,FROM->x,_nV*sizeof(real_t) );
127  memcpy( TO->y,FROM->y,_nV*sizeof(real_t) );
128 
129  TO->tau = FROM->tau;
130 
131  TO->hessianType = FROM->hessianType;
132  TO->regVal = FROM->regVal;
133 
134  TO->infeasible = FROM->infeasible;
135  TO->unbounded = FROM->unbounded;
136 
137  TO->status = FROM->status;
138 
139  TO->count = FROM->count;
140 
141  TO->ramp0 = FROM->ramp0;
142  TO->ramp1 = FROM->ramp1;
143 
144  OptionsCPY( &(FROM->options),&(TO->options) );
145  QProblemB_setPrintLevel( TO,TO->options.printLevel );
146 }
147 
148 
149 
150 /*
151  * r e s e t
152  */
153 returnValue QProblemB_reset( QProblemB* _THIS )
154 {
155  int i;
156  int nV = QProblemB_getNV( _THIS );
157 
158  if ( nV == 0 )
159  return THROWERROR( RET_QPOBJECT_NOT_SETUP );
160 
161  /* 1) Reset bounds. */
162  Bounds_init( &(_THIS->bounds),nV );
163 
164  /* 2) Reset Cholesky decomposition. */
165  for( i=0; i<NVMAX*NVMAX; ++i )
166  _THIS->R[i] = 0.0;
167 
168  _THIS->haveCholesky = BT_FALSE;
169 
170  /* 3) Reset steplength and status flags. */
171  _THIS->tau = 0.0;
172 
173  _THIS->hessianType = HST_UNKNOWN;
174  _THIS->regVal = 0.0;
175 
176  _THIS->infeasible = BT_FALSE;
177  _THIS->unbounded = BT_FALSE;
178 
179  _THIS->status = QPS_NOTINITIALISED;
180 
181  _THIS->ramp0 = _THIS->options.initialRamping;
182  _THIS->ramp1 = _THIS->options.finalRamping;
183  _THIS->rampOffset = 0;
184 
185  return SUCCESSFUL_RETURN;
186 }
187 
188 
189 /*
190  * i n i t
191  */
192 returnValue QProblemB_initM( QProblemB* _THIS, DenseMatrix *_H, const real_t* const _g,
193  const real_t* const _lb, const real_t* const _ub,
194  int* nWSR, real_t* const cputime
195  )
196 {
197  if ( QProblemB_getNV( _THIS ) == 0 )
198  return THROWERROR( RET_QPOBJECT_NOT_SETUP );
199 
200  /* 1) Consistency check. */
201  if ( QProblemB_isInitialised( _THIS ) == BT_TRUE )
202  {
203  THROWWARNING( RET_QP_ALREADY_INITIALISED );
204  QProblemB_reset( _THIS );
205  }
206 
207  /* 2) Setup QP data. */
208  if ( QProblemB_setupQPdataM( _THIS,_H,_g,_lb,_ub ) != SUCCESSFUL_RETURN )
209  return THROWERROR( RET_INVALID_ARGUMENTS );
210 
211  /* 3) Call to main initialisation routine (without any additional information). */
212  return QProblemB_solveInitialQP( _THIS,0,0,0,0, nWSR,cputime );
213 }
214 
215 
216 /*
217  * i n i t
218  */
219 returnValue QProblemB_init( QProblemB* _THIS, real_t* const _H, const real_t* const _g,
220  const real_t* const _lb, const real_t* const _ub,
221  int* nWSR, real_t* const cputime
222  )
223 {
224  if ( QProblemB_getNV( _THIS ) == 0 )
225  return THROWERROR( RET_QPOBJECT_NOT_SETUP );
226 
227  /* 1) Consistency check. */
228  if ( QProblemB_isInitialised( _THIS ) == BT_TRUE )
229  {
230  THROWWARNING( RET_QP_ALREADY_INITIALISED );
231  QProblemB_reset( _THIS );
232  }
233 
234  /* 2) Setup QP data. */
235  if ( QProblemB_setupQPdata( _THIS,_H,_g,_lb,_ub ) != SUCCESSFUL_RETURN )
236  return THROWERROR( RET_INVALID_ARGUMENTS );
237 
238  /* 3) Call to main initialisation routine (without any additional information). */
239  return QProblemB_solveInitialQP( _THIS,0,0,0,0, nWSR,cputime );
240 }
241 
242 
243 /*
244  * i n i t
245  */
246 returnValue QProblemB_initF( QProblemB* _THIS, const char* const H_file, const char* const g_file,
247  const char* const lb_file, const char* const ub_file,
248  int* nWSR, real_t* const cputime
249  )
250 {
251  if ( QProblemB_getNV( _THIS ) == 0 )
252  return THROWERROR( RET_QPOBJECT_NOT_SETUP );
253 
254  /* 1) Consistency check. */
255  if ( QProblemB_isInitialised( _THIS ) == BT_TRUE )
256  {
257  THROWWARNING( RET_QP_ALREADY_INITIALISED );
258  QProblemB_reset( _THIS );
259  }
260 
261  /* 2) Setup QP data from files. */
262  if ( QProblemB_setupQPdataFromFile( _THIS,H_file,g_file,lb_file,ub_file ) != SUCCESSFUL_RETURN )
263  return THROWERROR( RET_UNABLE_TO_READ_FILE );
264 
265  /* 3) Call to main initialisation routine (without any additional information). */
266  return QProblemB_solveInitialQP( _THIS,0,0,0,0, nWSR,cputime );
267 }
268 
269 
270 /*
271  * i n i t
272  */
273 returnValue QProblemB_initMW( QProblemB* _THIS, DenseMatrix *_H, const real_t* const _g,
274  const real_t* const _lb, const real_t* const _ub,
275  int* nWSR, real_t* const cputime,
276  const real_t* const xOpt, const real_t* const yOpt,
277  Bounds* const guessedBounds,
278  const real_t* const _R
279  )
280 {
281  int i;
282  int nV = QProblemB_getNV( _THIS );
283 
284  if ( nV == 0 )
285  return THROWERROR( RET_QPOBJECT_NOT_SETUP );
286 
287  /* 1) Consistency checks. */
288  if ( QProblemB_isInitialised( _THIS ) == BT_TRUE )
289  {
290  THROWWARNING( RET_QP_ALREADY_INITIALISED );
291  QProblemB_reset( _THIS );
292  }
293 
294  if ( guessedBounds != 0 )
295  {
296  for( i=0; i<nV; ++i )
297  {
298  if ( Bounds_getStatus( guessedBounds,i ) == ST_UNDEFINED )
299  return THROWERROR( RET_INVALID_ARGUMENTS );
300  }
301  }
302 
303  /* exclude _THIS possibility in order to avoid inconsistencies */
304  if ( ( xOpt == 0 ) && ( yOpt != 0 ) && ( guessedBounds != 0 ) )
305  return THROWERROR( RET_INVALID_ARGUMENTS );
306 
307  if ( ( _R != 0 ) && ( ( xOpt != 0 ) || ( yOpt != 0 ) || ( guessedBounds != 0 ) ) )
308  return THROWERROR( RET_NO_CHOLESKY_WITH_INITIAL_GUESS );
309 
310  /* 2) Setup QP data. */
311  if ( QProblemB_setupQPdataM( _THIS,_H,_g,_lb,_ub ) != SUCCESSFUL_RETURN )
312  return THROWERROR( RET_INVALID_ARGUMENTS );
313 
314  /* 3) Call to main initialisation routine. */
315  return QProblemB_solveInitialQP( _THIS,xOpt,yOpt,guessedBounds,_R, nWSR,cputime );
316 }
317 
318 
319 /*
320  * i n i t
321  */
322 returnValue QProblemB_initW( QProblemB* _THIS, real_t* const _H, const real_t* const _g,
323  const real_t* const _lb, const real_t* const _ub,
324  int* nWSR, real_t* const cputime,
325  const real_t* const xOpt, const real_t* const yOpt,
326  Bounds* const guessedBounds,
327  const real_t* const _R
328  )
329 {
330  int i;
331  int nV = QProblemB_getNV( _THIS );
332 
333  if ( nV == 0 )
334  return THROWERROR( RET_QPOBJECT_NOT_SETUP );
335 
336  /* 1) Consistency checks. */
337  if ( QProblemB_isInitialised( _THIS ) == BT_TRUE )
338  {
339  THROWWARNING( RET_QP_ALREADY_INITIALISED );
340  QProblemB_reset( _THIS );
341  }
342 
343  if ( guessedBounds != 0 )
344  {
345  for( i=0; i<nV; ++i )
346  {
347  if ( Bounds_getStatus( guessedBounds,i ) == ST_UNDEFINED )
348  return THROWERROR( RET_INVALID_ARGUMENTS );
349  }
350  }
351 
352  /* exclude _THIS possibility in order to avoid inconsistencies */
353  if ( ( xOpt == 0 ) && ( yOpt != 0 ) && ( guessedBounds != 0 ) )
354  return THROWERROR( RET_INVALID_ARGUMENTS );
355 
356  if ( ( _R != 0 ) && ( ( xOpt != 0 ) || ( yOpt != 0 ) || ( guessedBounds != 0 ) ) )
357  return THROWERROR( RET_NO_CHOLESKY_WITH_INITIAL_GUESS );
358 
359  /* 2) Setup QP data. */
360  if ( QProblemB_setupQPdata( _THIS,_H,_g,_lb,_ub ) != SUCCESSFUL_RETURN )
361  return THROWERROR( RET_INVALID_ARGUMENTS );
362 
363  /* 3) Call to main initialisation routine. */
364  return QProblemB_solveInitialQP( _THIS,xOpt,yOpt,guessedBounds,_R, nWSR,cputime );
365 }
366 
367 
368 /*
369  * i n i t
370  */
371 returnValue QProblemB_initFW( QProblemB* _THIS, const char* const H_file, const char* const g_file,
372  const char* const lb_file, const char* const ub_file,
373  int* nWSR, real_t* const cputime,
374  const real_t* const xOpt, const real_t* const yOpt,
375  Bounds* const guessedBounds,
376  const char* const R_file
377  )
378 {
379  int i;
380  int nV = QProblemB_getNV( _THIS );
381 
382  returnValue returnvalue;
383 
384  if ( nV == 0 )
385  return THROWERROR( RET_QPOBJECT_NOT_SETUP );
386 
387  /* 1) Consistency checks. */
388  if ( QProblemB_isInitialised( _THIS ) == BT_TRUE )
389  {
390  THROWWARNING( RET_QP_ALREADY_INITIALISED );
391  QProblemB_reset( _THIS );
392  }
393 
394  if ( guessedBounds != 0 )
395  {
396  for( i=0; i<nV; ++i )
397  {
398  if ( Bounds_getStatus( guessedBounds,i ) == ST_UNDEFINED )
399  return THROWERROR( RET_INVALID_ARGUMENTS );
400  }
401  }
402 
403  /* exclude _THIS possibility in order to avoid inconsistencies */
404  if ( ( xOpt == 0 ) && ( yOpt != 0 ) && ( guessedBounds != 0 ) )
405  return THROWERROR( RET_INVALID_ARGUMENTS );
406 
407  if ( ( R_file != 0 ) && ( ( xOpt != 0 ) || ( yOpt != 0 ) || ( guessedBounds != 0 ) ) )
408  return THROWERROR( RET_NO_CHOLESKY_WITH_INITIAL_GUESS );
409 
410  /* 2) Setup QP data from files. */
411  if ( QProblemB_setupQPdataFromFile( _THIS,H_file,g_file,lb_file,ub_file ) != SUCCESSFUL_RETURN )
412  return THROWERROR( RET_UNABLE_TO_READ_FILE );
413 
414  if ( R_file == 0 )
415  {
416  /* 3) Call to main initialisation routine. */
417  return QProblemB_solveInitialQP( _THIS,xOpt,yOpt,guessedBounds,0, nWSR,cputime );
418  }
419  else
420  {
421  /* Also read Cholesky factor from file and store it directly into R [thus... */
422  returnvalue = qpOASES_readFromFileM( _THIS->R, nV,nV, R_file );
423  if ( returnvalue != SUCCESSFUL_RETURN )
424  return THROWWARNING( returnvalue );
425 
426  /* 3) Call to main initialisation routine. ...passing R here!] */
427  return QProblemB_solveInitialQP( _THIS,xOpt,yOpt,guessedBounds,_THIS->R, nWSR,cputime );
428  }
429 }
430 
431 
432 /*
433  * s e t u p I n i t i a l C h o l e s k y
434  */
435 returnValue QProblemB_setupInitialCholesky( QProblemB* _THIS )
436 {
437  returnValue returnvalueCholesky;
438 
439  /* If regularisation shall be used, always regularise at beginning
440  * if initial working set is not empty. */
441  if ( ( QProblemB_getNV( _THIS ) != QProblemB_getNFR( _THIS ) - QProblemB_getNFV( _THIS ) ) && ( _THIS->options.enableRegularisation == BT_TRUE ) )
442  {
443  if ( QProblemB_regulariseHessian( _THIS ) != SUCCESSFUL_RETURN )
444  return RET_INIT_FAILED_REGULARISATION;
445  }
446 
447  /* Factorise projected Hessian
448  * now handles all special cases (no active bounds/constraints, no nullspace) */
449  returnvalueCholesky = QProblemB_computeCholesky( _THIS );
450 
451  /* If Hessian is not positive definite, regularise and try again. */
452  if ( returnvalueCholesky == RET_HESSIAN_NOT_SPD )
453  {
454  if ( QProblemB_regulariseHessian( _THIS ) != SUCCESSFUL_RETURN )
455  return RET_INIT_FAILED_REGULARISATION;
456 
457  returnvalueCholesky = QProblemB_computeCholesky( _THIS );
458  }
459 
460  if ( returnvalueCholesky != SUCCESSFUL_RETURN )
461  return RET_INIT_FAILED_CHOLESKY;
462 
463  _THIS->haveCholesky = BT_TRUE;
464  return SUCCESSFUL_RETURN;
465 }
466 
467 
468 /*
469  * h o t s t a r t
470  */
471 returnValue QProblemB_hotstart( QProblemB* _THIS, const real_t* const g_new,
472  const real_t* const lb_new, const real_t* const ub_new,
473  int* nWSR, real_t* const cputime
474  )
475 {
476  returnValue returnvalue = SUCCESSFUL_RETURN;
477  int i, nActiveFar;
478  int nV = QProblemB_getNV( _THIS );
479 
480  int nWSR_max = *nWSR;
481  int nWSR_performed = 0;
482 
483  real_t cputime_remaining = QPOASES_INFTY;
484  real_t cputime_needed = 0.0;
485 
486  real_t farbound = _THIS->options.initialFarBounds;
487 
488  BooleanType isFirstCall = BT_TRUE;
489 
490  myStatic real_t ub_new_far[NVMAX];
491  myStatic real_t lb_new_far[NVMAX];
492 
493  real_t tol;
494 
495  if ( QProblemB_getNV( _THIS ) == 0 )
496  return THROWERROR( RET_QPOBJECT_NOT_SETUP );
497 
498  /* Simple check for consistency of bounds */
499  if ( QProblemB_areBoundsConsistent( _THIS,lb_new,ub_new ) != SUCCESSFUL_RETURN )
500  return QProblemB_setInfeasibilityFlag( _THIS,returnvalue,BT_TRUE );
501 
502  ++(_THIS->count);
503 
504 
505  if ( _THIS->haveCholesky == BT_FALSE )
506  {
507  returnvalue = QProblemB_setupInitialCholesky( _THIS );
508  if (returnvalue != SUCCESSFUL_RETURN)
509  return THROWERROR(returnvalue);
510  }
511 
512  if ( _THIS->options.enableFarBounds == BT_FALSE )
513  {
514  /* Automatically call standard solveQP if regularisation is not active. */
515  returnvalue = QProblemB_solveRegularisedQP( _THIS,g_new,lb_new,ub_new,
516  nWSR,cputime,0,
517  isFirstCall
518  );
519  }
520  else
521  {
522  /* possibly extend initial far bounds to largest bound/constraint data */
523  if (ub_new)
524  for (i = 0; i < nV; i++)
525  if ((ub_new[i] < QPOASES_INFTY) && (ub_new[i] > farbound)) farbound = ub_new[i];
526  if (lb_new)
527  for (i = 0; i < nV; i++)
528  if ((lb_new[i] > -QPOASES_INFTY) && (lb_new[i] < -farbound)) farbound = -lb_new[i];
529 
530  QProblemB_updateFarBounds( _THIS,farbound,nV,
531  lb_new,lb_new_far, ub_new,ub_new_far
532  );
533 
534  for ( ;; )
535  {
536  *nWSR = nWSR_max;
537  if ( cputime != 0 )
538  cputime_remaining = *cputime - cputime_needed;
539 
540  /* Automatically call standard solveQP if regularisation is not active. */
541  returnvalue = QProblemB_solveRegularisedQP( _THIS,g_new,lb_new_far,ub_new_far,
542  nWSR,&cputime_remaining,nWSR_performed,
543  isFirstCall
544  );
545 
546  nWSR_performed = *nWSR;
547  cputime_needed += cputime_remaining;
548  isFirstCall = BT_FALSE;
549 
550  /* Check for active far-bounds and move them away */
551  nActiveFar = 0;
552  farbound *= _THIS->options.growFarBounds;
553 
554  if ( _THIS->infeasible == BT_TRUE )
555  {
556  if ( farbound >= QPOASES_INFTY )
557  {
558  returnvalue = RET_HOTSTART_STOPPED_INFEASIBILITY;
559  goto farewell;
560  }
561 
562  QProblemB_updateFarBounds( _THIS,farbound,nV,
563  lb_new,lb_new_far, ub_new,ub_new_far
564  );
565  }
566  else if ( _THIS->status == QPS_SOLVED )
567  {
568  tol = farbound/_THIS->options.growFarBounds * _THIS->options.boundTolerance;
569  _THIS->status = QPS_HOMOTOPYQPSOLVED;
570 
571  for ( i=0; i<nV; ++i )
572  {
573  if ( ( ( lb_new == 0 ) || ( lb_new_far[i] > lb_new[i] ) ) && ( qpOASES_getAbs ( lb_new_far[i] - _THIS->x[i] ) < tol ) )
574  ++nActiveFar;
575  if ( ( ( ub_new == 0 ) || ( ub_new_far[i] < ub_new[i] ) ) && ( qpOASES_getAbs ( ub_new_far[i] - _THIS->x[i] ) < tol ) )
576  ++nActiveFar;
577  }
578 
579  if ( nActiveFar == 0 )
580  break;
581 
582  if ( farbound >= QPOASES_INFTY )
583  {
584  _THIS->unbounded = BT_TRUE;
585  returnvalue = RET_HOTSTART_STOPPED_UNBOUNDEDNESS;
586  goto farewell;
587  }
588 
589  QProblemB_updateFarBounds( _THIS,farbound,nV,
590  lb_new,lb_new_far, ub_new,ub_new_far
591  );
592  }
593  else
594  {
595  /* some other error when solving QP */
596  break;
597  }
598 
599  /* advance ramp offset to avoid Ramping cycles */
600  (_THIS->rampOffset)++;
601  }
602 
603  farewell:
604  if ( cputime != 0 )
605  *cputime = cputime_needed;
606  }
607 
608  return ( returnvalue != SUCCESSFUL_RETURN ) ? THROWERROR( returnvalue ) : returnvalue;
609 }
610 
611 
612 /*
613  * h o t s t a r t
614  */
615 returnValue QProblemB_hotstartF( QProblemB* _THIS, const char* const g_file,
616  const char* const lb_file, const char* const ub_file,
617  int* nWSR, real_t* const cputime
618  )
619 {
620  int nV = QProblemB_getNV( _THIS );
621  returnValue returnvalue;
622 
623  /* 1) Allocate memory (if bounds exist). */
624  myStatic real_t g_new[NVMAX];
625  myStatic real_t lb_new[NVMAX];
626  myStatic real_t ub_new[NVMAX];
627 
628 
629  if ( nV == 0 )
630  return THROWERROR( RET_QPOBJECT_NOT_SETUP );
631 
632  /* consistency check */
633  if ( g_file == 0 )
634  return THROWERROR( RET_INVALID_ARGUMENTS );
635 
636 
637  /* 2) Load new QP vectors from file. */
638  returnvalue = QProblemB_loadQPvectorsFromFile( _THIS,g_file,lb_file,ub_file,
639  g_new,lb_new,ub_new
640  );
641  if ( returnvalue != SUCCESSFUL_RETURN )
642  {
643  return THROWERROR( RET_UNABLE_TO_READ_FILE );
644  }
645 
646  /* 3) Actually perform hotstart. */
647  returnvalue = QProblemB_hotstart( _THIS,g_new,lb_new,ub_new, nWSR,cputime );
648 
649  return returnvalue;
650 }
651 
652 
653 /*
654  * h o t s t a r t
655  */
656 returnValue QProblemB_hotstartW( QProblemB* _THIS, const real_t* const g_new,
657  const real_t* const lb_new, const real_t* const ub_new,
658  int* nWSR, real_t* const cputime,
659  Bounds* const guessedBounds
660  )
661 {
662  int nV = QProblemB_getNV( _THIS );
663 
664  returnValue returnvalue;
665  real_t starttime = 0.0;
666  real_t auxTime = 0.0;
667 
668  myStatic Bounds emptyBounds;
669  BoundsCON( &emptyBounds,nV );
670 
671 
672  if ( nV == 0 )
673  return THROWERROR( RET_QPOBJECT_NOT_SETUP );
674 
675 
676  /* 1) Update working set according to guess for working set of bounds. */
677  if ( guessedBounds != 0 )
678  {
679  if ( cputime != 0 )
680  starttime = qpOASES_getCPUtime( );
681 
682  if ( QProblemB_setupAuxiliaryQP( _THIS,guessedBounds ) != SUCCESSFUL_RETURN )
683  return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
684 
685  /* Allow only remaining CPU time for usual hotstart. */
686  if ( cputime != 0 )
687  {
688  auxTime = qpOASES_getCPUtime( ) - starttime;
689  *cputime -= auxTime;
690  }
691  }
692 
693  _THIS->status = QPS_AUXILIARYQPSOLVED;
694 
695  /* 2) Perform usual homotopy. */
696  returnvalue = QProblemB_hotstart( _THIS,g_new,lb_new,ub_new, nWSR,cputime );
697 
698  /* stop runtime measurement */
699  if ( cputime != 0 )
700  *cputime += auxTime;
701 
702  return returnvalue;
703 }
704 
705 
706 /*
707  * h o t s t a r t
708  */
709 returnValue QProblemB_hotstartFW( QProblemB* _THIS, const char* const g_file,
710  const char* const lb_file, const char* const ub_file,
711  int* nWSR, real_t* const cputime,
712  Bounds* const guessedBounds
713  )
714 {
715  int nV = QProblemB_getNV( _THIS );
716  returnValue returnvalue;
717 
718  /* 1) Allocate memory (if bounds exist). */
719  myStatic real_t g_new[NVMAX];
720  myStatic real_t lb_new[NVMAX];
721  myStatic real_t ub_new[NVMAX];
722 
723 
724  if ( nV == 0 )
725  return THROWERROR( RET_QPOBJECT_NOT_SETUP );
726 
727  /* consistency check */
728  if ( g_file == 0 )
729  return THROWERROR( RET_INVALID_ARGUMENTS );
730 
731 
732  /* 2) Load new QP vectors from file. */
733  returnvalue = QProblemB_loadQPvectorsFromFile( _THIS,g_file,lb_file,ub_file,
734  g_new,lb_new,ub_new
735  );
736  if ( returnvalue != SUCCESSFUL_RETURN )
737  {
738  return THROWERROR( RET_UNABLE_TO_READ_FILE );
739  }
740 
741  /* 3) Actually perform hotstart using initialised homotopy. */
742  returnvalue = QProblemB_hotstartW( _THIS,g_new,lb_new,ub_new, nWSR,cputime,
743  guessedBounds
744  );
745 
746  return returnvalue;
747 }
748 
749 
750 
751 /*
752  * g e t W o r k i n g S e t
753  */
754 returnValue QProblemB_getWorkingSet( QProblemB* _THIS, real_t* workingSet )
755 {
756  return QProblemB_getWorkingSetBounds( _THIS,workingSet );
757 }
758 
759 
760 /*
761  * g e t W o r k i n g S e t B o u n d s
762  */
763 returnValue QProblemB_getWorkingSetBounds( QProblemB* _THIS, real_t* workingSetB )
764 {
765  int i;
766  int nV = QProblemB_getNV( _THIS );
767 
768  /* At which limit is the bound active? */
769  for (i = 0; i < nV; i++) {
770  switch ( Bounds_getStatus( &(_THIS->bounds),i ) ) {
771  case ST_LOWER: workingSetB[i] = -1.0; break;
772  case ST_UPPER: workingSetB[i] = +1.0; break;
773  default: workingSetB[i] = 0.0; break;
774  }
775  }
776 
777  return SUCCESSFUL_RETURN;
778 }
779 
780 
781 /*
782  * g e t W o r k i n g S e t C o n s t r a i n t s
783  */
784 returnValue QProblemB_getWorkingSetConstraints( QProblemB* _THIS, real_t* workingSetC )
785 {
786  if ( workingSetC == 0 )
787  return THROWERROR( RET_INVALID_ARGUMENTS );
788  else
789  return SUCCESSFUL_RETURN;
790 }
791 
792 
793 
794 /*
795  * g e t N Z
796  */
797 int QProblemB_getNZ( QProblemB* _THIS )
798 {
799  /* if no constraints are present: nZ=nFR */
800  return QProblemB_getNFR( _THIS );
801 }
802 
803 
804 /*
805  * g e t O b j V a l
806  */
807 real_t QProblemB_getObjVal( QProblemB* _THIS )
808 {
809  real_t objVal;
810 
811  /* calculated optimal objective function value
812  * only if current QP has been solved */
813  if ( ( QProblemB_getStatus( _THIS ) == QPS_AUXILIARYQPSOLVED ) ||
814  ( QProblemB_getStatus( _THIS ) == QPS_HOMOTOPYQPSOLVED ) ||
815  ( QProblemB_getStatus( _THIS ) == QPS_SOLVED ) )
816  {
817  objVal = QProblemB_getObjValX( _THIS,_THIS->x );
818  }
819  else
820  {
821  objVal = QPOASES_INFTY;
822  }
823 
824  return objVal;
825 }
826 
827 
828 /*
829  * g e t O b j V a l
830  */
831 real_t QProblemB_getObjValX( QProblemB* _THIS, const real_t* const _x )
832 {
833  int i;
834  int nV = QProblemB_getNV( _THIS );
835 
836  real_t objVal = 0.0;
837  myStatic real_t Hx[NVMAX];
838 
839  if ( nV == 0 )
840  return 0.0;
841 
842  for( i=0; i<nV; ++i )
843  objVal += _x[i]*_THIS->g[i];
844 
845  switch ( _THIS->hessianType )
846  {
847  case HST_ZERO:
848  break;
849 
850  case HST_IDENTITY:
851  for( i=0; i<nV; ++i )
852  objVal += 0.5*_x[i]*_x[i];
853  break;
854 
855  default:
856  DenseMatrix_times(_THIS->H,1, 1.0, _x, nV, 0.0, Hx, nV);
857  for( i=0; i<nV; ++i )
858  objVal += 0.5*_x[i]*Hx[i];
859  break;
860  }
861 
862  /* When using regularisation, the objective function value
863  * needs to be modified as follows:
864  * objVal = objVal - 0.5*_x*(Hmod-H)*_x - _x'*(gMod-g)
865  * = objVal - 0.5*_x*eps*_x * - _x'*(-eps*_x)
866  * = objVal + 0.5*_x*eps*_x */
867  if ( QProblemB_usingRegularisation( _THIS ) == BT_TRUE )
868  {
869  for( i=0; i<nV; ++i )
870  objVal += 0.5*_x[i]*_THIS->regVal*_x[i];
871  }
872 
873  return objVal;
874 }
875 
876 
877 /*
878  * g e t P r i m a l S o l u t i o n
879  */
880 returnValue QProblemB_getPrimalSolution( QProblemB* _THIS, real_t* const xOpt )
881 {
882  int i;
883 
884  /* return optimal primal solution vector
885  * only if current QP has been solved */
886  if ( ( QProblemB_getStatus( _THIS ) == QPS_AUXILIARYQPSOLVED ) ||
887  ( QProblemB_getStatus( _THIS ) == QPS_HOMOTOPYQPSOLVED ) ||
888  ( QProblemB_getStatus( _THIS ) == QPS_SOLVED ) )
889  {
890  for( i=0; i<QProblemB_getNV( _THIS ); ++i )
891  xOpt[i] = _THIS->x[i];
892 
893  return SUCCESSFUL_RETURN;
894  }
895  else
896  {
897  return RET_QP_NOT_SOLVED;
898  }
899 }
900 
901 
902 /*
903  * g e t D u a l S o l u t i o n
904  */
905 returnValue QProblemB_getDualSolution( QProblemB* _THIS, real_t* const yOpt )
906 {
907  int i;
908 
909  for( i=0; i<QProblemB_getNV( _THIS ); ++i )
910  yOpt[i] = _THIS->y[i];
911 
912  /* return optimal dual solution vector
913  * only if current QP has been solved */
914  if ( ( QProblemB_getStatus( _THIS ) == QPS_AUXILIARYQPSOLVED ) ||
915  ( QProblemB_getStatus( _THIS ) == QPS_HOMOTOPYQPSOLVED ) ||
916  ( QProblemB_getStatus( _THIS ) == QPS_SOLVED ) )
917  {
918  return SUCCESSFUL_RETURN;
919  }
920  else
921  {
922  return RET_QP_NOT_SOLVED;
923  }
924 }
925 
926 
927 /*
928  * s e t P r i n t L e v e l
929  */
930 returnValue QProblemB_setPrintLevel( QProblemB* _THIS, PrintLevel _printLevel )
931 {
932  #ifndef __SUPPRESSANYOUTPUT__
933  #ifndef __MATLAB__
934  if ( ( _THIS->options.printLevel == PL_HIGH ) && ( _THIS->options.printLevel != _printLevel ) )
935  THROWINFO( RET_PRINTLEVEL_CHANGED );
936  #endif /* __MATLAB__ */
937  _THIS->options.printLevel = _printLevel;
938  #else
939  _THIS->options.printLevel = PL_NONE;
940  #endif /* __SUPPRESSANYOUTPUT__ */
941 
942 
943  /* update message handler preferences */
944  switch ( _THIS->options.printLevel )
945  {
946  case PL_NONE:
947  MessageHandling_setErrorVisibilityStatus( qpOASES_getGlobalMessageHandler(),VS_HIDDEN );
948  MessageHandling_setWarningVisibilityStatus( qpOASES_getGlobalMessageHandler(),VS_HIDDEN );
949  MessageHandling_setInfoVisibilityStatus( qpOASES_getGlobalMessageHandler(),VS_HIDDEN );
950  break;
951 
952  case PL_TABULAR:
953  case PL_LOW:
954  MessageHandling_setErrorVisibilityStatus( qpOASES_getGlobalMessageHandler(),VS_VISIBLE );
955  MessageHandling_setWarningVisibilityStatus( qpOASES_getGlobalMessageHandler(),VS_HIDDEN );
956  MessageHandling_setInfoVisibilityStatus( qpOASES_getGlobalMessageHandler(),VS_HIDDEN );
957  break;
958 
959  case PL_DEBUG_ITER:
960  case PL_MEDIUM:
961  MessageHandling_setErrorVisibilityStatus( qpOASES_getGlobalMessageHandler(),VS_VISIBLE );
962  MessageHandling_setWarningVisibilityStatus( qpOASES_getGlobalMessageHandler(),VS_VISIBLE );
963  MessageHandling_setInfoVisibilityStatus( qpOASES_getGlobalMessageHandler(),VS_HIDDEN );
964  break;
965 
966  default: /* PL_HIGH */
967  MessageHandling_setErrorVisibilityStatus( qpOASES_getGlobalMessageHandler(),VS_VISIBLE );
968  MessageHandling_setWarningVisibilityStatus( qpOASES_getGlobalMessageHandler(),VS_VISIBLE );
969  MessageHandling_setInfoVisibilityStatus( qpOASES_getGlobalMessageHandler(),VS_VISIBLE );
970  break;
971  }
972 
973  return SUCCESSFUL_RETURN;
974 }
975 
976 
977 
978 /*
979  * p r i n t P r o p e r t i e s
980  */
981 returnValue QProblemB_printProperties( QProblemB* _THIS )
982 {
983  #ifndef __SUPPRESSANYOUTPUT__
984 
985  myStatic char myPrintfString[QPOASES_MAX_STRING_LENGTH];
986 
987  /* Do not print properties if print level is set to none! */
988  if ( _THIS->options.printLevel == PL_NONE )
989  return SUCCESSFUL_RETURN;
990 
991  qpOASES_myPrintf( "\n################# qpOASES -- QP PROPERTIES #################\n" );
992  qpOASES_myPrintf( "\n" );
993 
994  /* 1) Variables properties. */
995  snprintf( myPrintfString,QPOASES_MAX_STRING_LENGTH, "Number of Variables: %4.1d\n",QProblemB_getNV( _THIS ) );
996  qpOASES_myPrintf( myPrintfString );
997 
998  if ( Bounds_hasNoLower( &(_THIS->bounds) ) == BT_TRUE )
999  qpOASES_myPrintf( "Variables are not bounded from below.\n" );
1000  else
1001  qpOASES_myPrintf( "Variables are bounded from below.\n" );
1002 
1003  if ( Bounds_hasNoUpper( &(_THIS->bounds) ) == BT_TRUE )
1004  qpOASES_myPrintf( "Variables are not bounded from above.\n" );
1005  else
1006  qpOASES_myPrintf( "Variables are bounded from above.\n" );
1007 
1008  qpOASES_myPrintf( "\n" );
1009 
1010 
1011  /* 2) Further properties. */
1012  switch ( _THIS->hessianType )
1013  {
1014  case HST_ZERO:
1015  qpOASES_myPrintf( "Hessian is zero matrix (i.e. actually an LP is solved).\n" );
1016  break;
1017 
1018  case HST_IDENTITY:
1019  qpOASES_myPrintf( "Hessian is identity matrix.\n" );
1020  break;
1021 
1022  case HST_POSDEF:
1023  qpOASES_myPrintf( "Hessian matrix is (strictly) positive definite.\n" );
1024  break;
1025 
1026  case HST_POSDEF_NULLSPACE:
1027  qpOASES_myPrintf( "Hessian matrix is positive definite on null space of active constraints.\n" );
1028  break;
1029 
1030  case HST_SEMIDEF:
1031  qpOASES_myPrintf( "Hessian matrix is positive semi-definite.\n" );
1032  break;
1033 
1034  case HST_INDEF:
1035  qpOASES_myPrintf( "Hessian matrix is indefinite.\n" );
1036  break;
1037 
1038  default:
1039  qpOASES_myPrintf( "Hessian matrix has unknown type.\n" );
1040  break;
1041  }
1042 
1043  if ( _THIS->infeasible == BT_TRUE )
1044  qpOASES_myPrintf( "QP was found to be infeasible.\n" );
1045  else
1046  qpOASES_myPrintf( "QP seems to be feasible.\n" );
1047 
1048  if ( _THIS->unbounded == BT_TRUE )
1049  qpOASES_myPrintf( "QP was found to be unbounded from below.\n" );
1050  else
1051  qpOASES_myPrintf( "QP seems to be bounded from below.\n" );
1052 
1053  qpOASES_myPrintf( "\n" );
1054 
1055 
1056  /* 3) QP object properties. */
1057  switch ( _THIS->status )
1058  {
1059  case QPS_NOTINITIALISED:
1060  qpOASES_myPrintf( "Status of QP object: freshly instantiated or reset.\n" );
1061  break;
1062 
1063  case QPS_PREPARINGAUXILIARYQP:
1064  qpOASES_myPrintf( "Status of QP object: an auxiliary QP is currently setup.\n" );
1065  break;
1066 
1067  case QPS_AUXILIARYQPSOLVED:
1068  qpOASES_myPrintf( "Status of QP object: an auxilary QP was solved.\n" );
1069  break;
1070 
1071  case QPS_PERFORMINGHOMOTOPY:
1072  qpOASES_myPrintf( "Status of QP object: a homotopy step is performed.\n" );
1073  break;
1074 
1075  case QPS_HOMOTOPYQPSOLVED:
1076  qpOASES_myPrintf( "Status of QP object: an intermediate QP along the homotopy path was solved.\n" );
1077  break;
1078 
1079  case QPS_SOLVED:
1080  qpOASES_myPrintf( "Status of QP object: solution of the actual QP was found.\n" );
1081  break;
1082  }
1083 
1084  switch ( _THIS->options.printLevel )
1085  {
1086  case PL_DEBUG_ITER:
1087  qpOASES_myPrintf( "Print level of QP object is set to display a tabular output for debugging.\n" );
1088  break;
1089 
1090  case PL_TABULAR:
1091  qpOASES_myPrintf( "Print level of QP object is set to display a tabular output.\n" );
1092  break;
1093 
1094  case PL_LOW:
1095  qpOASES_myPrintf( "Print level of QP object is low, i.e. only error are printed.\n" );
1096  break;
1097 
1098  case PL_MEDIUM:
1099  qpOASES_myPrintf( "Print level of QP object is medium, i.e. error and warnings are printed.\n" );
1100  break;
1101 
1102  case PL_HIGH:
1103  qpOASES_myPrintf( "Print level of QP object is high, i.e. all available output is printed.\n" );
1104  break;
1105 
1106  default:
1107  break;
1108  }
1109 
1110  qpOASES_myPrintf( "\n" );
1111 
1112  #endif /* __SUPPRESSANYOUTPUT__ */
1113 
1114  return SUCCESSFUL_RETURN;
1115 }
1116 
1117 
1118 returnValue QProblemB_printOptions( QProblemB* _THIS )
1119 {
1120  return Options_print( &(_THIS->options) );
1121 }
1122 
1123 
1124 
1125 /*****************************************************************************
1126  * P R O T E C T E D *
1127  *****************************************************************************/
1128 
1129 
1130 
1131 
1132 /*
1133  * d e t e r m i n e H e s s i a n T y p e
1134  */
1135 returnValue QProblemB_determineHessianType( QProblemB* _THIS )
1136 {
1137  int i;
1138  real_t curDiag;
1139  BooleanType isIdentity, isZero;
1140 
1141  int nV = QProblemB_getNV( _THIS );
1142 
1143  /* if Hessian type has been set by user, do NOT change it! */
1144  switch ( _THIS->hessianType )
1145  {
1146  case HST_ZERO:
1147  /* ensure regularisation as default options do not always solve LPs */
1148  if ( _THIS->options.enableRegularisation == BT_FALSE )
1149  {
1150  _THIS->options.enableRegularisation = BT_TRUE;
1151  _THIS->options.numRegularisationSteps = 1;
1152  }
1153  return SUCCESSFUL_RETURN;
1154 
1155  case HST_IDENTITY:
1156  return SUCCESSFUL_RETURN;
1157 
1158  case HST_POSDEF:
1159  case HST_POSDEF_NULLSPACE:
1160  case HST_SEMIDEF:
1161  case HST_INDEF:
1162  /* if H == 0, continue to reset hessianType to HST_ZERO
1163  * to avoid segmentation faults! */
1164  if ( _THIS->H != 0 )
1165  return SUCCESSFUL_RETURN;
1166 
1167  default:
1168  /* HST_UNKNOWN, continue */
1169  break;
1170  }
1171 
1172  /* if Hessian has not been allocated, assume it to be all zeros! */
1173  if ( _THIS->H == 0 )
1174  {
1175  _THIS->hessianType = HST_ZERO;
1176  THROWINFO( RET_ZERO_HESSIAN_ASSUMED );
1177 
1178  /* ensure regularisation as default options do not always solve LPs */
1179  if ( _THIS->options.enableRegularisation == BT_FALSE )
1180  _THIS->options.enableRegularisation = BT_TRUE;
1181 
1182  return SUCCESSFUL_RETURN;
1183  }
1184 
1185  /* 1) If Hessian has outer-diagonal elements,
1186  * Hessian is assumed to be positive definite. */
1187  _THIS->hessianType = HST_POSDEF;
1188  if (DenseMatrix_isDiag(_THIS->H) == BT_FALSE)
1189  return SUCCESSFUL_RETURN;
1190 
1191  /* 2) Otherwise it is diagonal and test for identity or zero matrix is performed. */
1192  isIdentity = BT_TRUE;
1193  isZero = BT_TRUE;
1194 
1195  for ( i=0; i<nV; ++i )
1196  {
1197  curDiag = DenseMatrix_diag( _THIS->H,i );
1198  if ( curDiag >= QPOASES_INFTY )
1199  return RET_DIAGONAL_NOT_INITIALISED;
1200 
1201  if ( curDiag < -QPOASES_ZERO )
1202  {
1203  _THIS->hessianType = HST_INDEF;
1204  if ( _THIS->options.enableFlippingBounds == BT_FALSE )
1205  return THROWERROR( RET_HESSIAN_INDEFINITE );
1206  else
1207  return SUCCESSFUL_RETURN;
1208  }
1209 
1210  if ( qpOASES_getAbs( curDiag - 1.0 ) > QPOASES_EPS )
1211  isIdentity = BT_FALSE;
1212 
1213  if ( qpOASES_getAbs( curDiag ) > QPOASES_EPS )
1214  isZero = BT_FALSE;
1215  }
1216 
1217  if ( isIdentity == BT_TRUE )
1218  _THIS->hessianType = HST_IDENTITY;
1219 
1220  if ( isZero == BT_TRUE )
1221  {
1222  _THIS->hessianType = HST_ZERO;
1223 
1224  /* ensure regularisation as default options do not always solve LPs */
1225  if ( _THIS->options.enableRegularisation == BT_FALSE )
1226  {
1227  _THIS->options.enableRegularisation = BT_TRUE;
1228  _THIS->options.numRegularisationSteps = 1;
1229  }
1230  }
1231 
1232  return SUCCESSFUL_RETURN;
1233 }
1234 
1235 
1236 /*
1237  * s e t u p S u b j e c t T o T y p e
1238  */
1239 returnValue QProblemB_setupSubjectToType( QProblemB* _THIS )
1240 {
1241  return QProblemB_setupSubjectToTypeNew( _THIS,_THIS->lb,_THIS->ub );
1242 }
1243 
1244 
1245 /*
1246  * s e t u p S u b j e c t T o T y p e
1247  */
1248 returnValue QProblemB_setupSubjectToTypeNew( QProblemB* _THIS, const real_t* const lb_new, const real_t* const ub_new )
1249 {
1250  int i;
1251  int nV = QProblemB_getNV( _THIS );
1252 
1253 
1254  /* 1) Check if lower bounds are present. */
1255  Bounds_setNoLower( &(_THIS->bounds),BT_TRUE );
1256  if ( lb_new != 0 )
1257  {
1258  for( i=0; i<nV; ++i )
1259  {
1260  if ( lb_new[i] > -QPOASES_INFTY )
1261  {
1262  Bounds_setNoLower( &(_THIS->bounds),BT_FALSE );
1263  break;
1264  }
1265  }
1266  }
1267 
1268  /* 2) Check if upper bounds are present. */
1269  Bounds_setNoUpper( &(_THIS->bounds),BT_TRUE );
1270  if ( ub_new != 0 )
1271  {
1272  for( i=0; i<nV; ++i )
1273  {
1274  if ( ub_new[i] < QPOASES_INFTY )
1275  {
1276  Bounds_setNoUpper( &(_THIS->bounds),BT_FALSE );
1277  break;
1278  }
1279  }
1280  }
1281 
1282  /* 3) Determine implicitly fixed and unbounded variables. */
1283  if ( ( lb_new != 0 ) && ( ub_new != 0 ) )
1284  {
1285  for( i=0; i<nV; ++i )
1286  {
1287  if ( ( lb_new[i] <= -QPOASES_INFTY ) && ( ub_new[i] >= QPOASES_INFTY )
1288  && (_THIS->options.enableFarBounds == BT_FALSE))
1289  {
1290  Bounds_setType( &(_THIS->bounds),i,ST_UNBOUNDED );
1291  }
1292  else
1293  {
1294  if ( _THIS->options.enableEqualities
1295  && _THIS->lb[i] > _THIS->ub[i] - _THIS->options.boundTolerance
1296  && lb_new[i] > ub_new[i] - _THIS->options.boundTolerance)
1297  Bounds_setType( &(_THIS->bounds),i,ST_EQUALITY );
1298  else
1299  Bounds_setType( &(_THIS->bounds),i,ST_BOUNDED );
1300  }
1301  }
1302  }
1303  else
1304  {
1305  if ( ( lb_new == 0 ) && ( ub_new == 0 ) )
1306  {
1307  for( i=0; i<nV; ++i )
1308  Bounds_setType( &(_THIS->bounds),i,ST_UNBOUNDED );
1309  }
1310  else
1311  {
1312  for( i=0; i<nV; ++i )
1313  Bounds_setType( &(_THIS->bounds),i,ST_BOUNDED );
1314  }
1315  }
1316 
1317  return SUCCESSFUL_RETURN;
1318 }
1319 
1320 
1321 /*
1322  * c o m p u t e C h o l e s k y
1323  */
1324 returnValue QProblemB_computeCholesky( QProblemB* _THIS )
1325 {
1326  int i, j;
1327  int nV = QProblemB_getNV( _THIS );
1328  int nFR = QProblemB_getNFR( _THIS );
1329 
1330  int* FR_idx;
1331 
1332  long info = 0;
1333  unsigned long _nFR = (unsigned long)nFR, _nV = NVMAX;
1334 
1335  /* 1) Initialises R with all zeros. */
1336  for( i=0; i<NVMAX*NVMAX; ++i )
1337  _THIS->R[i] = 0.0;
1338 
1339  /* 2) Calculate Cholesky decomposition of H (projected to free variables). */
1340  switch ( _THIS->hessianType )
1341  {
1342  case HST_ZERO:
1343  /* if Hessian is zero matrix and it has been regularised,
1344  * its Cholesky factor is the identity matrix scaled by sqrt(eps). */
1345  if ( QProblemB_usingRegularisation( _THIS ) == BT_TRUE )
1346  {
1347  for( i=0; i<nV; ++i )
1348  RR(i,i) = qpOASES_getSqrt( _THIS->regVal );
1349  }
1350  else
1351  {
1352  return THROWERROR( RET_CHOLESKY_OF_ZERO_HESSIAN );
1353  }
1354  break;
1355 
1356  case HST_IDENTITY:
1357  /* if Hessian is identity, so is its Cholesky factor. */
1358  for( i=0; i<nV; ++i )
1359  RR(i,i) = 1.0;
1360  break;
1361 
1362  default:
1363  if ( nFR > 0 )
1364  {
1365  Indexlist_getNumberArray( Bounds_getFree( &(_THIS->bounds) ),&FR_idx );
1366 
1367  /* get H */
1368  for ( j=0; j < nFR; ++j )
1369  DenseMatrix_getCol(_THIS->H,FR_idx[j], Bounds_getFree( &(_THIS->bounds) ), 1.0, &(_THIS->R[j*NVMAX]));
1370 
1371  /* R'*R = H */
1372  POTRF( "U", &_nFR, _THIS->R, &_nV, &info );
1373 
1374  /* <0 = invalid call, =0 ok, >0 not spd */
1375  if (info > 0) {
1376  if ( _THIS->R[0] < 0.0 )
1377  {
1378  /* Cholesky decomposition has tunneled a negative
1379  * diagonal element. */
1380  _THIS->options.epsRegularisation = qpOASES_getMin( -_THIS->R[0]+_THIS->options.epsRegularisation,qpOASES_getSqrt(qpOASES_getAbs(_THIS->options.epsRegularisation)) );
1381  }
1382 
1383  _THIS->hessianType = HST_SEMIDEF;
1384  return RET_HESSIAN_NOT_SPD;
1385  }
1386 
1387 
1388  /* zero first subdiagonal to make givens updates work */
1389  for (i=0;i<nFR-1;++i)
1390  RR(i+1,i) = 0.0;
1391 
1392  }
1393  break;
1394  }
1395 
1396  return SUCCESSFUL_RETURN;
1397 }
1398 
1399 
1400 /*
1401  * 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
1402  */
1403 returnValue QProblemB_obtainAuxiliaryWorkingSet( QProblemB* _THIS, const real_t* const xOpt, const real_t* const yOpt,
1404  Bounds* const guessedBounds, Bounds* auxiliaryBounds
1405  )
1406 {
1407  int i = 0;
1408  int nV = QProblemB_getNV( _THIS );
1409 
1410 
1411  /* 1) Ensure that desiredBounds is allocated (and different from guessedBounds). */
1412  if ( ( auxiliaryBounds == 0 ) || ( auxiliaryBounds == guessedBounds ) )
1413  return THROWERROR( RET_INVALID_ARGUMENTS );
1414 
1415 
1416  /* 2) Setup working set for auxiliary initial QP. */
1417  if ( guessedBounds != 0 )
1418  {
1419  /* If an initial working set is specific, use it!
1420  * Moreover, add all implictly fixed variables if specified. */
1421  for( i=0; i<nV; ++i )
1422  {
1423  #ifdef __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
1424  if ( Bounds_getType( &(_THIS->bounds),i ) == ST_EQUALITY )
1425  {
1426  if ( Bounds_setupBound( auxiliaryBounds,i,ST_LOWER ) != SUCCESSFUL_RETURN )
1427  return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
1428  }
1429  else
1430  #endif
1431  {
1432  if ( Bounds_setupBound( auxiliaryBounds,i,Bounds_getStatus( guessedBounds,i ) ) != SUCCESSFUL_RETURN )
1433  return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
1434  }
1435  }
1436  }
1437  else /* No initial working set specified. */
1438  {
1439  if ( ( xOpt != 0 ) && ( yOpt == 0 ) )
1440  {
1441  /* Obtain initial working set by "clipping". */
1442  for( i=0; i<nV; ++i )
1443  {
1444  if ( xOpt[i] <= _THIS->lb[i] + _THIS->options.boundTolerance )
1445  {
1446  if ( Bounds_setupBound( auxiliaryBounds,i,ST_LOWER ) != SUCCESSFUL_RETURN )
1447  return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
1448  continue;
1449  }
1450 
1451  if ( xOpt[i] >= _THIS->ub[i] - _THIS->options.boundTolerance )
1452  {
1453  if ( Bounds_setupBound( auxiliaryBounds,i,ST_UPPER ) != SUCCESSFUL_RETURN )
1454  return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
1455  continue;
1456  }
1457 
1458  /* Moreover, add all implictly fixed variables if specified. */
1459  #ifdef __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
1460  if ( Bounds_getType( &(_THIS->bounds),i ) == ST_EQUALITY )
1461  {
1462  if ( Bounds_setupBound( auxiliaryBounds,i,ST_LOWER ) != SUCCESSFUL_RETURN )
1463  return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
1464  }
1465  else
1466  #endif
1467  {
1468  if ( Bounds_setupBound( auxiliaryBounds,i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
1469  return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
1470  }
1471  }
1472  }
1473 
1474  if ( ( xOpt == 0 ) && ( yOpt != 0 ) )
1475  {
1476  /* Obtain initial working set in accordance to sign of dual solution vector. */
1477  for( i=0; i<nV; ++i )
1478  {
1479  if ( yOpt[i] > QPOASES_EPS )
1480  {
1481  if ( Bounds_setupBound( auxiliaryBounds,i,ST_LOWER ) != SUCCESSFUL_RETURN )
1482  return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
1483  continue;
1484  }
1485 
1486  if ( yOpt[i] < -QPOASES_EPS )
1487  {
1488  if ( Bounds_setupBound( auxiliaryBounds,i,ST_UPPER ) != SUCCESSFUL_RETURN )
1489  return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
1490  continue;
1491  }
1492 
1493  /* Moreover, add all implictly fixed variables if specified. */
1494  #ifdef __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
1495  if ( Bounds_getType( &(_THIS->bounds),i ) == ST_EQUALITY )
1496  {
1497  if ( Bounds_setupBound( auxiliaryBounds,i,ST_LOWER ) != SUCCESSFUL_RETURN )
1498  return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
1499  }
1500  else
1501  #endif
1502  {
1503  if ( Bounds_setupBound( auxiliaryBounds,i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
1504  return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
1505  }
1506  }
1507  }
1508 
1509  /* If xOpt and yOpt are null pointer and no initial working is specified,
1510  * start with empty working set (or implicitly fixed bounds only)
1511  * for auxiliary QP. */
1512  if ( ( xOpt == 0 ) && ( yOpt == 0 ) )
1513  {
1514  for( i=0; i<nV; ++i )
1515  {
1516  switch( Bounds_getType( &(_THIS->bounds),i ) )
1517  {
1518  case ST_UNBOUNDED:
1519  if ( Bounds_setupBound( auxiliaryBounds,i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
1520  return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
1521  break;
1522 
1523  /* Only add all implictly fixed variables if specified. */
1524  #ifdef __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
1525  case ST_EQUALITY:
1526  if ( Bounds_setupBound( auxiliaryBounds,i,ST_LOWER ) != SUCCESSFUL_RETURN )
1527  return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
1528  break;
1529  #endif
1530 
1531  default:
1532  if ( Bounds_setupBound( auxiliaryBounds,i,_THIS->options.initialStatusBounds ) != SUCCESSFUL_RETURN )
1533  return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
1534  break;
1535  }
1536  }
1537  }
1538  }
1539 
1540  return SUCCESSFUL_RETURN;
1541 }
1542 
1543 
1544 /*
1545  * b a c k s o l v e R
1546  */
1547 returnValue QProblemB_backsolveR( QProblemB* _THIS, const real_t* const b, BooleanType transposed,
1548  real_t* const a
1549  )
1550 {
1551  /* Call standard backsolve procedure (i.e. removingBound == BT_FALSE). */
1552  return QProblemB_backsolveRrem( _THIS,b,transposed,BT_FALSE,a );
1553 }
1554 
1555 
1556 /*
1557  * b a c k s o l v e R
1558  */
1559 returnValue QProblemB_backsolveRrem( QProblemB* _THIS, const real_t* const b, BooleanType transposed,
1560  BooleanType removingBound,
1561  real_t* const a
1562  )
1563 {
1564  int i, j;
1565  int nR = QProblemB_getNZ( _THIS );
1566 
1567  real_t sum;
1568 
1569  /* if backsolve is called while removing a bound, reduce nZ by one. */
1570  if ( removingBound == BT_TRUE )
1571  --nR;
1572 
1573  /* nothing to do */
1574  if ( nR <= 0 )
1575  return SUCCESSFUL_RETURN;
1576 
1577 
1578  /* Solve Ra = b, where R might be transposed. */
1579  if ( transposed == BT_FALSE )
1580  {
1581  /* solve Ra = b */
1582  for( i=(nR-1); i>=0; --i )
1583  {
1584  sum = b[i];
1585  for( j=(i+1); j<nR; ++j )
1586  sum -= RR(i,j) * a[j];
1587 
1588  if ( qpOASES_getAbs( RR(i,i) ) >= QPOASES_ZERO*qpOASES_getAbs( sum ) )
1589  a[i] = sum / RR(i,i);
1590  else
1591  return THROWERROR( RET_DIV_BY_ZERO );
1592  }
1593  }
1594  else
1595  {
1596  /* solve R^T*a = b */
1597  for( i=0; i<nR; ++i )
1598  {
1599  sum = b[i];
1600  for( j=0; j<i; ++j )
1601  sum -= RR(j,i) * a[j];
1602 
1603  if ( qpOASES_getAbs( RR(i,i) ) >= QPOASES_ZERO*qpOASES_getAbs( sum ) )
1604  a[i] = sum / RR(i,i);
1605  else
1606  return THROWERROR( RET_DIV_BY_ZERO );
1607  }
1608  }
1609 
1610  return SUCCESSFUL_RETURN;
1611 }
1612 
1613 
1614 /*
1615  * d e t e r m i n e D a t a S h i f t
1616  */
1617 returnValue QProblemB_determineDataShift( QProblemB* _THIS, const real_t* const g_new, const real_t* const lb_new, const real_t* const ub_new,
1618  real_t* const delta_g, real_t* const delta_lb, real_t* const delta_ub,
1619  BooleanType* Delta_bB_isZero
1620  )
1621 {
1622  int i, ii;
1623  int nV = QProblemB_getNV( _THIS );
1624  int nFX = QProblemB_getNFX( _THIS );
1625 
1626  int* FX_idx;
1627  Indexlist_getNumberArray( Bounds_getFixed( &(_THIS->bounds) ),&FX_idx );
1628 
1629 
1630  /* 1) Calculate shift directions. */
1631  for( i=0; i<nV; ++i )
1632  delta_g[i] = g_new[i] - _THIS->g[i];
1633 
1634  if ( lb_new != 0 )
1635  {
1636  for( i=0; i<nV; ++i )
1637  delta_lb[i] = lb_new[i] - _THIS->lb[i];
1638  }
1639  else
1640  {
1641  /* if no lower bounds exist, assume the new lower bounds to be -infinity */
1642  for( i=0; i<nV; ++i )
1643  delta_lb[i] = -QPOASES_INFTY - _THIS->lb[i];
1644  }
1645 
1646  if ( ub_new != 0 )
1647  {
1648  for( i=0; i<nV; ++i )
1649  delta_ub[i] = ub_new[i] - _THIS->ub[i];
1650  }
1651  else
1652  {
1653  /* if no upper bounds exist, assume the new upper bounds to be infinity */
1654  for( i=0; i<nV; ++i )
1655  delta_ub[i] = QPOASES_INFTY - _THIS->ub[i];
1656  }
1657 
1658  /* 2) Determine if active bounds are to be shifted. */
1659  *Delta_bB_isZero = BT_TRUE;
1660 
1661  for ( i=0; i<nFX; ++i )
1662  {
1663  ii = FX_idx[i];
1664 
1665  if ( ( qpOASES_getAbs( delta_lb[ii] ) > QPOASES_EPS ) || ( qpOASES_getAbs( delta_ub[ii] ) > QPOASES_EPS ) )
1666  {
1667  *Delta_bB_isZero = BT_FALSE;
1668  break;
1669  }
1670  }
1671 
1672  return SUCCESSFUL_RETURN;
1673 }
1674 
1675 
1676 
1677 /*
1678  * s e t u p Q P d a t a
1679  */
1680 returnValue QProblemB_setupQPdataM( QProblemB* _THIS, DenseMatrix *_H, const real_t* const _g,
1681  const real_t* const _lb, const real_t* const _ub
1682  )
1683 {
1684  if ( _H == 0 )
1685  return QProblemB_setupQPdata( _THIS,(real_t*)0,_g,_lb,_ub );
1686  else
1687  return QProblemB_setupQPdata( _THIS,DenseMatrix_getVal(_H),_g,_lb,_ub );
1688 }
1689 
1690 
1691 /*
1692  * s e t u p Q P d a t a
1693  */
1694 returnValue QProblemB_setupQPdata( QProblemB* _THIS, real_t* const _H, const real_t* const _g,
1695  const real_t* const _lb, const real_t* const _ub
1696  )
1697 {
1698  /* 1) Setup Hessian matrix. */
1699  QProblemB_setH( _THIS,_H );
1700 
1701  /* 2) Setup gradient vector. */
1702  if ( _g == 0 )
1703  return THROWERROR( RET_INVALID_ARGUMENTS );
1704  else
1705  QProblemB_setG( _THIS,_g );
1706 
1707  /* 3) Setup lower/upper bounds vector. */
1708  QProblemB_setLB( _THIS,_lb );
1709  QProblemB_setUB( _THIS,_ub );
1710 
1711  return SUCCESSFUL_RETURN;
1712 }
1713 
1714 
1715 /*
1716  * s e t u p Q P d a t a F r o m F i l e
1717  */
1718 returnValue QProblemB_setupQPdataFromFile( QProblemB* _THIS, const char* const H_file, const char* const g_file,
1719  const char* const lb_file, const char* const ub_file
1720  )
1721 {
1722  int i;
1723  int nV = QProblemB_getNV( _THIS );
1724 
1725  returnValue returnvalue;
1726 
1727 
1728  /* 1) Load Hessian matrix from file. */
1729  myStatic real_t _H[NVMAX*NVMAX];
1730 
1731  if ( H_file != 0 )
1732  {
1733  returnvalue = qpOASES_readFromFileM( _H, nV,nV, H_file );
1734  if ( returnvalue != SUCCESSFUL_RETURN )
1735  return THROWERROR( returnvalue );
1736  QProblemB_setH( _THIS,_H );
1737  }
1738  else
1739  {
1740  QProblemB_setH( _THIS,(real_t*)0 );
1741  }
1742 
1743  /* 2) Load gradient vector from file. */
1744  if ( g_file == 0 )
1745  return THROWERROR( RET_INVALID_ARGUMENTS );
1746 
1747  returnvalue = qpOASES_readFromFileV( _THIS->g, nV, g_file );
1748  if ( returnvalue != SUCCESSFUL_RETURN )
1749  return THROWERROR( returnvalue );
1750 
1751  /* 3) Load lower bounds vector from file. */
1752  if ( lb_file != 0 )
1753  {
1754  returnvalue = qpOASES_readFromFileV( _THIS->lb, nV, lb_file );
1755  if ( returnvalue != SUCCESSFUL_RETURN )
1756  return THROWERROR( returnvalue );
1757  }
1758  else
1759  {
1760  /* if no lower bounds are specified, set them to -infinity */
1761  for( i=0; i<nV; ++i )
1762  _THIS->lb[i] = -QPOASES_INFTY;
1763  }
1764 
1765  /* 4) Load upper bounds vector from file. */
1766  if ( ub_file != 0 )
1767  {
1768  returnvalue = qpOASES_readFromFileV( _THIS->ub, nV, ub_file );
1769  if ( returnvalue != SUCCESSFUL_RETURN )
1770  return THROWERROR( returnvalue );
1771  }
1772  else
1773  {
1774  /* if no upper bounds are specified, set them to infinity */
1775  for( i=0; i<nV; ++i )
1776  _THIS->ub[i] = QPOASES_INFTY;
1777  }
1778 
1779  return SUCCESSFUL_RETURN;
1780 }
1781 
1782 
1783 /*
1784  * l o a d Q P v e c t o r s F r o m F i l e
1785  */
1786 returnValue QProblemB_loadQPvectorsFromFile( QProblemB* _THIS, const char* const g_file, const char* const lb_file, const char* const ub_file,
1787  real_t* const g_new, real_t* const lb_new, real_t* const ub_new
1788  )
1789 {
1790  int nV = QProblemB_getNV( _THIS );
1791 
1792  returnValue returnvalue;
1793 
1794 
1795  /* 1) Load gradient vector from file. */
1796  if ( ( g_file != 0 ) && ( g_new != 0 ) )
1797  {
1798  returnvalue = qpOASES_readFromFileV( g_new, nV, g_file );
1799  if ( returnvalue != SUCCESSFUL_RETURN )
1800  return THROWERROR( returnvalue );
1801  }
1802  else
1803  {
1804  /* At least gradient vector needs to be specified! */
1805  return THROWERROR( RET_INVALID_ARGUMENTS );
1806  }
1807 
1808  /* 2) Load lower bounds vector from file. */
1809  if ( lb_file != 0 )
1810  {
1811  if ( lb_new != 0 )
1812  {
1813  returnvalue = qpOASES_readFromFileV( lb_new, nV, lb_file );
1814  if ( returnvalue != SUCCESSFUL_RETURN )
1815  return THROWERROR( returnvalue );
1816  }
1817  else
1818  {
1819  /* If filename is given, storage must be provided! */
1820  return THROWERROR( RET_INVALID_ARGUMENTS );
1821  }
1822  }
1823 
1824  /* 3) Load upper bounds vector from file. */
1825  if ( ub_file != 0 )
1826  {
1827  if ( ub_new != 0 )
1828  {
1829  returnvalue = qpOASES_readFromFileV( ub_new, nV, ub_file );
1830  if ( returnvalue != SUCCESSFUL_RETURN )
1831  return THROWERROR( returnvalue );
1832  }
1833  else
1834  {
1835  /* If filename is given, storage must be provided! */
1836  return THROWERROR( RET_INVALID_ARGUMENTS );
1837  }
1838  }
1839 
1840  return SUCCESSFUL_RETURN;
1841 }
1842 
1843 
1844 /*
1845  * s e t I n f e a s i b i l i t y F l a g
1846  */
1847 returnValue QProblemB_setInfeasibilityFlag( QProblemB* _THIS,
1848  returnValue returnvalue, BooleanType doThrowError
1849  )
1850 {
1851  _THIS->infeasible = BT_TRUE;
1852 
1853  if ( ( doThrowError == BT_TRUE ) || ( _THIS->options.enableFarBounds == BT_FALSE ) )
1854  THROWERROR( returnvalue );
1855 
1856  return returnvalue;
1857 }
1858 
1859 
1860 /*
1861  * a r e B o u n d s C o n s i s t e n t
1862  */
1863 returnValue QProblemB_areBoundsConsistent( QProblemB* _THIS,
1864  const real_t* const lb_new, const real_t* const ub_new )
1865 {
1866  int i;
1867 
1868  if (lb_new && ub_new) {
1869  for (i = 0; i < QProblemB_getNV(_THIS); ++i) {
1870  if (lb_new[i] > ub_new[i]+QPOASES_EPS) {
1871  return RET_QP_INFEASIBLE;
1872  }
1873  }
1874  }
1875  return SUCCESSFUL_RETURN;
1876 }
1877 
1878 
1879 /*
1880  * i s C P U t i m e L i m i t E x c e e d e d
1881  */
1882 BooleanType QProblemB_isCPUtimeLimitExceeded( QProblemB* _THIS, const real_t* const cputime,
1883  real_t starttime,
1884  int nWSR
1885  )
1886 {
1887  real_t elapsedTime, timePerIteration;
1888 
1889  /* Always perform next QP iteration if no CPU time limit is given. */
1890  if ( cputime == 0 )
1891  return BT_FALSE;
1892 
1893  /* Always perform first QP iteration. */
1894  if ( nWSR <= 0 )
1895  return BT_FALSE;
1896 
1897  elapsedTime = qpOASES_getCPUtime( ) - starttime;
1898  timePerIteration = elapsedTime / ((real_t) nWSR);
1899 
1900  /* Determine if next QP iteration exceed CPU time limit
1901  * considering the (current) average CPU time per iteration. */
1902  if ( ( elapsedTime + timePerIteration*1.25 ) <= ( *cputime ) )
1903  return BT_FALSE;
1904  else
1905  return BT_TRUE;
1906 }
1907 
1908 
1909 /*
1910  * r e g u l a r i s e H e s s i a n
1911  */
1912 returnValue QProblemB_regulariseHessian( QProblemB* _THIS )
1913 {
1914  /* Do nothing if Hessian regularisation is disbaled! */
1915  if ( _THIS->options.enableRegularisation == BT_FALSE )
1916  return SUCCESSFUL_RETURN;
1917 
1918  /* Regularisation of identity Hessian not possible. */
1919  if ( _THIS->hessianType == HST_IDENTITY )
1920  return THROWERROR( RET_CANNOT_REGULARISE_IDENTITY );
1921 
1922  /* Determine regularisation parameter. */
1923  if ( QProblemB_usingRegularisation( _THIS ) == BT_TRUE )
1924  return SUCCESSFUL_RETURN; /*THROWERROR( RET_HESSIAN_ALREADY_REGULARISED );*/
1925  else
1926  {
1927  /* Regularisation of zero Hessian is done implicitly. */
1928  if ( _THIS->hessianType == HST_ZERO )
1929  {
1930  _THIS->regVal = qpOASES_getNorm( _THIS->g,QProblemB_getNV( _THIS ),2 ) * _THIS->options.epsRegularisation;
1931  }
1932  else
1933  {
1934  _THIS->regVal = DenseMatrix_getNorm( _THIS->H,2 ) * _THIS->options.epsRegularisation;
1935 
1936  if ( DenseMatrix_addToDiag( _THIS->H,_THIS->regVal ) == RET_NO_DIAGONAL_AVAILABLE )
1937  return THROWERROR( RET_CANNOT_REGULARISE_SPARSE );
1938  }
1939 
1940  THROWINFO( RET_USING_REGULARISATION );
1941  }
1942 
1943  return SUCCESSFUL_RETURN;
1944 }
1945 
1946 
1947 
1948 /*
1949  * p e r f o r m R a t i o T e s t
1950  */
1951 returnValue QProblemB_performRatioTestB( QProblemB* _THIS,
1952  int nIdx,
1953  const int* const idxList,
1954  Bounds* const subjectTo,
1955  const real_t* const num,
1956  const real_t* const den,
1957  real_t epsNum,
1958  real_t epsDen,
1959  real_t* t,
1960  int* BC_idx
1961  )
1962 {
1963  int i, ii;
1964 
1965  *BC_idx = -1;
1966 
1967  for( i=0; i<nIdx; ++i )
1968  {
1969  ii = idxList[i];
1970 
1971  if ( Bounds_getType( subjectTo,ii ) != ST_EQUALITY )
1972  {
1973  if ( ( Bounds_getStatus( subjectTo,ii ) == ST_LOWER ) || ( Bounds_getStatus( subjectTo,ii ) == ST_INACTIVE ) )
1974  {
1975  if ( QProblemB_isBlocking( _THIS,num[i],den[i],epsNum,epsDen,t ) == BT_TRUE )
1976  {
1977  *t = num[i] / den[i];
1978  *BC_idx = ii;
1979  }
1980  }
1981  else
1982  if ( Bounds_getStatus( subjectTo,ii ) == ST_UPPER )
1983  {
1984  if ( QProblemB_isBlocking( _THIS,-num[i],-den[i],epsNum,epsDen,t ) == BT_TRUE )
1985  {
1986  *t = num[i] / den[i];
1987  *BC_idx = ii;
1988  }
1989  }
1990  }
1991  }
1992 
1993  return SUCCESSFUL_RETURN;
1994 }
1995 
1996 
1997 
1998 /*
1999  * g e t R e l a t i v e H o m o t o p y L e n g t h
2000  */
2001 real_t QProblemB_getRelativeHomotopyLength( QProblemB* _THIS,
2002  const real_t* const g_new, const real_t* const lb_new, const real_t* const ub_new
2003  )
2004 {
2005  int i;
2006  int nV = QProblemB_getNV( _THIS );
2007  real_t d, s, len = 0.0;
2008 
2009  /* gradient */
2010  for (i = 0; i < nV; i++)
2011  {
2012  s = qpOASES_getAbs(g_new[i]);
2013  if (s < 1.0) s = 1.0;
2014  d = qpOASES_getAbs(g_new[i] - _THIS->g[i]) / s;
2015  if (d > len) len = d;
2016  }
2017 
2018  /* lower bounds */
2019  if ( lb_new != 0 )
2020  {
2021  for (i = 0; i < nV; i++)
2022  {
2023  s = qpOASES_getAbs(lb_new[i]);
2024  if (s < 1.0) s = 1.0;
2025  d = qpOASES_getAbs(lb_new[i] - _THIS->lb[i]) / s;
2026  if (d > len) len = d;
2027  }
2028  }
2029 
2030  /* upper bounds */
2031  if ( ub_new != 0 )
2032  {
2033  for (i = 0; i < nV; i++)
2034  {
2035  s = qpOASES_getAbs(ub_new[i]);
2036  if (s < 1.0) s = 1.0;
2037  d = qpOASES_getAbs(ub_new[i] - _THIS->ub[i]) / s;
2038  if (d > len) len = d;
2039  }
2040  }
2041 
2042  return len;
2043 }
2044 
2045 
2046 /*
2047  * u p d a t e F a r B o u n d s
2048  */
2049 returnValue QProblemB_updateFarBounds( QProblemB* _THIS,
2050  real_t curFarBound, int nRamp,
2051  const real_t* const lb_new, real_t* const lb_new_far,
2052  const real_t* const ub_new, real_t* const ub_new_far
2053  )
2054 {
2055  int i;
2056  real_t rampVal, t;
2057  int nV = QProblemB_getNV( _THIS );
2058 
2059  if ( _THIS->options.enableRamping == BT_TRUE )
2060  {
2061  for ( i=0; i<nV; ++i )
2062  {
2063  t = (real_t)((i + _THIS->rampOffset) % nRamp) / (real_t)(nRamp-1);
2064  rampVal = curFarBound * (1.0 + (1.0-t)*_THIS->ramp0 + t*_THIS->ramp1);
2065 
2066  if ( lb_new == 0 )
2067  lb_new_far[i] = -rampVal;
2068  else
2069  lb_new_far[i] = qpOASES_getMax( -rampVal,lb_new[i] );
2070 
2071  if ( ub_new == 0 )
2072  ub_new_far[i] = rampVal;
2073  else
2074  ub_new_far[i] = qpOASES_getMin( rampVal,ub_new[i] );
2075  }
2076  }
2077  else
2078  {
2079  for ( i=0; i<nV; ++i )
2080  {
2081  if ( lb_new == 0 )
2082  lb_new_far[i] = -curFarBound;
2083  else
2084  lb_new_far[i] = qpOASES_getMax( -curFarBound,lb_new[i] );
2085 
2086  if ( ub_new == 0 )
2087  ub_new_far[i] = curFarBound;
2088  else
2089  ub_new_far[i] = qpOASES_getMin( curFarBound,ub_new[i] );
2090  }
2091  }
2092 
2093  return SUCCESSFUL_RETURN;
2094 }
2095 
2096 
2097 
2098 /*
2099  * p e r f o r m R a m p i n g
2100  */
2101 returnValue QProblemB_performRamping( QProblemB* _THIS )
2102 {
2103  int nV = QProblemB_getNV( _THIS ), bstat, i;
2104  real_t t, rampVal;
2105 
2106  /* ramp inactive bounds and active dual variables */
2107  for (i = 0; i < nV; i++)
2108  {
2109  switch (Bounds_getType( &(_THIS->bounds),i))
2110  {
2111  case ST_EQUALITY: _THIS->lb[i] = _THIS->x[i]; _THIS->ub[i] = _THIS->x[i]; continue; /* reestablish exact feasibility */
2112  case ST_UNBOUNDED: continue;
2113  case ST_DISABLED: continue;
2114  default: break;
2115  }
2116 
2117  t = (real_t)((i + _THIS->rampOffset) % nV) / (real_t)(nV-1);
2118  rampVal = (1.0-t) * _THIS->ramp0 + t * _THIS->ramp1;
2119  bstat = Bounds_getStatus(&(_THIS->bounds),i);
2120  if (bstat != ST_LOWER) { _THIS->lb[i] = _THIS->x[i] - rampVal; }
2121  if (bstat != ST_UPPER) { _THIS->ub[i] = _THIS->x[i] + rampVal; }
2122  if (bstat == ST_LOWER) { _THIS->lb[i] = _THIS->x[i]; _THIS->y[i] = +rampVal; }
2123  if (bstat == ST_UPPER) { _THIS->ub[i] = _THIS->x[i]; _THIS->y[i] = -rampVal; }
2124  if (bstat == ST_INACTIVE) _THIS->y[i] = 0.0; /* reestablish exact complementarity */
2125  }
2126 
2127  /* reestablish exact stationarity */
2128  QProblemB_setupAuxiliaryQPgradient( _THIS );
2129 
2130  /* advance ramp offset to avoid Ramping cycles */
2131  (_THIS->rampOffset)++;
2132 
2133  return SUCCESSFUL_RETURN;
2134 }
2135 
2136 
2137 
2138 /*****************************************************************************
2139  * P R I V A T E *
2140  *****************************************************************************/
2141 
2142 /*
2143  * s o l v e I n i t i a l Q P
2144  */
2145 returnValue QProblemB_solveInitialQP( QProblemB* _THIS, const real_t* const xOpt, const real_t* const yOpt,
2146  Bounds* const guessedBounds,
2147  const real_t* const _R,
2148  int* nWSR, real_t* const cputime
2149  )
2150 {
2151  int i,j;
2152  int nV = QProblemB_getNV( _THIS );
2153 
2154  myStatic Bounds auxiliaryBounds;
2155 
2156  returnValue returnvalue;
2157 
2158  myStatic real_t g_original[NVMAX];
2159  myStatic real_t lb_original[NVMAX];
2160  myStatic real_t ub_original[NVMAX];
2161 
2162  /* start runtime measurement */
2163  real_t starttime = 0.0;
2164  if ( cputime != 0 )
2165  starttime = qpOASES_getCPUtime( );
2166 
2167  BoundsCON( &auxiliaryBounds,nV );
2168 
2169 
2170  _THIS->status = QPS_NOTINITIALISED;
2171 
2172  /* I) ANALYSE QP DATA: */
2173  /* 1) Check if Hessian happens to be the identity matrix. */
2174  if ( QProblemB_determineHessianType( _THIS ) != SUCCESSFUL_RETURN )
2175  return THROWERROR( RET_INIT_FAILED );
2176 
2177  /* 2) Setup type of bounds (i.e. unbounded, implicitly fixed etc.). */
2178  if ( QProblemB_setupSubjectToType( _THIS ) != SUCCESSFUL_RETURN )
2179  return THROWERROR( RET_INIT_FAILED );
2180 
2181  _THIS->status = QPS_PREPARINGAUXILIARYQP;
2182 
2183 
2184  /* II) SETUP AUXILIARY QP WITH GIVEN OPTIMAL SOLUTION: */
2185  /* 1) Setup bounds data structure. */
2186  if ( Bounds_setupAllFree( &(_THIS->bounds) ) != SUCCESSFUL_RETURN )
2187  return THROWERROR( RET_INIT_FAILED );
2188 
2189  /* 2) Setup optimal primal/dual solution for auxiliary QP. */
2190  if ( QProblemB_setupAuxiliaryQPsolution( _THIS,xOpt,yOpt ) != SUCCESSFUL_RETURN )
2191  return THROWERROR( RET_INIT_FAILED );
2192 
2193  /* 3) Obtain linear independent working set for auxiliary QP. */
2194  if ( QProblemB_obtainAuxiliaryWorkingSet( _THIS,xOpt,yOpt,guessedBounds, &auxiliaryBounds ) != SUCCESSFUL_RETURN )
2195  return THROWERROR( RET_INIT_FAILED );
2196 
2197  /* 4) Setup working set of auxiliary QP and possibly cholesky decomposition. */
2198  /* a) Working set of auxiliary QP. */
2199  if ( QProblemB_setupAuxiliaryWorkingSet( _THIS,&auxiliaryBounds,BT_TRUE ) != SUCCESSFUL_RETURN )
2200  return THROWERROR( RET_INIT_FAILED );
2201 
2202  /* b) Regularise Hessian if necessary. */
2203  if ( ( _THIS->hessianType == HST_ZERO ) || ( _THIS->hessianType == HST_SEMIDEF ) )
2204  {
2205  if ( QProblemB_regulariseHessian( _THIS ) != SUCCESSFUL_RETURN )
2206  return THROWERROR( RET_INIT_FAILED_REGULARISATION );
2207  }
2208 
2209  /* c) Copy external Cholesky factor if provided */
2210  _THIS->haveCholesky = BT_FALSE;
2211 
2212  if ( _R != 0 )
2213  {
2214  if ( _THIS->options.initialStatusBounds != ST_INACTIVE )
2215  {
2216  THROWWARNING( RET_NO_CHOLESKY_WITH_INITIAL_GUESS );
2217  }
2218  else
2219  {
2220  if ( _R == _THIS->R )
2221  {
2222  /* Cholesky factor read from file and already loaded into R. */
2223  _THIS->haveCholesky = BT_TRUE;
2224  }
2225  else if ( ( xOpt == 0 ) && ( yOpt == 0 ) && ( guessedBounds == 0 ) )
2226  {
2227  for( i=0; i<nV; ++i )
2228  for( j=i; j<nV; ++j )
2229  RR(i,j) = _R[i*nV+j];
2230  _THIS->haveCholesky = BT_TRUE;
2231  }
2232  }
2233  }
2234 
2235  /* 5) Store original QP formulation... */
2236  for( i=0; i<nV; ++i )
2237  g_original[i] = _THIS->g[i];
2238  for( i=0; i<nV; ++i )
2239  lb_original[i] = _THIS->lb[i];
2240  for( i=0; i<nV; ++i )
2241  ub_original[i] = _THIS->ub[i];
2242 
2243  /* ... and setup QP data of an auxiliary QP having an optimal solution
2244  * as specified by the user (or xOpt = yOpt = 0, by default). */
2245  if ( QProblemB_setupAuxiliaryQPgradient( _THIS ) != SUCCESSFUL_RETURN )
2246  return THROWERROR( RET_INIT_FAILED );
2247 
2248  if ( QProblemB_setupAuxiliaryQPbounds( _THIS,BT_TRUE ) != SUCCESSFUL_RETURN )
2249  return THROWERROR( RET_INIT_FAILED );
2250 
2251  _THIS->status = QPS_AUXILIARYQPSOLVED;
2252 
2253 
2254  /* III) SOLVE ACTUAL INITIAL QP: */
2255 
2256  /* Allow only remaining CPU time for usual hotstart. */
2257  if ( cputime != 0 )
2258  *cputime -= qpOASES_getCPUtime( ) - starttime;
2259 
2260  /* Use hotstart method to find the solution of the original initial QP,... */
2261  returnvalue = QProblemB_hotstart( _THIS,g_original,lb_original,ub_original, nWSR,cputime );
2262 
2263 
2264  /* ... check for infeasibility and unboundedness... */
2265  if ( QProblemB_isInfeasible( _THIS ) == BT_TRUE )
2266  return THROWERROR( RET_INIT_FAILED_INFEASIBILITY );
2267 
2268  if ( QProblemB_isUnbounded( _THIS ) == BT_TRUE )
2269  return THROWERROR( RET_INIT_FAILED_UNBOUNDEDNESS );
2270 
2271  /* ... and internal errors. */
2272  if ( ( returnvalue != SUCCESSFUL_RETURN ) && ( returnvalue != RET_MAX_NWSR_REACHED ) )
2273  return THROWERROR( RET_INIT_FAILED_HOTSTART );
2274 
2275 
2276  /* stop runtime measurement */
2277  if ( cputime != 0 )
2278  *cputime = qpOASES_getCPUtime( ) - starttime;
2279 
2280  THROWINFO( RET_INIT_SUCCESSFUL );
2281 
2282  return returnvalue;
2283 }
2284 
2285 
2286 /*
2287  * s o l v e Q P
2288  */
2289 returnValue QProblemB_solveQP( QProblemB* _THIS, const real_t* const g_new,
2290  const real_t* const lb_new, const real_t* const ub_new,
2291  int* nWSR, real_t* const cputime, int nWSRperformed,
2292  BooleanType isFirstCall
2293  )
2294 {
2295  int iter;
2296 
2297  /* I) PREPARATIONS */
2298  /* 1) Allocate delta vectors of gradient and bounds,
2299  * index arrays and step direction arrays. */
2300  myStatic real_t delta_xFR[NVMAX];
2301  myStatic real_t delta_xFX[NVMAX];
2302  myStatic real_t delta_yFX[NVMAX];
2303 
2304  myStatic real_t delta_g[NVMAX];
2305  myStatic real_t delta_lb[NVMAX];
2306  myStatic real_t delta_ub[NVMAX];
2307 
2308  returnValue returnvalue;
2309  BooleanType Delta_bB_isZero;
2310 
2311  int BC_idx;
2312  SubjectToStatus BC_status;
2313 
2314  real_t homotopyLength;
2315 
2316  #ifndef __SUPPRESSANYOUTPUT__
2317  myStatic char messageString[QPOASES_MAX_STRING_LENGTH];
2318  #endif
2319 
2320  /* start runtime measurement */
2321  real_t starttime = 0.0;
2322  if ( cputime != 0 )
2323  starttime = qpOASES_getCPUtime( );
2324 
2325  /* consistency check */
2326  if ( ( QProblemB_getStatus( _THIS ) == QPS_NOTINITIALISED ) ||
2327  ( QProblemB_getStatus( _THIS ) == QPS_PREPARINGAUXILIARYQP ) ||
2328  ( QProblemB_getStatus( _THIS ) == QPS_PERFORMINGHOMOTOPY ) )
2329  {
2330  return THROWERROR( RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED );
2331  }
2332 
2333  /* 2) Update type of bounds,e.g. a formerly implicitly fixed
2334  * variable might have become a normal one etc. */
2335  if ( QProblemB_setupSubjectToTypeNew( _THIS,lb_new,ub_new ) != SUCCESSFUL_RETURN )
2336  return THROWERROR( RET_HOTSTART_FAILED );
2337 
2338  /* 3) Reset status flags. */
2339  _THIS->infeasible = BT_FALSE;
2340  _THIS->unbounded = BT_FALSE;
2341 
2342 
2343  /* II) MAIN HOMOTOPY LOOP */
2344  for( iter=nWSRperformed; iter<*nWSR; ++iter )
2345  {
2346  _THIS->tabularOutput.idxAddB = _THIS->tabularOutput.idxRemB = _THIS->tabularOutput.idxAddC = _THIS->tabularOutput.idxRemC = -1;
2347  _THIS->tabularOutput.excAddB = _THIS->tabularOutput.excRemB = _THIS->tabularOutput.excAddC = _THIS->tabularOutput.excRemC = 0;
2348 
2349  if ( QProblemB_isCPUtimeLimitExceeded( _THIS,cputime,starttime,iter-nWSRperformed ) == BT_TRUE )
2350  {
2351  /* Assign number of working set recalculations and stop runtime measurement. */
2352  *nWSR = iter;
2353  if ( cputime != 0 )
2354  *cputime = qpOASES_getCPUtime( ) - starttime;
2355 
2356  break;
2357  }
2358 
2359  _THIS->status = QPS_PERFORMINGHOMOTOPY;
2360 
2361  #ifndef __SUPPRESSANYOUTPUT__
2362  if ( isFirstCall == BT_TRUE )
2363  snprintf( messageString,QPOASES_MAX_STRING_LENGTH,"%d ...",iter );
2364  else
2365  snprintf( messageString,QPOASES_MAX_STRING_LENGTH,"%d* ...",iter );
2366  MessageHandling_throwInfo( qpOASES_getGlobalMessageHandler(),RET_ITERATION_STARTED,messageString,__FUNC__,__FILE__,__LINE__,VS_VISIBLE );
2367  #endif
2368 
2369  /* 2) Initialise shift direction of the gradient and the bounds. */
2370  returnvalue = QProblemB_determineDataShift( _THIS,g_new,lb_new,ub_new,
2371  delta_g,delta_lb,delta_ub,
2372  &Delta_bB_isZero
2373  );
2374  if ( returnvalue != SUCCESSFUL_RETURN )
2375  {
2376  /* Assign number of working set recalculations and stop runtime measurement. */
2377  *nWSR = iter;
2378  if ( cputime != 0 )
2379  *cputime = qpOASES_getCPUtime( ) - starttime;
2380 
2381  THROWERROR( RET_SHIFT_DETERMINATION_FAILED );
2382  return returnvalue;
2383  }
2384 
2385  /* 3) Determination of step direction of X and Y. */
2386  returnvalue = QProblemB_determineStepDirection( _THIS,delta_g,delta_lb,delta_ub,
2387  Delta_bB_isZero,
2388  delta_xFX,delta_xFR,delta_yFX
2389  );
2390  if ( returnvalue != SUCCESSFUL_RETURN )
2391  {
2392  /* Assign number of working set recalculations and stop runtime measurement. */
2393  *nWSR = iter;
2394  if ( cputime != 0 )
2395  *cputime = qpOASES_getCPUtime( ) - starttime;
2396 
2397  THROWERROR( RET_STEPDIRECTION_DETERMINATION_FAILED );
2398  return returnvalue;
2399  }
2400 
2401 
2402  /* 4) Determination of step length TAU.
2403  * This step along the homotopy path is also taken (without changing working set). */
2404  returnvalue = QProblemB_performStep( _THIS,delta_g,delta_lb,delta_ub,
2405  delta_xFX,delta_xFR,delta_yFX,
2406  &BC_idx,&BC_status
2407  );
2408  if ( returnvalue != SUCCESSFUL_RETURN )
2409  {
2410  /* Assign number of working set recalculations and stop runtime measurement. */
2411  *nWSR = iter;
2412  if ( cputime != 0 )
2413  *cputime = qpOASES_getCPUtime( ) - starttime;
2414 
2415  THROWERROR( RET_STEPLENGTH_DETERMINATION_FAILED );
2416  return returnvalue;
2417  }
2418 
2419  /* 5) Termination criterion. */
2420  homotopyLength = QProblemB_getRelativeHomotopyLength( _THIS,g_new, lb_new, ub_new );
2421  if ( homotopyLength <= _THIS->options.terminationTolerance )
2422  {
2423  _THIS->status = QPS_SOLVED;
2424 
2425  THROWINFO( RET_OPTIMAL_SOLUTION_FOUND );
2426 
2427  if ( QProblemB_printIteration( _THIS,iter,BC_idx,BC_status,homotopyLength,isFirstCall ) != SUCCESSFUL_RETURN )
2428  THROWERROR( RET_PRINT_ITERATION_FAILED ); /* do not pass _THIS as return value! */
2429 
2430  *nWSR = iter;
2431 
2432  if ( cputime != 0 )
2433  *cputime = qpOASES_getCPUtime( ) - starttime;
2434 
2435  return SUCCESSFUL_RETURN;
2436  }
2437 
2438 
2439  /* 6) Change active set. */
2440  returnvalue = QProblemB_changeActiveSet( _THIS,BC_idx,BC_status );
2441 
2442  if ( returnvalue != SUCCESSFUL_RETURN )
2443  {
2444  /* Assign number of working set recalculations and stop runtime measurement. */
2445  *nWSR = iter;
2446  if ( cputime != 0 )
2447  *cputime = qpOASES_getCPUtime( ) - starttime;
2448 
2449  /* checks for infeasibility... */
2450  if ( _THIS->infeasible == BT_TRUE )
2451  {
2452  _THIS->status = QPS_HOMOTOPYQPSOLVED;
2453  return QProblemB_setInfeasibilityFlag( _THIS,RET_HOTSTART_STOPPED_INFEASIBILITY,BT_FALSE );
2454  }
2455 
2456  /* ...unboundedness... */
2457  if ( _THIS->unbounded == BT_TRUE ) /* not necessary since objective function convex! */
2458  return THROWERROR( RET_HOTSTART_STOPPED_UNBOUNDEDNESS );
2459 
2460  /* ... and throw unspecific error otherwise */
2461  THROWERROR( RET_HOMOTOPY_STEP_FAILED );
2462  return returnvalue;
2463  }
2464 
2465  /* 6a) Possibly refactorise projected Hessian from scratch. */
2466  if (_THIS->options.enableCholeskyRefactorisation > 0 && iter % _THIS->options.enableCholeskyRefactorisation == 0)
2467  {
2468  returnvalue = QProblemB_computeCholesky( _THIS );
2469  if (returnvalue != SUCCESSFUL_RETURN)
2470  return returnvalue;
2471  }
2472 
2473 
2474  /* 7) Perform Ramping Strategy on zero homotopy step or drift correction (if desired). */
2475  if ( ( _THIS->tau <= QPOASES_EPS ) && ( _THIS->options.enableRamping == BT_TRUE ) )
2476  QProblemB_performRamping( _THIS );
2477  else
2478  if ( (_THIS->options.enableDriftCorrection > 0)
2479  && ((iter+1) % _THIS->options.enableDriftCorrection == 0) )
2480  QProblemB_performDriftCorrection( _THIS ); /* always returns SUCCESSFUL_RETURN */
2481 
2482  /* 8) Output information of successful QP iteration. */
2483  _THIS->status = QPS_HOMOTOPYQPSOLVED;
2484 
2485  if ( QProblemB_printIteration( _THIS,iter,BC_idx,BC_status,homotopyLength,isFirstCall ) != SUCCESSFUL_RETURN )
2486  THROWERROR( RET_PRINT_ITERATION_FAILED ); /* do not pass _THIS as return value! */
2487  }
2488 
2489  /* stop runtime measurement */
2490  if ( cputime != 0 )
2491  *cputime = qpOASES_getCPUtime( ) - starttime;
2492 
2493 
2494  /* if programm gets to here, output information that QP could not be solved
2495  * within the given maximum numbers of working set changes */
2496  if ( _THIS->options.printLevel == PL_HIGH )
2497  {
2498  #ifndef __SUPPRESSANYOUTPUT__
2499  snprintf( messageString,QPOASES_MAX_STRING_LENGTH,"(nWSR = %d)",iter );
2500  return MessageHandling_throwWarning( qpOASES_getGlobalMessageHandler(),RET_MAX_NWSR_REACHED,messageString,__FUNC__,__FILE__,__LINE__,VS_VISIBLE );
2501  #else
2502  return RET_MAX_NWSR_REACHED;
2503  #endif
2504  }
2505  else
2506  {
2507  return RET_MAX_NWSR_REACHED;
2508  }
2509 }
2510 
2511 
2512 /*
2513  * s o l v e R e g u l a r i s e d Q P
2514  */
2515 returnValue QProblemB_solveRegularisedQP( QProblemB* _THIS, const real_t* const g_new,
2516  const real_t* const lb_new, const real_t* const ub_new,
2517  int* nWSR, real_t* const cputime, int nWSRperformed,
2518  BooleanType isFirstCall
2519  )
2520 {
2521  int i, step;
2522  int nV = QProblemB_getNV( _THIS );
2523 
2524  returnValue returnvalue;
2525 
2526  int nWSR_max = *nWSR;
2527  int nWSR_total = nWSRperformed;
2528 
2529  real_t cputime_total = 0.0;
2530  real_t cputime_cur = 0.0;
2531 
2532  myStatic real_t gMod[NVMAX];
2533 
2534 
2535  /* Perform normal QP solution if QP has not been regularised. */
2536  if ( QProblemB_usingRegularisation( _THIS ) == BT_FALSE )
2537  return QProblemB_solveQP( _THIS,g_new,lb_new,ub_new, nWSR,cputime,nWSRperformed,isFirstCall );
2538 
2539 
2540  /* I) SOLVE USUAL REGULARISED QP */
2541  if ( cputime == 0 )
2542  {
2543  returnvalue = QProblemB_solveQP( _THIS,g_new,lb_new,ub_new, nWSR,0,nWSRperformed,isFirstCall );
2544  }
2545  else
2546  {
2547  cputime_cur = *cputime;
2548  returnvalue = QProblemB_solveQP( _THIS,g_new,lb_new,ub_new, nWSR,&cputime_cur,nWSRperformed,isFirstCall );
2549  }
2550  nWSR_total = *nWSR;
2551  cputime_total += cputime_cur;
2552  isFirstCall = BT_FALSE;
2553 
2554 
2555  /* Only continue if QP solution has been successful. */
2556  if ( returnvalue != SUCCESSFUL_RETURN )
2557  {
2558  if ( cputime != 0 )
2559  *cputime = cputime_total;
2560 
2561  if ( returnvalue == RET_MAX_NWSR_REACHED )
2562  THROWWARNING( RET_NO_REGSTEP_NWSR );
2563 
2564  return returnvalue;
2565  }
2566 
2567 
2568  /* II) PERFORM SUCCESSIVE REGULARISATION STEPS */
2569  for( step=0; step<_THIS->options.numRegularisationSteps; ++step )
2570  {
2571  /* 1) Modify gradient: gMod = g - eps*xOpt
2572  * (assuming regularisation matrix to be regVal*Id). */
2573  for( i=0; i<nV; ++i )
2574  gMod[i] = g_new[i] - _THIS->regVal * _THIS->x[i];
2575 
2576  /* 2) Solve regularised QP with modified gradient allowing
2577  * only as many working set recalculations and CPU time
2578  * as have been left from previous QP solutions. */
2579  if ( cputime == 0 )
2580  {
2581  *nWSR = nWSR_max;
2582  returnvalue = QProblemB_solveQP( _THIS,gMod,lb_new,ub_new, nWSR,0,nWSR_total,isFirstCall );
2583  }
2584  else
2585  {
2586  *nWSR = nWSR_max;
2587  cputime_cur = *cputime - cputime_total;
2588  returnvalue = QProblemB_solveQP( _THIS,gMod,lb_new,ub_new, nWSR,&cputime_cur,nWSR_total,isFirstCall );
2589  }
2590 
2591  nWSR_total = *nWSR;
2592  cputime_total += cputime_cur;
2593 
2594  /* Only continue if QP solution has been successful. */
2595  if ( returnvalue != SUCCESSFUL_RETURN )
2596  {
2597  if ( cputime != 0 )
2598  *cputime = cputime_total;
2599 
2600  if ( returnvalue == RET_MAX_NWSR_REACHED )
2601  THROWWARNING( RET_FEWER_REGSTEPS_NWSR );
2602 
2603  return returnvalue;
2604  }
2605  }
2606 
2607  for( i=0; i<nV; ++i )
2608  _THIS->g[i] = g_new[i];
2609 
2610  if ( cputime != 0 )
2611  *cputime = cputime_total;
2612 
2613  return SUCCESSFUL_RETURN;
2614 }
2615 
2616 
2617 /*
2618  * s e t u p A u x i l i a r y W o r k i n g S e t
2619  */
2620 returnValue QProblemB_setupAuxiliaryWorkingSet( QProblemB* _THIS,
2621  Bounds* const auxiliaryBounds,
2622  BooleanType setupAfresh
2623  )
2624 {
2625  int i;
2626  int nV = QProblemB_getNV( _THIS );
2627 
2628  BooleanType updateCholesky;
2629 
2630  /* consistency checks */
2631  if ( auxiliaryBounds != 0 )
2632  {
2633  for( i=0; i<nV; ++i )
2634  if ( ( Bounds_getStatus(&(_THIS->bounds),i ) == ST_UNDEFINED ) || ( Bounds_getStatus( auxiliaryBounds,i ) == ST_UNDEFINED ) )
2635  return THROWERROR( RET_UNKNOWN_BUG );
2636  }
2637  else
2638  {
2639  return THROWERROR( RET_INVALID_ARGUMENTS );
2640  }
2641 
2642 
2643  /* I) SETUP CHOLESKY FLAG:
2644  * Cholesky decomposition shall only be updated if working set
2645  * shall be updated (i.e. NOT setup afresh!) */
2646  if ( setupAfresh == BT_TRUE )
2647  updateCholesky = BT_FALSE;
2648  else
2649  updateCholesky = BT_TRUE;
2650 
2651 
2652  /* II) REMOVE FORMERLY ACTIVE BOUNDS (IF NECESSARY): */
2653  if ( setupAfresh == BT_FALSE )
2654  {
2655  /* Remove all active bounds that shall be inactive AND
2656  * all active bounds that are active at the wrong bound. */
2657  for( i=0; i<nV; ++i )
2658  {
2659  if ( ( Bounds_getStatus(&(_THIS->bounds),i ) == ST_LOWER ) && ( Bounds_getStatus( auxiliaryBounds,i ) != ST_LOWER ) )
2660  if ( QProblemB_removeBound( _THIS,i,updateCholesky ) != SUCCESSFUL_RETURN )
2661  return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
2662 
2663  if ( ( Bounds_getStatus(&(_THIS->bounds),i ) == ST_UPPER ) && ( Bounds_getStatus( auxiliaryBounds,i ) != ST_UPPER ) )
2664  if ( QProblemB_removeBound( _THIS,i,updateCholesky ) != SUCCESSFUL_RETURN )
2665  return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
2666  }
2667  }
2668 
2669 
2670  /* III) ADD NEWLY ACTIVE BOUNDS: */
2671  /* Add all inactive bounds that shall be active AND
2672  * all formerly active bounds that have been active at the wrong bound. */
2673  for( i=0; i<nV; ++i )
2674  {
2675  if ( ( Bounds_getStatus(&(_THIS->bounds),i ) == ST_INACTIVE ) && ( Bounds_getStatus( auxiliaryBounds,i ) != ST_INACTIVE ) )
2676  {
2677  if ( QProblemB_addBound( _THIS,i,Bounds_getStatus( auxiliaryBounds,i ),updateCholesky ) != SUCCESSFUL_RETURN )
2678  return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
2679  }
2680  }
2681 
2682  return SUCCESSFUL_RETURN;
2683 }
2684 
2685 
2686 /*
2687  * s e t u p A u x i l i a r y Q P s o l u t i o n
2688  */
2689 returnValue QProblemB_setupAuxiliaryQPsolution( QProblemB* _THIS, const real_t* const xOpt, const real_t* const yOpt
2690  )
2691 {
2692  int i;
2693  int nV = QProblemB_getNV( _THIS );
2694 
2695 
2696  /* Setup primal/dual solution vectors for auxiliary initial QP:
2697  * if a null pointer is passed, a zero vector is assigned;
2698  * old solution vector is kept if pointer to internal solution vector is passed. */
2699  if ( xOpt != 0 )
2700  {
2701  if ( xOpt != _THIS->x )
2702  for( i=0; i<nV; ++i )
2703  _THIS->x[i] = xOpt[i];
2704  }
2705  else
2706  {
2707  for( i=0; i<nV; ++i )
2708  _THIS->x[i] = 0.0;
2709  }
2710 
2711  if ( yOpt != 0 )
2712  {
2713  if ( yOpt != _THIS->y )
2714  for( i=0; i<nV; ++i )
2715  _THIS->y[i] = yOpt[i];
2716  }
2717  else
2718  {
2719  for( i=0; i<nV; ++i )
2720  _THIS->y[i] = 0.0;
2721  }
2722 
2723  return SUCCESSFUL_RETURN;
2724 }
2725 
2726 
2727 /*
2728  * s e t u p A u x i l i a r y Q P g r a d i e n t
2729  */
2730 returnValue QProblemB_setupAuxiliaryQPgradient( QProblemB* _THIS )
2731 {
2732  int i;
2733  int nV = QProblemB_getNV( _THIS );
2734 
2735  /* Setup gradient vector: g = -H*x + y'*Id. */
2736  switch ( _THIS->hessianType )
2737  {
2738  case HST_ZERO:
2739  if ( QProblemB_usingRegularisation( _THIS ) == BT_FALSE )
2740  for ( i=0; i<nV; ++i )
2741  _THIS->g[i] = _THIS->y[i];
2742  else
2743  for ( i=0; i<nV; ++i )
2744  _THIS->g[i] = _THIS->y[i] - _THIS->regVal * _THIS->x[i];
2745  break;
2746 
2747  case HST_IDENTITY:
2748  for ( i=0; i<nV; ++i )
2749  _THIS->g[i] = _THIS->y[i] - _THIS->x[i];
2750  break;
2751 
2752  default:
2753  /* y'*Id */
2754  for ( i=0; i<nV; ++i )
2755  _THIS->g[i] = _THIS->y[i];
2756 
2757  /* -H*x */
2758  DenseMatrix_times(_THIS->H,1, -1.0, _THIS->x, nV, 1.0, _THIS->g, nV);
2759 
2760  break;
2761  }
2762 
2763  return SUCCESSFUL_RETURN;
2764 }
2765 
2766 
2767 /*
2768  * s e t u p A u x i l i a r y Q P b o u n d s
2769  */
2770 returnValue QProblemB_setupAuxiliaryQPbounds( QProblemB* _THIS, BooleanType useRelaxation )
2771 {
2772  int i;
2773  int nV = QProblemB_getNV( _THIS );
2774 
2775 
2776  /* Setup bound vectors. */
2777  for ( i=0; i<nV; ++i )
2778  {
2779  switch ( Bounds_getStatus(&(_THIS->bounds),i ) )
2780  {
2781  case ST_INACTIVE:
2782  if ( useRelaxation == BT_TRUE )
2783  {
2784  if ( Bounds_getType( &(_THIS->bounds),i ) == ST_EQUALITY )
2785  {
2786  _THIS->lb[i] = _THIS->x[i];
2787  _THIS->ub[i] = _THIS->x[i];
2788  }
2789  else
2790  {
2791  _THIS->lb[i] = _THIS->x[i] - _THIS->options.boundRelaxation;
2792  _THIS->ub[i] = _THIS->x[i] + _THIS->options.boundRelaxation;
2793  }
2794  }
2795  break;
2796 
2797  case ST_LOWER:
2798  _THIS->lb[i] = _THIS->x[i];
2799  if ( Bounds_getType( &(_THIS->bounds),i ) == ST_EQUALITY )
2800  {
2801  _THIS->ub[i] = _THIS->x[i];
2802  }
2803  else
2804  {
2805  if ( useRelaxation == BT_TRUE )
2806  _THIS->ub[i] = _THIS->x[i] + _THIS->options.boundRelaxation;
2807  }
2808  break;
2809 
2810  case ST_UPPER:
2811  _THIS->ub[i] = _THIS->x[i];
2812  if ( Bounds_getType( &(_THIS->bounds),i ) == ST_EQUALITY )
2813  {
2814  _THIS->lb[i] = _THIS->x[i];
2815  }
2816  else
2817  {
2818  if ( useRelaxation == BT_TRUE )
2819  _THIS->lb[i] = _THIS->x[i] - _THIS->options.boundRelaxation;
2820  }
2821  break;
2822 
2823  case ST_DISABLED:
2824  break;
2825 
2826  default:
2827  return THROWERROR( RET_UNKNOWN_BUG );
2828  }
2829  }
2830 
2831  return SUCCESSFUL_RETURN;
2832 }
2833 
2834 
2835 /*
2836  * s e t u p A u x i l i a r y Q P
2837  */
2838 returnValue QProblemB_setupAuxiliaryQP( QProblemB* _THIS, Bounds* const guessedBounds )
2839 {
2840  int i;
2841  int nV = QProblemB_getNV( _THIS );
2842 
2843  /* nothing to do */
2844  if ( guessedBounds == &(_THIS->bounds) )
2845  return SUCCESSFUL_RETURN;
2846 
2847  _THIS->status = QPS_PREPARINGAUXILIARYQP;
2848 
2849 
2850  /* I) SETUP WORKING SET ... */
2851  if ( QProblemB_shallRefactorise( _THIS,guessedBounds ) == BT_TRUE )
2852  {
2853  /* ... WITH REFACTORISATION: */
2854  /* 1) Reset bounds ... */
2855  Bounds_init( &(_THIS->bounds),nV );
2856 
2857  /* ... and set them up afresh. */
2858  if ( QProblemB_setupSubjectToType( _THIS ) != SUCCESSFUL_RETURN )
2859  return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
2860 
2861  if ( Bounds_setupAllFree( &(_THIS->bounds) ) != SUCCESSFUL_RETURN )
2862  return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
2863 
2864  /* 2) Setup guessed working set afresh. */
2865  if ( QProblemB_setupAuxiliaryWorkingSet( _THIS,guessedBounds,BT_TRUE ) != SUCCESSFUL_RETURN )
2866  THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
2867 
2868  /* 3) Calculate Cholesky decomposition. */
2869  if ( QProblemB_computeCholesky( _THIS ) != SUCCESSFUL_RETURN )
2870  return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
2871  }
2872  else
2873  {
2874  /* ... WITHOUT REFACTORISATION: */
2875  if ( QProblemB_setupAuxiliaryWorkingSet( _THIS,guessedBounds,BT_FALSE ) != SUCCESSFUL_RETURN )
2876  THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
2877  }
2878 
2879 
2880  /* II) SETUP AUXILIARY QP DATA: */
2881  /* 1) Ensure that dual variable is zero for free bounds. */
2882  for ( i=0; i<nV; ++i )
2883  if ( Bounds_getStatus(&(_THIS->bounds),i ) == ST_INACTIVE )
2884  _THIS->y[i] = 0.0;
2885 
2886  /* 2) Setup gradient and bound vectors. */
2887  if ( QProblemB_setupAuxiliaryQPgradient( _THIS ) != SUCCESSFUL_RETURN )
2888  THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
2889 
2890  if ( QProblemB_setupAuxiliaryQPbounds( _THIS,BT_FALSE ) != SUCCESSFUL_RETURN )
2891  THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
2892 
2893  return SUCCESSFUL_RETURN;
2894 }
2895 
2896 
2897 /*
2898  * d e t e r m i n e S t e p D i r e c t i o n
2899  */
2900 returnValue QProblemB_determineStepDirection( QProblemB* _THIS,
2901  const real_t* const delta_g, const real_t* const delta_lb, const real_t* const delta_ub,
2902  BooleanType Delta_bB_isZero,
2903  real_t* const delta_xFX, real_t* const delta_xFR,
2904  real_t* const delta_yFX
2905  )
2906 {
2907  int i, ii;
2908  int r;
2909  int nFR = QProblemB_getNFR( _THIS );
2910  int nFX = QProblemB_getNFX( _THIS );
2911 
2912  int* FR_idx;
2913  int* FX_idx;
2914 
2915  real_t rnrm;
2916 
2917  Indexlist_getNumberArray( Bounds_getFree( &(_THIS->bounds) ),&FR_idx );
2918  Indexlist_getNumberArray( Bounds_getFixed( &(_THIS->bounds) ),&FX_idx );
2919 
2920  /* This routine computes
2921  * delta_xFX := delta_b
2922  * delta_xFR := R \ R' \ -( delta_g + HMX*delta_xFX )
2923  * delta_yFX := HMX'*delta_xFR + HFX*delta_xFX { + eps*delta_xFX }
2924  */
2925 
2926  /* I) DETERMINE delta_xFX := delta_{l|u}b */
2927  if ( Delta_bB_isZero == BT_FALSE )
2928  {
2929  for( i=0; i<nFX; ++i )
2930  {
2931  ii = FX_idx[i];
2932 
2933  if ( Bounds_getStatus(&(_THIS->bounds),ii ) == ST_LOWER )
2934  delta_xFX[i] = delta_lb[ii];
2935  else
2936  delta_xFX[i] = delta_ub[ii];
2937  }
2938  }
2939  else
2940  {
2941  for( i=0; i<nFX; ++i )
2942  delta_xFX[i] = 0.0;
2943  }
2944 
2945 
2946  /* delta_xFR_TMP holds the residual, initialized with right hand side
2947  * delta_xFR holds the step that gets refined incrementally */
2948  for ( i=0; i<nFR; ++i )
2949  {
2950  ii = FR_idx[i];
2951  _THIS->delta_xFR_TMP[i] = - delta_g[ii];
2952  delta_xFR[i] = 0.0;
2953  }
2954 
2955 
2956  /* Iterative refinement loop for delta_xFR */
2957  for ( r=0; r<=_THIS->options.numRefinementSteps; ++r )
2958  {
2959  /* II) DETERMINE delta_xFR */
2960  if ( nFR > 0 )
2961  {
2962  /* Add - HMX*delta_xFX
2963  * This is skipped if delta_b=0 or mixed part HM=0 (H=0 or H=Id) */
2964  if ( ( _THIS->hessianType != HST_ZERO ) && ( _THIS->hessianType != HST_IDENTITY ) && ( Delta_bB_isZero == BT_FALSE ) && ( r == 0 ) )
2965  DenseMatrix_subTimes(_THIS->H,Bounds_getFree( &(_THIS->bounds) ), Bounds_getFixed( &(_THIS->bounds) ), 1, -1.0, delta_xFX, nFX, 1.0, _THIS->delta_xFR_TMP, nFR, BT_TRUE);
2966 
2967  /* Determine R' \ ( - HMX*delta_xFX - delta_gFR ) where R'R = HFR */
2968  if ( QProblemB_backsolveR( _THIS,_THIS->delta_xFR_TMP,BT_TRUE,_THIS->delta_xFR_TMP ) != SUCCESSFUL_RETURN )
2969  return THROWERROR( RET_STEPDIRECTION_FAILED_CHOLESKY );
2970 
2971  /* Determine HFR \ ( - HMX*delta_xFX - delta_gFR ) */
2972  if ( QProblemB_backsolveR( _THIS,_THIS->delta_xFR_TMP,BT_FALSE,_THIS->delta_xFR_TMP ) != SUCCESSFUL_RETURN )
2973  return THROWERROR( RET_STEPDIRECTION_FAILED_CHOLESKY );
2974  }
2975 
2976  /* refine solution found for delta_xFR so far */
2977  for ( i=0; i<nFR; ++i )
2978  delta_xFR[i] += _THIS->delta_xFR_TMP[i];
2979 
2980  if ( _THIS->options.numRefinementSteps > 0 )
2981  {
2982  rnrm = 0.0;
2983  /* compute new residual in delta_xFR_TMP:
2984  * residual := - HFR*delta_xFR - HMX*delta_xFX - delta_gFR
2985  * set to -delta_gFR */
2986  for ( i=0; i<nFR; ++i )
2987  {
2988  ii = FR_idx[i];
2989  _THIS->delta_xFR_TMP[i] = -delta_g[ii];
2990  }
2991  /* add - HFR*delta_xFR */
2992  switch ( _THIS->hessianType )
2993  {
2994  case HST_ZERO:
2995  break;
2996 
2997  case HST_IDENTITY:
2998  for ( i=0; i<nFR; ++i )
2999  {
3000  _THIS->delta_xFR_TMP[i] -= delta_xFR[i];
3001 
3002  /* compute max norm */
3003  if (rnrm < qpOASES_getAbs (_THIS->delta_xFR_TMP[i]))
3004  rnrm = qpOASES_getAbs (_THIS->delta_xFR_TMP[i]);
3005  }
3006  break;
3007 
3008  default:
3009  DenseMatrix_subTimes(_THIS->H,Bounds_getFree( &(_THIS->bounds) ), Bounds_getFree( &(_THIS->bounds) ), 1, -1.0, delta_xFR, nFR, 1.0, _THIS->delta_xFR_TMP, nFR, BT_TRUE);
3010  DenseMatrix_subTimes(_THIS->H,Bounds_getFree( &(_THIS->bounds) ), Bounds_getFixed( &(_THIS->bounds) ), 1, -1.0, delta_xFX, nFX, 1.0, _THIS->delta_xFR_TMP, nFR, BT_TRUE);
3011 
3012  /* compute max norm */
3013  for ( i=0; i<nFR; ++i )
3014  if (rnrm < qpOASES_getAbs (_THIS->delta_xFR_TMP[i]))
3015  rnrm = qpOASES_getAbs (_THIS->delta_xFR_TMP[i]);
3016 
3017  break;
3018  }
3019 
3020  /* early termination of residual norm small enough */
3021  if ( rnrm < _THIS->options.epsIterRef )
3022  break;
3023  }
3024 
3025  } /* end of refinement loop for delta_xFR */
3026 
3027  /* III) DETERMINE delta_yFX */
3028  if ( nFX > 0 )
3029  {
3030  if ( ( _THIS->hessianType == HST_ZERO ) || ( _THIS->hessianType == HST_IDENTITY ) )
3031  {
3032  for( i=0; i<nFX; ++i )
3033  {
3034  /* set to delta_g */
3035  ii = FX_idx[i];
3036  delta_yFX[i] = delta_g[ii];
3037 
3038  /* add HFX*delta_xFX = {0|I}*delta_xFX */
3039  if ( _THIS->hessianType == HST_ZERO )
3040  {
3041  if ( QProblemB_usingRegularisation( _THIS ) == BT_TRUE )
3042  delta_yFX[i] += _THIS->regVal*delta_xFX[i];
3043  }
3044  else
3045  delta_yFX[i] += delta_xFX[i];
3046  }
3047  }
3048  else
3049  {
3050  for( i=0; i<nFX; ++i )
3051  {
3052  /* set to delta_g */
3053  ii = FX_idx[i];
3054  delta_yFX[i] = delta_g[ii];
3055  }
3056  DenseMatrix_subTimes(_THIS->H,Bounds_getFixed( &(_THIS->bounds) ), Bounds_getFree( &(_THIS->bounds) ), 1, 1.0, delta_xFR, nFR, 1.0, delta_yFX, nFX, BT_TRUE);
3057  if (Delta_bB_isZero == BT_FALSE)
3058  DenseMatrix_subTimes(_THIS->H,Bounds_getFixed( &(_THIS->bounds) ), Bounds_getFixed( &(_THIS->bounds) ), 1, 1.0, delta_xFX, nFX, 1.0, delta_yFX, nFX, BT_TRUE);
3059  }
3060  }
3061 
3062  return SUCCESSFUL_RETURN;
3063 }
3064 
3065 
3066 /*
3067  * p e r f o r m S t e p
3068  */
3069 returnValue QProblemB_performStep( QProblemB* _THIS,
3070  const real_t* const delta_g,
3071  const real_t* const delta_lb, const real_t* const delta_ub,
3072  const real_t* const delta_xFX,
3073  const real_t* const delta_xFR,
3074  const real_t* const delta_yFX,
3075  int* BC_idx, SubjectToStatus* BC_status
3076  )
3077 {
3078  int i, ii;
3079  int nV = QProblemB_getNV( _THIS );
3080  int nFR = QProblemB_getNFR( _THIS );
3081  int nFX = QProblemB_getNFX( _THIS );
3082 
3083  myStatic char messageString[QPOASES_MAX_STRING_LENGTH];
3084 
3085  int BC_idx_tmp = -1;
3086 
3087  myStatic real_t num[NVMAX];
3088  myStatic real_t den[NVMAX];
3089 
3090  int* FR_idx;
3091  int* FX_idx;
3092 
3093  _THIS->tau = 1.0;
3094  *BC_idx = -1;
3095  *BC_status = ST_UNDEFINED;
3096 
3097  Indexlist_getNumberArray( Bounds_getFree( &(_THIS->bounds) ),&FR_idx );
3098  Indexlist_getNumberArray( Bounds_getFixed( &(_THIS->bounds) ),&FX_idx );
3099 
3100 
3101  /* I) DETERMINE MAXIMUM DUAL STEPLENGTH, i.e. ensure that
3102  * active dual bounds remain valid (ignoring implicitly fixed variables): */
3103  for( i=0; i<nFX; ++i )
3104  {
3105  ii = FX_idx[i];
3106  num[i] = _THIS->y[ii];
3107  den[i] = -delta_yFX[i];
3108  }
3109 
3110  QProblemB_performRatioTestB( _THIS,nFX,FX_idx,&(_THIS->bounds),num,den, _THIS->options.epsNum,_THIS->options.epsDen, &(_THIS->tau),&BC_idx_tmp );
3111 
3112  if ( BC_idx_tmp >= 0 )
3113  {
3114  *BC_idx = BC_idx_tmp;
3115  *BC_status = ST_INACTIVE;
3116  }
3117 
3118 
3119  /* II) DETERMINE MAXIMUM PRIMAL STEPLENGTH, i.e. ensure that
3120  * inactive bounds remain valid (ignoring unbounded variables). */
3121  /* 1) Inactive lower bounds. */
3122  if ( Bounds_hasNoLower( &(_THIS->bounds) ) == BT_FALSE )
3123  {
3124  for( i=0; i<nFR; ++i )
3125  {
3126  ii = FR_idx[i];
3127  num[i] = qpOASES_getMax( _THIS->x[ii] - _THIS->lb[ii],0.0 );
3128  den[i] = delta_lb[ii] - delta_xFR[i];
3129  }
3130 
3131  QProblemB_performRatioTestB( _THIS,nFR,FR_idx,&(_THIS->bounds),num,den, _THIS->options.epsNum,_THIS->options.epsDen, &(_THIS->tau),&BC_idx_tmp );
3132 
3133  if ( BC_idx_tmp >= 0 )
3134  {
3135  *BC_idx = BC_idx_tmp;
3136  *BC_status = ST_LOWER;
3137  }
3138  }
3139 
3140  /* 2) Inactive upper bounds. */
3141  if ( Bounds_hasNoUpper( &(_THIS->bounds) ) == BT_FALSE )
3142  {
3143  for( i=0; i<nFR; ++i )
3144  {
3145  ii = FR_idx[i];
3146  num[i] = qpOASES_getMax( _THIS->ub[ii] - _THIS->x[ii],0.0 );
3147  den[i] = delta_xFR[i] - delta_ub[ii];
3148  }
3149 
3150  QProblemB_performRatioTestB( _THIS,nFR,FR_idx,&(_THIS->bounds),num,den, _THIS->options.epsNum,_THIS->options.epsDen, &(_THIS->tau),&BC_idx_tmp );
3151 
3152  if ( BC_idx_tmp >= 0 )
3153  {
3154  *BC_idx = BC_idx_tmp;
3155  *BC_status = ST_UPPER;
3156  }
3157  }
3158 
3159 
3160  #ifndef __SUPPRESSANYOUTPUT__
3161  if ( *BC_status == ST_UNDEFINED )
3162  snprintf( messageString,QPOASES_MAX_STRING_LENGTH,"Stepsize is %.15e!",_THIS->tau );
3163  else
3164  snprintf( messageString,QPOASES_MAX_STRING_LENGTH,"Stepsize is %.15e! (idx = %d, status = %d)",_THIS->tau,*BC_idx,*BC_status );
3165 
3166  MessageHandling_throwInfo( qpOASES_getGlobalMessageHandler(),RET_STEPSIZE_NONPOSITIVE,messageString,__FUNC__,__FILE__,__LINE__,VS_VISIBLE );
3167  #endif
3168 
3169 
3170  /* III) PERFORM STEP ALONG HOMOTOPY PATH */
3171  if ( _THIS->tau > QPOASES_ZERO )
3172  {
3173  /* 1) Perform step in primal und dual space. */
3174  for( i=0; i<nFR; ++i )
3175  {
3176  ii = FR_idx[i];
3177  _THIS->x[ii] += _THIS->tau * delta_xFR[i];
3178  }
3179 
3180  for( i=0; i<nFX; ++i )
3181  {
3182  ii = FX_idx[i];
3183  _THIS->x[ii] += _THIS->tau * delta_xFX[i];
3184  _THIS->y[ii] += _THIS->tau * delta_yFX[i];
3185  }
3186 
3187  /* 2) Shift QP data. */
3188  for( i=0; i<nV; ++i )
3189  {
3190  _THIS->g[i] += _THIS->tau * delta_g[i];
3191  _THIS->lb[i] += _THIS->tau * delta_lb[i];
3192  _THIS->ub[i] += _THIS->tau * delta_ub[i];
3193  }
3194  }
3195  else
3196  {
3197  /* print a warning if stepsize is zero */
3198  #ifndef __SUPPRESSANYOUTPUT__
3199  snprintf( messageString,QPOASES_MAX_STRING_LENGTH,"Stepsize is %.15e",_THIS->tau );
3200  MessageHandling_throwWarning( qpOASES_getGlobalMessageHandler(),RET_STEPSIZE,messageString,__FUNC__,__FILE__,__LINE__,VS_VISIBLE );
3201  #endif
3202  }
3203 
3204 
3205  return SUCCESSFUL_RETURN;
3206 }
3207 
3208 
3209 /*
3210  * c h a n g e A c t i v e S e t
3211  */
3212 returnValue QProblemB_changeActiveSet( QProblemB* _THIS, int BC_idx, SubjectToStatus BC_status )
3213 {
3214  #ifndef __SUPPRESSANYOUTPUT__
3215  myStatic char messageString[QPOASES_MAX_STRING_LENGTH];
3216  #endif
3217 
3218  /* IV) UPDATE ACTIVE SET */
3219  switch ( BC_status )
3220  {
3221  /* Optimal solution found as no working set change detected. */
3222  case ST_UNDEFINED:
3223  return RET_OPTIMAL_SOLUTION_FOUND;
3224 
3225 
3226  /* Remove one variable from active set. */
3227  case ST_INACTIVE:
3228  #ifndef __SUPPRESSANYOUTPUT__
3229  snprintf( messageString,QPOASES_MAX_STRING_LENGTH,"bound no. %d.", BC_idx );
3230  MessageHandling_throwInfo( qpOASES_getGlobalMessageHandler(),RET_REMOVE_FROM_ACTIVESET,messageString,__FUNC__,__FILE__,__LINE__,VS_VISIBLE );
3231  #endif
3232 
3233  if ( QProblemB_removeBound( _THIS,BC_idx,BT_TRUE ) != SUCCESSFUL_RETURN )
3234  return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );
3235 
3236  _THIS->y[BC_idx] = 0.0;
3237  break;
3238 
3239 
3240  /* Add one variable to active set. */
3241  default:
3242  #ifndef __SUPPRESSANYOUTPUT__
3243  if ( BC_status == ST_LOWER )
3244  snprintf( messageString,QPOASES_MAX_STRING_LENGTH,"lower bound no. %d.", BC_idx );
3245  else
3246  snprintf( messageString,QPOASES_MAX_STRING_LENGTH,"upper bound no. %d.", BC_idx );
3247  MessageHandling_throwInfo( qpOASES_getGlobalMessageHandler(),RET_ADD_TO_ACTIVESET,messageString,__FUNC__,__FILE__,__LINE__,VS_VISIBLE );
3248  #endif
3249 
3250  if ( QProblemB_addBound( _THIS,BC_idx,BC_status,BT_TRUE ) != SUCCESSFUL_RETURN )
3251  return THROWERROR( RET_ADD_TO_ACTIVESET_FAILED );
3252  break;
3253  }
3254 
3255  return SUCCESSFUL_RETURN;
3256 }
3257 
3258 
3259 
3260 /*
3261  * p e r f o r m D r i f t C o r r e c t i o n
3262  */
3263 returnValue QProblemB_performDriftCorrection( QProblemB* _THIS )
3264 {
3265  int i;
3266  int nV = QProblemB_getNV( _THIS );
3267 
3268  for ( i=0; i<nV; ++i )
3269  {
3270  switch ( Bounds_getType ( &(_THIS->bounds),i ) )
3271  {
3272  case ST_BOUNDED:
3273  switch ( Bounds_getStatus( &(_THIS->bounds),i ) )
3274  {
3275  case ST_LOWER:
3276  _THIS->lb[i] = _THIS->x[i];
3277  _THIS->ub[i] = qpOASES_getMax (_THIS->ub[i], _THIS->x[i]);
3278  _THIS->y[i] = qpOASES_getMax (_THIS->y[i], 0.0);
3279  break;
3280  case ST_UPPER:
3281  _THIS->lb[i] = qpOASES_getMin (_THIS->lb[i], _THIS->x[i]);
3282  _THIS->ub[i] = _THIS->x[i];
3283  _THIS->y[i] = qpOASES_getMin (_THIS->y[i], 0.0);
3284  break;
3285  case ST_INACTIVE:
3286  _THIS->lb[i] = qpOASES_getMin (_THIS->lb[i], _THIS->x[i]);
3287  _THIS->ub[i] = qpOASES_getMax (_THIS->ub[i], _THIS->x[i]);
3288  _THIS->y[i] = 0.0;
3289  break;
3290  case ST_UNDEFINED:
3291  case ST_INFEASIBLE_LOWER:
3292  case ST_INFEASIBLE_UPPER:
3293  break;
3294  }
3295  break;
3296  case ST_EQUALITY:
3297  _THIS->lb[i] = _THIS->x[i];
3298  _THIS->ub[i] = _THIS->x[i];
3299  break;
3300  case ST_UNBOUNDED:
3301  case ST_UNKNOWN:
3302  case ST_DISABLED:
3303  break;
3304  }
3305  }
3306 
3307  return QProblemB_setupAuxiliaryQPgradient( _THIS );
3308 }
3309 
3310 
3311 /*
3312  * s h a l l R e f a c t o r i s e
3313  */
3314 BooleanType QProblemB_shallRefactorise( QProblemB* _THIS, Bounds* const guessedBounds )
3315 {
3316  int i;
3317  int nV = QProblemB_getNV( _THIS );
3318 
3319  int differenceNumber = 0;
3320 
3321  /* always refactorise if Hessian is not known to be positive definite */
3322  if ( ( _THIS->hessianType == HST_SEMIDEF ) || ( _THIS->hessianType == HST_INDEF ) )
3323  return BT_TRUE;
3324 
3325  /* 1) Determine number of bounds that have same status
3326  * in guessed AND current bounds.*/
3327  for( i=0; i<nV; ++i )
3328  if ( Bounds_getStatus( guessedBounds,i ) != Bounds_getStatus(&(_THIS->bounds),i ) )
3329  ++differenceNumber;
3330 
3331  /* 2) Decide wheter to refactorise or not. */
3332  if ( 2*differenceNumber > Bounds_getNFX( guessedBounds ) )
3333  return BT_TRUE;
3334  else
3335  return BT_FALSE;
3336 }
3337 
3338 
3339 /*
3340  * a d d B o u n d
3341  */
3342 returnValue QProblemB_addBound( QProblemB* _THIS,
3343  int number, SubjectToStatus B_status,
3344  BooleanType updateCholesky
3345  )
3346 {
3347  int i, j;
3348  int nFR = QProblemB_getNFR( _THIS );
3349 
3350  int number_idx;
3351  real_t c, s, nu;
3352 
3353  /* consistency check */
3354  if ( ( QProblemB_getStatus( _THIS ) == QPS_NOTINITIALISED ) ||
3355  ( QProblemB_getStatus( _THIS ) == QPS_AUXILIARYQPSOLVED ) ||
3356  ( QProblemB_getStatus( _THIS ) == QPS_HOMOTOPYQPSOLVED ) ||
3357  ( QProblemB_getStatus( _THIS ) == QPS_SOLVED ) )
3358  {
3359  return THROWERROR( RET_UNKNOWN_BUG );
3360  }
3361 
3362  /* Perform cholesky updates only if QProblemB has been initialised! */
3363  if ( QProblemB_getStatus( _THIS ) == QPS_PREPARINGAUXILIARYQP )
3364  {
3365  /* UPDATE INDICES */
3366  if ( Bounds_moveFreeToFixed( &(_THIS->bounds),number,B_status ) != SUCCESSFUL_RETURN )
3367  return THROWERROR( RET_ADDBOUND_FAILED );
3368 
3369  return SUCCESSFUL_RETURN;
3370  }
3371 
3372 
3373  /* I) PERFORM CHOLESKY UPDATE: */
3374  if ( ( updateCholesky == BT_TRUE ) &&
3375  ( _THIS->hessianType != HST_ZERO ) && ( _THIS->hessianType != HST_IDENTITY ) )
3376  {
3377  /* 1) Index of variable to be added within the list of free variables. */
3378  number_idx = Indexlist_getIndex( Bounds_getFree( &(_THIS->bounds) ),number );
3379 
3380  /* 2) Use row-wise Givens rotations to restore upper triangular form of R. */
3381  for( i=number_idx+1; i<nFR; ++i )
3382  {
3383  QProblemB_computeGivens( RR(i-1,i),RR(i,i), &RR(i-1,i),&RR(i,i),&c,&s );
3384  nu = s/(1.0+c);
3385 
3386  for( j=(1+i); j<nFR; ++j ) /* last column of R is thrown away */
3387  QProblemB_applyGivens( c,s,nu,RR(i-1,j),RR(i,j), &RR(i-1,j),&RR(i,j) );
3388  }
3389 
3390  /* 3) Delete <number_idx>th column and ... */
3391  for( i=0; i<nFR-1; ++i )
3392  for( j=number_idx+1; j<nFR; ++j )
3393  RR(i,j-1) = RR(i,j);
3394  /* ... last column of R. */
3395  for( i=0; i<nFR; ++i )
3396  RR(i,nFR-1) = 0.0;
3397  }
3398 
3399  /* II) UPDATE INDICES */
3400  _THIS->tabularOutput.idxAddB = number;
3401  if ( Bounds_moveFreeToFixed( &(_THIS->bounds),number,B_status ) != SUCCESSFUL_RETURN )
3402  return THROWERROR( RET_ADDBOUND_FAILED );
3403 
3404 
3405  return SUCCESSFUL_RETURN;
3406 }
3407 
3408 
3409 /*
3410  * r e m o v e B o u n d
3411  */
3412 returnValue QProblemB_removeBound( QProblemB* _THIS,
3413  int number,
3414  BooleanType updateCholesky
3415  )
3416 {
3417  int i;
3418  int nFR = QProblemB_getNFR( _THIS );
3419 
3420  int* FR_idx;
3421 
3422  myStatic real_t rhs[NVMAX+1];
3423  myStatic real_t r[NVMAX];
3424 
3425  real_t r0;
3426 
3427 
3428  /* consistency check */
3429  if ( ( QProblemB_getStatus( _THIS ) == QPS_NOTINITIALISED ) ||
3430  ( QProblemB_getStatus( _THIS ) == QPS_AUXILIARYQPSOLVED ) ||
3431  ( QProblemB_getStatus( _THIS ) == QPS_HOMOTOPYQPSOLVED ) ||
3432  ( QProblemB_getStatus( _THIS ) == QPS_SOLVED ) )
3433  {
3434  return THROWERROR( RET_UNKNOWN_BUG );
3435  }
3436 
3437  /* save index sets and decompositions for flipping bounds strategy */
3438  if ( _THIS->options.enableFlippingBounds == BT_TRUE )
3439  Flipper_set( &(_THIS->flipper), &(_THIS->bounds),_THIS->R, 0,0,0 );
3440 
3441  /* I) UPDATE INDICES */
3442  _THIS->tabularOutput.idxRemB = number;
3443  if ( Bounds_moveFixedToFree( &(_THIS->bounds),number ) != SUCCESSFUL_RETURN )
3444  return THROWERROR( RET_REMOVEBOUND_FAILED );
3445 
3446  /* Perform cholesky updates only if QProblemB has been initialised! */
3447  if ( QProblemB_getStatus( _THIS ) == QPS_PREPARINGAUXILIARYQP )
3448  return SUCCESSFUL_RETURN;
3449 
3450 
3451  /* II) PERFORM CHOLESKY UPDATE */
3452  if ( ( updateCholesky == BT_TRUE ) &&
3453  ( _THIS->hessianType != HST_ZERO ) && ( _THIS->hessianType != HST_IDENTITY ) )
3454  {
3455  Indexlist_getNumberArray( Bounds_getFree( &(_THIS->bounds) ),&FR_idx );
3456 
3457  /* 1) Calculate new column of cholesky decomposition. */
3458  switch ( _THIS->hessianType )
3459  {
3460  case HST_ZERO:
3461  if ( QProblemB_usingRegularisation( _THIS ) == BT_FALSE )
3462  r0 = 0.0;
3463  else
3464  r0 = _THIS->regVal;
3465  for( i=0; i<nFR; ++i )
3466  rhs[i] = 0.0;
3467  break;
3468 
3469  case HST_IDENTITY:
3470  r0 = 1.0;
3471  for( i=0; i<nFR; ++i )
3472  rhs[i] = 0.0;
3473  break;
3474 
3475  default:
3476  DenseMatrix_getRow(_THIS->H,number, Bounds_getFree( &(_THIS->bounds) ), 1.0, rhs);
3477  r0 = DenseMatrix_diag(_THIS->H,number);
3478  break;
3479  }
3480 
3481  if ( QProblemB_backsolveRrem( _THIS,rhs,BT_TRUE,BT_TRUE,r ) != SUCCESSFUL_RETURN )
3482  return THROWERROR( RET_REMOVEBOUND_FAILED );
3483 
3484  for( i=0; i<nFR; ++i )
3485  r0 -= r[i]*r[i];
3486 
3487  /* 2) Store new column into R. */
3488  for( i=0; i<nFR; ++i )
3489  RR(i,nFR) = r[i];
3490 
3491  if ( _THIS->options.enableFlippingBounds == BT_TRUE )
3492  {
3493  if ( r0 > _THIS->options.epsFlipping )
3494  RR(nFR,nFR) = qpOASES_getSqrt( r0 );
3495  else
3496  {
3497  _THIS->hessianType = HST_SEMIDEF;
3498 
3499  Flipper_get( &(_THIS->flipper), &(_THIS->bounds),_THIS->R, 0,0,0 );
3500  Bounds_flipFixed( &(_THIS->bounds),number );
3501 
3502  switch ( Bounds_getStatus( &(_THIS->bounds),number) )
3503  {
3504  case ST_LOWER: _THIS->lb[number] = _THIS->ub[number]; break;
3505  case ST_UPPER: _THIS->ub[number] = _THIS->lb[number]; break;
3506  default: return THROWERROR( RET_MOVING_BOUND_FAILED );
3507  }
3508 
3509  }
3510  }
3511  else
3512  {
3513  if ( r0 > QPOASES_ZERO )
3514  RR(nFR,nFR) = qpOASES_getSqrt( r0 );
3515  else
3516  {
3517  _THIS->hessianType = HST_SEMIDEF;
3518  return THROWERROR( RET_HESSIAN_NOT_SPD );
3519  }
3520  }
3521  }
3522 
3523  if ( ( _THIS->hessianType == HST_ZERO ) && ( _THIS->options.enableFlippingBounds == BT_TRUE ) )
3524  {
3525  Flipper_get( &(_THIS->flipper), &(_THIS->bounds),_THIS->R, 0,0,0 );
3526  Bounds_flipFixed( &(_THIS->bounds),number );
3527 
3528  switch ( Bounds_getStatus( &(_THIS->bounds),number) )
3529  {
3530  case ST_LOWER: _THIS->lb[number] = _THIS->ub[number]; break;
3531  case ST_UPPER: _THIS->ub[number] = _THIS->lb[number]; break;
3532  default: return THROWERROR( RET_MOVING_BOUND_FAILED );
3533  }
3534 
3535  }
3536 
3537  return SUCCESSFUL_RETURN;
3538 }
3539 
3540 
3541 /*
3542  * p r i n t I t e r a t i o n
3543  */
3544 returnValue QProblemB_printIteration( QProblemB* _THIS,
3545  int iter,
3546  int BC_idx, SubjectToStatus BC_status,
3547  real_t homotopyLength,
3548  BooleanType isFirstCall
3549  )
3550 {
3551  #ifndef __SUPPRESSANYOUTPUT__
3552 
3553  myStatic char myPrintfString[QPOASES_MAX_STRING_LENGTH];
3554  myStatic char info[QPOASES_MAX_STRING_LENGTH];
3555 
3556  /* consistency check */
3557  if ( iter < 0 )
3558  return THROWERROR( RET_INVALID_ARGUMENTS );
3559 
3560  /* nothing to do */
3561  if ( _THIS->options.printLevel != PL_MEDIUM )
3562  return SUCCESSFUL_RETURN;
3563 
3564 
3565  /* 1) Print header at first iteration. */
3566  if ( ( iter == 0 ) && ( isFirstCall == BT_TRUE ) )
3567  {
3568  snprintf( myPrintfString,QPOASES_MAX_STRING_LENGTH,"\n\n################# qpOASES -- QP NO. %3.0d ##################\n\n", _THIS->count );
3569  qpOASES_myPrintf( myPrintfString );
3570 
3571  qpOASES_myPrintf( " Iter | StepLength | Info | nFX \n" );
3572  qpOASES_myPrintf( " ----------+------------------+------------------+--------- \n" );
3573  }
3574 
3575  /* 2) Print iteration line. */
3576  if ( BC_status == ST_UNDEFINED )
3577  {
3578  if ( _THIS->hessianType == HST_ZERO )
3579  snprintf( info,3,"LP" );
3580  else
3581  snprintf( info,3,"QP" );
3582 
3583  if ( isFirstCall == BT_TRUE )
3584  snprintf( myPrintfString,QPOASES_MAX_STRING_LENGTH," %5.1d | %1.6e | %s SOLVED | %4.1d \n", iter,_THIS->tau,info,QProblemB_getNFX( _THIS ) );
3585  else
3586  snprintf( myPrintfString,QPOASES_MAX_STRING_LENGTH," %5.1d* | %1.6e | %s SOLVED | %4.1d \n", iter,_THIS->tau,info,QProblemB_getNFX( _THIS ) );
3587 
3588  qpOASES_myPrintf( myPrintfString );
3589  }
3590  else
3591  {
3592  if ( BC_status == ST_INACTIVE )
3593  snprintf( info,8,"REM BND" );
3594  else
3595  snprintf( info,8,"ADD BND" );
3596 
3597  snprintf( myPrintfString,QPOASES_MAX_STRING_LENGTH," %5.1d | %1.6e | %s %4.1d | %4.1d \n", iter,_THIS->tau,info,BC_idx,QProblemB_getNFX( _THIS ) );
3598  qpOASES_myPrintf( myPrintfString );
3599  }
3600 
3601  #endif /* __SUPPRESSANYOUTPUT__ */
3602 
3603  return SUCCESSFUL_RETURN;
3604 }
3605 
3606 
3607 
3608 END_NAMESPACE_QPOASES
3609 
3610 
3611 /*
3612  * end of file
3613  */
RET_REMOVEBOUND_FAILED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:146
RET_STEPSIZE_NONPOSITIVE
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:139
Bounds_setupBound
returnValue Bounds_setupBound(Bounds *_THIS, int number, SubjectToStatus _status)
Definition: Bounds.c:125
QProblemB_reset
returnValue QProblemB_reset(QProblemB *_THIS)
Definition: QProblemB.c:153
RET_INIT_FAILED_HOTSTART
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:103
DenseMatrix_getNorm
real_t DenseMatrix_getNorm(DenseMatrix *_THIS, int type)
Definition: Matrices.c:135
QProblemB_getNV
static int QProblemB_getNV(QProblemB *_THIS)
Definition: QProblemB.h:1172
Bounds_getFree
static Indexlist * Bounds_getFree(Bounds *_THIS)
Definition: Bounds.h:508
QProblemB_isCPUtimeLimitExceeded
BooleanType QProblemB_isCPUtimeLimitExceeded(QProblemB *_THIS, const real_t *const cputime, real_t starttime, int nWSR)
Definition: QProblemB.c:1882
QProblemB_backsolveR
returnValue QProblemB_backsolveR(QProblemB *_THIS, const real_t *const b, BooleanType transposed, real_t *const a)
Definition: QProblemB.c:1547
QProblemB_getStatus
static QProblemStatus QProblemB_getStatus(QProblemB *_THIS)
Definition: QProblemB.h:1208
DenseMatrix_subTimes
returnValue DenseMatrix_subTimes(DenseMatrix *_THIS, const Indexlist *const irows, const Indexlist *const icols, int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD, BooleanType yCompr)
Definition: Matrices.c:230
QProblemB_hotstart
returnValue QProblemB_hotstart(QProblemB *_THIS, const real_t *const g_new, const real_t *const lb_new, const real_t *const ub_new, int *nWSR, real_t *const cputime)
Definition: QProblemB.c:471
DenseMatrix_diag
real_t DenseMatrix_diag(DenseMatrix *_THIS, int i)
Definition: Matrices.c:108
RET_UNABLE_TO_READ_FILE
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:172
RET_UNKNOWN_BUG
Definition: acado_types.hpp:845
ST_LOWER
#define ST_LOWER
Definition: qpOASES_wrapper.h:73
RET_STEPLENGTH_DETERMINATION_FAILED
Definition: acado_types.hpp:1096
Bounds_hasNoLower
static BooleanType Bounds_hasNoLower(Bounds *_THIS)
Definition: Bounds.h:435
RET_HESSIAN_NOT_SPD
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:164
Bounds_setNoUpper
static void Bounds_setNoUpper(Bounds *_THIS, BooleanType _status)
Definition: Bounds.h:426
RET_SHIFT_DETERMINATION_FAILED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:121
__LINE__
#define __LINE__
Definition: acado_message_handling.hpp:98
DenseMatrix_addToDiag
returnValue DenseMatrix_addToDiag(DenseMatrix *_THIS, real_t alpha)
Definition: Matrices.c:474
QProblemB_initMW
returnValue QProblemB_initMW(QProblemB *_THIS, DenseMatrix *_H, const real_t *const _g, const real_t *const _lb, const real_t *const _ub, int *nWSR, real_t *const cputime, const real_t *const xOpt, const real_t *const yOpt, Bounds *const guessedBounds, const real_t *const _R)
Definition: QProblemB.c:273
isZero
BooleanType isZero(real_t x, real_t TOL=ZERO)
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
Indexlist_getIndex
int Indexlist_getIndex(Indexlist *_THIS, int givennumber)
Definition: Indexlist.c:129
Bounds_setupAllFree
returnValue Bounds_setupAllFree(Bounds *_THIS)
Definition: Bounds.c:161
QProblemB_isInfeasible
static BooleanType QProblemB_isInfeasible(QProblemB *_THIS)
Definition: QProblemB.h:1241
SUCCESSFUL_RETURN
Definition: acado_types.hpp:836
RET_QPOBJECT_NOT_SETUP
Definition: external_packages/qpOASES-3.0beta/include/qpOASES/MessageHandling.hpp:92
RET_NO_CHOLESKY_WITH_INITIAL_GUESS
Definition: external_packages/qpOASES-3.2.0/include/qpOASES/MessageHandling.hpp:115
RET_ITERATION_STARTED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:120
QProblemB_performStep
returnValue QProblemB_performStep(QProblemB *_THIS, 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)
Definition: QProblemB.c:3069
QProblemB_determineHessianType
returnValue QProblemB_determineHessianType(QProblemB *_THIS)
Definition: QProblemB.c:1135
RET_HOMOTOPY_STEP_FAILED
Definition: acado_types.hpp:1093
RET_PRINT_ITERATION_FAILED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:167
Bounds_getStatus
static SubjectToStatus Bounds_getStatus(Bounds *_THIS, int i)
Definition: Bounds.h:375
DenseMatrix_getCol
returnValue DenseMatrix_getCol(DenseMatrix *_THIS, int cNum, const Indexlist *const irows, real_t alpha, real_t *col)
Definition: Matrices.c:178
QProblemB_performRamping
returnValue QProblemB_performRamping(QProblemB *_THIS)
Definition: QProblemB.c:2101
RET_MOVING_BOUND_FAILED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:94
RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:119
QProblemB_getObjVal
real_t QProblemB_getObjVal(QProblemB *_THIS)
Definition: QProblemB.c:807
RET_INVALID_ARGUMENTS
Definition: acado_types.hpp:839
RET_NO_DIAGONAL_AVAILABLE
Definition: external_packages/qpOASES-3.0beta/include/qpOASES/MessageHandling.hpp:201
qpOASES_getMin
static real_t qpOASES_getMin(real_t x, real_t y)
Definition: Utils.h:452
RET_MAX_NWSR_REACHED
Definition: acado_types.hpp:1098
QProblemB_printOptions
returnValue QProblemB_printOptions(QProblemB *_THIS)
Definition: QProblemB.c:1118
THROWINFO
#define THROWINFO(retval)
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:397
QProblemB_getWorkingSetConstraints
returnValue QProblemB_getWorkingSetConstraints(QProblemB *_THIS, real_t *workingSetC)
Definition: QProblemB.c:784
ST_INFEASIBLE_UPPER
#define ST_INFEASIBLE_UPPER
Definition: qpOASES_wrapper.h:77
QProblemB_getWorkingSetBounds
returnValue QProblemB_getWorkingSetBounds(QProblemB *_THIS, real_t *workingSetB)
Definition: QProblemB.c:763
returnValue
Allows to pass back messages to the calling function.
Definition: acado_types.hpp:1139
QProblemB_obtainAuxiliaryWorkingSet
returnValue QProblemB_obtainAuxiliaryWorkingSet(QProblemB *_THIS, const real_t *const xOpt, const real_t *const yOpt, Bounds *const guessedBounds, Bounds *auxiliaryBounds)
Definition: QProblemB.c:1403
QProblemB_getNFX
static int QProblemB_getNFX(QProblemB *_THIS)
Definition: QProblemB.h:1190
HST_POSDEF
#define HST_POSDEF
Definition: qpOASES_wrapper.h:66
QProblemB_setupSubjectToType
returnValue QProblemB_setupSubjectToType(QProblemB *_THIS)
Definition: QProblemB.c:1239
DenseMatrix_getRow
returnValue DenseMatrix_getRow(DenseMatrix *_THIS, int rNum, const Indexlist *const icols, real_t alpha, real_t *row)
Definition: Matrices.c:147
QProblemB_getObjValX
real_t QProblemB_getObjValX(QProblemB *_THIS, const real_t *const _x)
Definition: QProblemB.c:831
QProblemB_init
returnValue QProblemB_init(QProblemB *_THIS, real_t *const _H, const real_t *const _g, const real_t *const _lb, const real_t *const _ub, int *nWSR, real_t *const cputime)
Definition: QProblemB.c:219
RET_INIT_FAILED_REGULARISATION
Definition: external_packages/qpOASES-3.0beta/include/qpOASES/MessageHandling.hpp:102
RR
#define RR(I, J)
Definition: external_packages/qpOASES-3.0beta/include/qpOASES/Types.hpp:132
Options_setToFast
returnValue Options_setToFast(Options *_THIS)
Definition: Options.c:252
QProblemB_isUnbounded
static BooleanType QProblemB_isUnbounded(QProblemB *_THIS)
Definition: QProblemB.h:1250
NVMAX
#define NVMAX
Definition: qpoases3/include/qpOASES_e/Constants.h:70
PL_MEDIUM
#define PL_MEDIUM
Definition: qpOASES_wrapper.h:85
Bounds_hasNoUpper
static BooleanType Bounds_hasNoUpper(Bounds *_THIS)
Definition: Bounds.h:444
QProblemB_setLB
static returnValue QProblemB_setLB(QProblemB *_THIS, const real_t *const lb_new)
Definition: QProblemB.h:1406
THROWERROR
#define THROWERROR(retval)
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:391
MessageHandling_throwWarning
returnValue MessageHandling_throwWarning(MessageHandling *_THIS, returnValue Wnumber, const char *additionaltext, const char *functionname, const char *filename, const unsigned long linenumber, VisibilityStatus localVisibilityStatus)
Definition: MessageHandling.c:293
VS_HIDDEN
Definition: acado_message_handling.hpp:54
QProblemB_setupQPdataFromFile
returnValue QProblemB_setupQPdataFromFile(QProblemB *_THIS, const char *const H_file, const char *const g_file, const char *const lb_file, const char *const ub_file)
Definition: QProblemB.c:1718
QProblemB_setupQPdata
returnValue QProblemB_setupQPdata(QProblemB *_THIS, real_t *const _H, const real_t *const _g, const real_t *const _lb, const real_t *const _ub)
Definition: QProblemB.c:1694
RET_INIT_FAILED_UNBOUNDEDNESS
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:105
HessianType
HessianType
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/Types.hpp:116
QProblemB_addBound
returnValue QProblemB_addBound(QProblemB *_THIS, int number, SubjectToStatus B_status, BooleanType updateCholesky)
Definition: QProblemB.c:3342
QPS_SOLVED
Definition: acado_types.hpp:674
QProblemB_determineStepDirection
returnValue QProblemB_determineStepDirection(QProblemB *_THIS, 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: QProblemB.c:2900
ST_INFEASIBLE_LOWER
#define ST_INFEASIBLE_LOWER
Definition: qpOASES_wrapper.h:76
QProblemB_getRelativeHomotopyLength
real_t QProblemB_getRelativeHomotopyLength(QProblemB *_THIS, const real_t *const g_new, const real_t *const lb_new, const real_t *const ub_new)
Definition: QProblemB.c:2001
HST_IDENTITY
#define HST_IDENTITY
Definition: qpOASES_wrapper.h:65
QPOASES_ZERO
static const real_t QPOASES_ZERO
Definition: qpoases3/include/qpOASES_e/Constants.h:110
Flipper_set
returnValue Flipper_set(Flipper *_THIS, const Bounds *const _bounds, const real_t *const _R, const Constraints *const _constraints, const real_t *const _Q, const real_t *const _T)
Definition: Flipper.c:119
Bounds_moveFreeToFixed
returnValue Bounds_moveFreeToFixed(Bounds *_THIS, int number, SubjectToStatus _status)
Definition: Bounds.c:208
QProblemB_isBlocking
static BooleanType QProblemB_isBlocking(QProblemB *_THIS, real_t num, real_t den, real_t epsNum, real_t epsDen, real_t *t)
Definition: QProblemB.h:1568
QProblemB_getNFR
static int QProblemB_getNFR(QProblemB *_THIS)
Definition: QProblemB.h:1181
RET_ADD_TO_ACTIVESET_FAILED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:150
DenseMatrix_isDiag
BooleanType DenseMatrix_isDiag(DenseMatrix *_THIS)
Definition: Matrices.c:119
MessageHandling_throwInfo
returnValue MessageHandling_throwInfo(MessageHandling *_THIS, returnValue Inumber, const char *additionaltext, const char *functionname, const char *filename, const unsigned long linenumber, VisibilityStatus localVisibilityStatus)
Definition: MessageHandling.c:317
PL_NONE
#define PL_NONE
Definition: qpOASES_wrapper.h:83
QProblemB_changeActiveSet
returnValue QProblemB_changeActiveSet(QProblemB *_THIS, int BC_idx, SubjectToStatus BC_status)
Definition: QProblemB.c:3212
qpOASES_getCPUtime
real_t qpOASES_getCPUtime()
Definition: Utils.c:606
qpOASES_readFromFileV
returnValue qpOASES_readFromFileV(real_t *data, int n, const char *datafilename)
Definition: Utils.c:367
Flipper_get
returnValue Flipper_get(Flipper *_THIS, Bounds *const _bounds, real_t *const R, Constraints *const _constraints, real_t *const _Q, real_t *const _T)
Definition: Flipper.c:89
ST_UNDEFINED
#define ST_UNDEFINED
Definition: qpOASES_wrapper.h:78
QProblemBCPY
void QProblemBCPY(QProblemB *FROM, QProblemB *TO)
Definition: QProblemB.c:107
Bounds_setNoLower
static void Bounds_setNoLower(Bounds *_THIS, BooleanType _status)
Definition: Bounds.h:417
RET_CANNOT_REGULARISE_IDENTITY
Definition: external_packages/qpOASES-3.0beta/include/qpOASES/MessageHandling.hpp:176
MessageHandling_reset
returnValue MessageHandling_reset(MessageHandling *_THIS)
Definition: MessageHandling.c:341
RET_HESSIAN_INDEFINITE
Definition: external_packages/qpOASES-3.0beta/include/qpOASES/MessageHandling.hpp:162
QProblemB_hotstartFW
returnValue QProblemB_hotstartFW(QProblemB *_THIS, const char *const g_file, const char *const lb_file, const char *const ub_file, int *nWSR, real_t *const cputime, Bounds *const guessedBounds)
Definition: QProblemB.c:709
ST_INACTIVE
#define ST_INACTIVE
Definition: qpOASES_wrapper.h:74
QProblemB_setPrintLevel
returnValue QProblemB_setPrintLevel(QProblemB *_THIS, PrintLevel _printLevel)
Definition: QProblemB.c:930
RET_QP_INFEASIBLE
Definition: acado_types.hpp:1081
QProblemB_getPrimalSolution
returnValue QProblemB_getPrimalSolution(QProblemB *_THIS, real_t *const xOpt)
Definition: QProblemB.c:880
RET_STEPDIRECTION_DETERMINATION_FAILED
Definition: acado_types.hpp:1094
qpOASES_readFromFileM
returnValue qpOASES_readFromFileM(real_t *data, int nrow, int ncol, const char *datafilename)
Definition: Utils.c:314
QProblemB_regulariseHessian
returnValue QProblemB_regulariseHessian(QProblemB *_THIS)
Definition: QProblemB.c:1912
QProblemB_setupSubjectToTypeNew
returnValue QProblemB_setupSubjectToTypeNew(QProblemB *_THIS, const real_t *const lb_new, const real_t *const ub_new)
Definition: QProblemB.c:1248
Bounds_init
returnValue Bounds_init(Bounds *_THIS, int _n)
Definition: Bounds.c:87
QProblemB_setInfeasibilityFlag
returnValue QProblemB_setInfeasibilityFlag(QProblemB *_THIS, returnValue returnvalue, BooleanType doThrowError)
Definition: QProblemB.c:1847
HST_INDEF
#define HST_INDEF
Definition: qpOASES_wrapper.h:69
RET_SETUP_WORKINGSET_FAILED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:108
RET_QP_NOT_SOLVED
Definition: acado_types.hpp:1083
QProblemB_getWorkingSet
returnValue QProblemB_getWorkingSet(QProblemB *_THIS, real_t *workingSet)
Definition: QProblemB.c:754
Bounds_setType
static returnValue Bounds_setType(Bounds *_THIS, int i, SubjectToType value)
Definition: Bounds.h:387
PL_HIGH
#define PL_HIGH
Definition: qpOASES_wrapper.h:86
RET_HOTSTART_FAILED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:117
QProblemB_loadQPvectorsFromFile
returnValue QProblemB_loadQPvectorsFromFile(QProblemB *_THIS, const char *const g_file, const char *const lb_file, const char *const ub_file, real_t *const g_new, real_t *const lb_new, real_t *const ub_new)
Definition: QProblemB.c:1786
QProblemB_performRatioTestB
returnValue QProblemB_performRatioTestB(QProblemB *_THIS, int nIdx, const int *const idxList, Bounds *const subjectTo, const real_t *const num, const real_t *const den, real_t epsNum, real_t epsDen, real_t *t, int *BC_idx)
Definition: QProblemB.c:1951
Options_print
returnValue Options_print(Options *_THIS)
Definition: Options.c:406
MessageHandling_setInfoVisibilityStatus
static void MessageHandling_setInfoVisibilityStatus(MessageHandling *_THIS, VisibilityStatus _infoVisibility)
Definition: MessageHandling.h:511
ST_UNBOUNDED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/Types.hpp:89
RET_CHOLESKY_OF_ZERO_HESSIAN
Definition: external_packages/qpOASES-3.0beta/include/qpOASES/MessageHandling.hpp:180
ST_BOUNDED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/Types.hpp:90
DenseMatrix
Interfaces matrix-vector operations tailored to general dense matrices.
Definition: qpOASES-3.0beta/include/qpOASES/Matrices.hpp:273
qpOASES_getAbs
static real_t qpOASES_getAbs(real_t x)
Definition: Utils.h:468
ST_DISABLED
Definition: external_packages/qpOASES-3.2.0/include/qpOASES/Types.hpp:262
QProblemB_getNFV
static int QProblemB_getNFV(QProblemB *_THIS)
Definition: QProblemB.h:1199
QProblemB_setupInitialCholesky
returnValue QProblemB_setupInitialCholesky(QProblemB *_THIS)
Definition: QProblemB.c:435
BooleanType
BooleanType
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/Types.hpp:44
QPS_AUXILIARYQPSOLVED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/Types.hpp:77
QProblemB_usingRegularisation
static BooleanType QProblemB_usingRegularisation(QProblemB *_THIS)
Definition: QProblemB.h:1278
RET_CANNOT_REGULARISE_SPARSE
Definition: external_packages/qpOASES-3.0beta/include/qpOASES/MessageHandling.hpp:177
nu
const int nu
Definition: rocket_with_templates_ocp.hpp:38
QProblemB_setupAuxiliaryQP
returnValue QProblemB_setupAuxiliaryQP(QProblemB *_THIS, Bounds *const guessedBounds)
Definition: QProblemB.c:2838
qpOASES_getMax
static real_t qpOASES_getMax(real_t x, real_t y)
Definition: Utils.h:436
QProblemB_getNZ
int QProblemB_getNZ(QProblemB *_THIS)
Definition: QProblemB.c:797
Bounds_getNFX
static int Bounds_getNFX(Bounds *_THIS)
Definition: Bounds.h:499
QProblemB_setupAuxiliaryQPsolution
returnValue QProblemB_setupAuxiliaryQPsolution(QProblemB *_THIS, const real_t *const xOpt, const real_t *const yOpt)
Definition: QProblemB.c:2689
QProblemB_updateFarBounds
returnValue QProblemB_updateFarBounds(QProblemB *_THIS, real_t curFarBound, int nRamp, const real_t *const lb_new, real_t *const lb_new_far, const real_t *const ub_new, real_t *const ub_new_far)
Definition: QProblemB.c:2049
QProblemB_applyGivens
static void QProblemB_applyGivens(real_t c, real_t s, real_t nu, real_t xold, real_t yold, real_t *xnew, real_t *ynew)
Definition: QProblemB.h:1541
PL_DEBUG_ITER
#define PL_DEBUG_ITER
Definition: qpOASES_wrapper.h:81
QProblemB_setH
static returnValue QProblemB_setH(QProblemB *_THIS, real_t *const H_new)
Definition: QProblemB.h:1362
RET_HOTSTART_STOPPED_UNBOUNDEDNESS
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:127
qpOASES_myPrintf
returnValue qpOASES_myPrintf(const char *s)
Definition: Utils.c:269
QProblemB_printProperties
returnValue QProblemB_printProperties(QProblemB *_THIS)
Definition: QProblemB.c:981
QProblemB_initW
returnValue QProblemB_initW(QProblemB *_THIS, real_t *const _H, const real_t *const _g, const real_t *const _lb, const real_t *const _ub, int *nWSR, real_t *const cputime, const real_t *const xOpt, const real_t *const yOpt, Bounds *const guessedBounds, const real_t *const _R)
Definition: QProblemB.c:322
RET_INIT_FAILED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:100
QProblemB_initFW
returnValue QProblemB_initFW(QProblemB *_THIS, const char *const H_file, const char *const g_file, const char *const lb_file, const char *const ub_file, int *nWSR, real_t *const cputime, const real_t *const xOpt, const real_t *const yOpt, Bounds *const guessedBounds, const char *const R_file)
Definition: QProblemB.c:371
RET_PRINTLEVEL_CHANGED
Definition: acado_types.hpp:846
QProblemB_performDriftCorrection
returnValue QProblemB_performDriftCorrection(QProblemB *_THIS)
Definition: QProblemB.c:3263
QPS_PERFORMINGHOMOTOPY
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/Types.hpp:79
assert
#define assert(ignore)
Definition: MessageHandling.h:46
QProblemB_shallRefactorise
BooleanType QProblemB_shallRefactorise(QProblemB *_THIS, Bounds *const guessedBounds)
Definition: QProblemB.c:3314
qpOASES_getSqrt
static real_t qpOASES_getSqrt(real_t x)
Definition: Utils.h:481
qpOASES_getNorm
real_t qpOASES_getNorm(const real_t *const v, int n, int type)
Definition: Utils.c:629
RET_ADDBOUND_FAILED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:143
RET_NO_REGSTEP_NWSR
Definition: external_packages/qpOASES-3.0beta/include/qpOASES/MessageHandling.hpp:178
SubjectToStatus
SubjectToStatus
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/Types.hpp:97
QProblemB_backsolveRrem
returnValue QProblemB_backsolveRrem(QProblemB *_THIS, const real_t *const b, BooleanType transposed, BooleanType removingBound, real_t *const a)
Definition: QProblemB.c:1559
RET_INIT_FAILED_CHOLESKY
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:102
PL_LOW
#define PL_LOW
Definition: qpOASES_wrapper.h:84
QProblemB_initM
returnValue QProblemB_initM(QProblemB *_THIS, DenseMatrix *_H, const real_t *const _g, const real_t *const _lb, const real_t *const _ub, int *nWSR, real_t *const cputime)
Definition: QProblemB.c:192
VS_VISIBLE
Definition: acado_message_handling.hpp:53
QProblemB_setupQPdataM
returnValue QProblemB_setupQPdataM(QProblemB *_THIS, DenseMatrix *_H, const real_t *const _g, const real_t *const _lb, const real_t *const _ub)
Definition: QProblemB.c:1680
QPOASES_INFTY
static const real_t QPOASES_INFTY
Definition: qpoases3/include/qpOASES_e/Constants.h:114
RET_DIAGONAL_NOT_INITIALISED
Definition: external_packages/qpOASES-3.2.0/include/qpOASES/MessageHandling.hpp:217
MessageHandling_setErrorVisibilityStatus
static void MessageHandling_setErrorVisibilityStatus(MessageHandling *_THIS, VisibilityStatus _errorVisibility)
Definition: MessageHandling.h:493
rhs
void rhs(const real_t *x, real_t *f)
Definition: examples/code_generation/simulation/externModel_export/model.c:5
MessageHandling_setWarningVisibilityStatus
static void MessageHandling_setWarningVisibilityStatus(MessageHandling *_THIS, VisibilityStatus _warningVisibility)
Definition: MessageHandling.h:502
END_NAMESPACE_QPOASES
#define END_NAMESPACE_QPOASES
Definition: external_packages/qpOASES-3.0beta/include/qpOASES/Types.hpp:120
PrintLevel
PrintLevel
Definition: acado_types.hpp:292
RET_QP_ALREADY_INITIALISED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:97
Bounds_moveFixedToFree
returnValue Bounds_moveFixedToFree(Bounds *_THIS, int number)
Definition: Bounds.c:188
OptionsCPY
void OptionsCPY(Options *FROM, Options *TO)
Definition: Options.c:64
RET_USING_REGULARISATION
Definition: external_packages/qpOASES-3.0beta/include/qpOASES/MessageHandling.hpp:172
QProblemB_hotstartF
returnValue QProblemB_hotstartF(QProblemB *_THIS, const char *const g_file, const char *const lb_file, const char *const ub_file, int *nWSR, real_t *const cputime)
Definition: QProblemB.c:615
HST_POSDEF_NULLSPACE
#define HST_POSDEF_NULLSPACE
Definition: qpOASES_wrapper.h:67
qpOASES_getGlobalMessageHandler
MessageHandling * qpOASES_getGlobalMessageHandler()
Definition: MessageHandling.c:545
RET_ADD_TO_ACTIVESET
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:148
Bounds_flipFixed
returnValue Bounds_flipFixed(Bounds *_THIS, int number)
Definition: Bounds.c:229
QProblemB_setupAuxiliaryQPbounds
returnValue QProblemB_setupAuxiliaryQPbounds(QProblemB *_THIS, BooleanType useRelaxation)
Definition: QProblemB.c:2770
BT_TRUE
#define BT_TRUE
Definition: acado_types.hpp:47
QProblemB_setupAuxiliaryWorkingSet
returnValue QProblemB_setupAuxiliaryWorkingSet(QProblemB *_THIS, Bounds *const auxiliaryBounds, BooleanType setupAfresh)
Definition: QProblemB.c:2620
DenseMatrix_getVal
static real_t * DenseMatrix_getVal(DenseMatrix *_THIS)
Definition: Matrices.h:257
QProblemB_isInitialised
static BooleanType QProblemB_isInitialised(QProblemB *_THIS)
Definition: QProblemB.h:1217
Options_setToDefault
returnValue Options_setToDefault(Options *_THIS)
Definition: Options.c:112
RET_OPTIMAL_SOLUTION_FOUND
Definition: acado_types.hpp:1097
RET_SETUP_AUXILIARYQP_FAILED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:109
RET_REMOVE_FROM_ACTIVESET
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:147
RET_INIT_SUCCESSFUL
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:106
ST_UNKNOWN
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/Types.hpp:92
HST_ZERO
#define HST_ZERO
Definition: qpOASES_wrapper.h:64
QProblemB_computeCholesky
returnValue QProblemB_computeCholesky(QProblemB *_THIS)
Definition: QProblemB.c:1324
QProblemB_hotstartW
returnValue QProblemB_hotstartW(QProblemB *_THIS, const real_t *const g_new, const real_t *const lb_new, const real_t *const ub_new, int *nWSR, real_t *const cputime, Bounds *const guessedBounds)
Definition: QProblemB.c:656
THROWWARNING
#define THROWWARNING(retval)
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:394
RET_OBTAINING_WORKINGSET_FAILED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:107
QProblemB_areBoundsConsistent
returnValue QProblemB_areBoundsConsistent(QProblemB *_THIS, const real_t *const lb_new, const real_t *const ub_new)
Definition: QProblemB.c:1863
HST_SEMIDEF
#define HST_SEMIDEF
Definition: qpOASES_wrapper.h:68
QPOASES_EPS
static const real_t QPOASES_EPS
Definition: qpoases3/include/qpOASES_e/Constants.h:101
RET_STEPSIZE
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:138
QPS_NOTINITIALISED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/Types.hpp:74
QPS_HOMOTOPYQPSOLVED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/Types.hpp:81
HST_UNKNOWN
#define HST_UNKNOWN
Definition: qpOASES_wrapper.h:70
QProblemB_solveInitialQP
returnValue QProblemB_solveInitialQP(QProblemB *_THIS, const real_t *const xOpt, const real_t *const yOpt, Bounds *const guessedBounds, const real_t *const _R, int *nWSR, real_t *const cputime)
Definition: QProblemB.c:2145
RET_ZERO_HESSIAN_ASSUMED
Definition: external_packages/qpOASES-3.2.0/include/qpOASES/MessageHandling.hpp:190
QProblemB_setupAuxiliaryQPgradient
returnValue QProblemB_setupAuxiliaryQPgradient(QProblemB *_THIS)
Definition: QProblemB.c:2730
Indexlist_getNumberArray
returnValue Indexlist_getNumberArray(Indexlist *_THIS, int **const numberarray)
Definition: Indexlist.c:105
QProblemB_setG
static returnValue QProblemB_setG(QProblemB *_THIS, const real_t *const g_new)
Definition: QProblemB.h:1387
QPS_PREPARINGAUXILIARYQP
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/Types.hpp:75
RET_INIT_FAILED_INFEASIBILITY
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:104
__FILE__
#define __FILE__
Definition: acado_message_handling.hpp:94
QPOASES_MAX_STRING_LENGTH
#define QPOASES_MAX_STRING_LENGTH
Definition: qpoases3/include/qpOASES_e/Constants.h:123
QProblemB_printIteration
returnValue QProblemB_printIteration(QProblemB *_THIS, int iter, int BC_idx, SubjectToStatus BC_status, real_t homotopyLength, BooleanType isFirstCall)
Definition: QProblemB.c:3544
BT_FALSE
#define BT_FALSE
Definition: acado_types.hpp:49
PL_TABULAR
#define PL_TABULAR
Definition: qpOASES_wrapper.h:82
ST_EQUALITY
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/Types.hpp:91
Bounds
Manages working sets of bounds (= box constraints).
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/Bounds.hpp:50
ST_UPPER
#define ST_UPPER
Definition: qpOASES_wrapper.h:75
BEGIN_NAMESPACE_QPOASES
#define BEGIN_NAMESPACE_QPOASES
Definition: external_packages/qpOASES-3.0beta/include/qpOASES/Types.hpp:117
__FUNC__
#define __FUNC__
Definition: external_packages/qpOASES-3.2.0/include/qpOASES/MessageHandling.hpp:450
DenseMatrix_times
returnValue DenseMatrix_times(DenseMatrix *_THIS, int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD)
Definition: Matrices.c:196
real_t
double real_t
Definition: AD_test.c:10
QProblemB_solveRegularisedQP
returnValue QProblemB_solveRegularisedQP(QProblemB *_THIS, 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 nWSRperformed, BooleanType isFirstCall)
Definition: QProblemB.c:2515
RET_STEPDIRECTION_FAILED_CHOLESKY
Definition: acado_types.hpp:1095
QProblemB_computeGivens
static void QProblemB_computeGivens(real_t xold, real_t yold, real_t *xnew, real_t *ynew, real_t *c, real_t *s)
Definition: QProblemB.h:1503
qpOASES_printCopyrightNotice
returnValue qpOASES_printCopyrightNotice()
Definition: Utils.c:296
Bounds_getType
static SubjectToType Bounds_getType(Bounds *_THIS, int i)
Definition: Bounds.h:363
QProblemB_setUB
static returnValue QProblemB_setUB(QProblemB *_THIS, const real_t *const ub_new)
Definition: QProblemB.h:1454
RET_DIV_BY_ZERO
Definition: acado_types.hpp:837
QProblemB_removeBound
returnValue QProblemB_removeBound(QProblemB *_THIS, int number, BooleanType updateCholesky)
Definition: QProblemB.c:3412
RET_REMOVE_FROM_ACTIVESET_FAILED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:149
QProblemB::HH
DenseMatrix HH
Definition: QProblemB.h:66
QProblemB_determineDataShift
returnValue QProblemB_determineDataShift(QProblemB *_THIS, 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)
Definition: QProblemB.c:1617
QProblemB_getDualSolution
returnValue QProblemB_getDualSolution(QProblemB *_THIS, real_t *const yOpt)
Definition: QProblemB.c:905
Bounds_getFixed
static Indexlist * Bounds_getFixed(Bounds *_THIS)
Definition: Bounds.h:517
QProblemBCON
BEGIN_NAMESPACE_QPOASES void QProblemBCON(QProblemB *_THIS, int _nV, HessianType _hessianType)
Definition: QProblemB.c:45
myStatic
#define myStatic
Definition: Types.h:103
RET_FEWER_REGSTEPS_NWSR
Definition: external_packages/qpOASES-3.0beta/include/qpOASES/MessageHandling.hpp:179
RET_HOTSTART_STOPPED_INFEASIBILITY
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:126
QProblemB_initF
returnValue QProblemB_initF(QProblemB *_THIS, const char *const H_file, const char *const g_file, const char *const lb_file, const char *const ub_file, int *nWSR, real_t *const cputime)
Definition: QProblemB.c:246
BoundsCON
void BoundsCON(Bounds *_THIS, int _n)
Definition: Bounds.c:50
QProblemB_solveQP
returnValue QProblemB_solveQP(QProblemB *_THIS, 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 nWSRperformed, BooleanType isFirstCall)
Definition: QProblemB.c:2289

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