qpOASES-3.2.0/src/SQProblem.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of qpOASES.
3  *
4  * qpOASES -- An Implementation of the Online Active Set Strategy.
5  * Copyright (C) 2007-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 
37 #include <qpOASES/SQProblem.hpp>
38 
39 
41 
42 
43 /*****************************************************************************
44  * P U B L I C *
45  *****************************************************************************/
46 
47 
48 /*
49  * S Q P r o b l e m
50  */
52 {
53 }
54 
55 
56 /*
57  * S Q P r o b l e m
58  */
59 SQProblem::SQProblem( int_t _nV, int_t _nC, HessianType _hessianType ) : QProblem( _nV,_nC,_hessianType )
60 {
61 }
62 
63 
64 /*
65  * S Q P r o b l e m
66  */
67 SQProblem::SQProblem( const SQProblem& rhs ) : QProblem( rhs )
68 {
69 }
70 
71 
72 /*
73  * ~ S Q P r o b l e m
74  */
76 {
77 }
78 
79 
80 /*
81  * o p e r a t o r =
82  */
84 {
85  if ( this != &rhs )
86  {
87  QProblem::operator=( rhs );
88  }
89 
90  return *this;
91 }
92 
93 
94 
95 /*
96  * h o t s t a r t
97  */
99  const real_t* const lb_new, const real_t* const ub_new,
100  const real_t* const lbA_new, const real_t* const ubA_new,
101  int_t& nWSR, real_t* const cputime,
102  const Bounds* const guessedBounds, const Constraints* const guessedConstraints
103  )
104 {
105  if ( ( getStatus( ) == QPS_NOTINITIALISED ) ||
108  {
110  }
111 
112 
113  real_t starttime = 0.0;
114  real_t auxTime = 0.0;
115 
116  if ( cputime != 0 )
117  starttime = getCPUtime( );
118 
119 
120  /* I) UPDATE QP MATRICES AND VECTORS */
121  if ( setupNewAuxiliaryQP( H_new,A_new,lb_new,ub_new,lbA_new,ubA_new ) != SUCCESSFUL_RETURN )
123 
124 
125  /* II) PERFORM USUAL HOMOTOPY */
126 
127  /* Allow only remaining CPU time for usual hotstart. */
128  if ( cputime != 0 )
129  {
130  auxTime = getCPUtime( ) - starttime;
131  *cputime -= auxTime;
132  }
133 
134  returnValue returnvalue = QProblem::hotstart( g_new,lb_new,ub_new,lbA_new,ubA_new,
135  nWSR,cputime,
136  guessedBounds,guessedConstraints
137  );
138 
139  if ( cputime != 0 )
140  *cputime += auxTime;
141 
142  return returnvalue;
143 }
144 
145 
146 /*
147  * h o t s t a r t
148  */
149 returnValue SQProblem::hotstart( const real_t* const H_new, const real_t* const g_new, const real_t* const A_new,
150  const real_t* const lb_new, const real_t* const ub_new,
151  const real_t* const lbA_new, const real_t* const ubA_new,
152  int_t& nWSR, real_t* const cputime,
153  const Bounds* const guessedBounds, const Constraints* const guessedConstraints
154  )
155 {
156  if ( ( getStatus( ) == QPS_NOTINITIALISED ) ||
159  {
161  }
162 
163  /* start runtime measurement */
164  real_t starttime = 0.0;
165  if ( cputime != 0 )
166  starttime = getCPUtime( );
167 
168 
169  /* I) UPDATE QP MATRICES AND VECTORS */
170  if ( setupNewAuxiliaryQP( H_new,A_new,lb_new,ub_new,lbA_new,ubA_new ) != SUCCESSFUL_RETURN )
172 
173 
174  /* II) PERFORM USUAL HOMOTOPY */
175 
176  /* Allow only remaining CPU time for usual hotstart. */
177  if ( cputime != 0 )
178  *cputime -= getCPUtime( ) - starttime;
179 
180  returnValue returnvalue = QProblem::hotstart( g_new,lb_new,ub_new,lbA_new,ubA_new,
181  nWSR,cputime,
182  guessedBounds,guessedConstraints
183  );
184 
185 
186  /* stop runtime measurement */
187  if ( cputime != 0 )
188  *cputime = getCPUtime( ) - starttime;
189 
190  return returnvalue;
191 }
192 
193 
194 /*
195  * h o t s t a r t
196  */
197 returnValue SQProblem::hotstart( const char* const H_file, const char* const g_file, const char* const A_file,
198  const char* const lb_file, const char* const ub_file,
199  const char* const lbA_file, const char* const ubA_file,
200  int_t& nWSR, real_t* const cputime,
201  const Bounds* const guessedBounds, const Constraints* const guessedConstraints
202  )
203 {
204  int_t nV = getNV( );
205  int_t nC = getNC( );
206 
207  returnValue returnvalue;
208 
209  /* consistency checks */
210  if ( ( H_file == 0 ) || ( g_file == 0 ) )
212 
213  if ( ( nC > 0 ) && ( A_file == 0 ) )
215 
216 
217  /* 1) Load new QP matrices from files. */
218  real_t* H_new = new real_t[nV*nV];
219  real_t* A_new = new real_t[nC*nV];
220 
221  if ( readFromFile( H_new, nV,nV, H_file ) != SUCCESSFUL_RETURN )
222  {
223  delete[] A_new;
224  delete[] H_new;
226  }
227 
228  if ( readFromFile( A_new, nC,nV, A_file ) != SUCCESSFUL_RETURN )
229  {
230  delete[] A_new;
231  delete[] H_new;
233  }
234 
235  /* 2) Load new QP vectors from files. */
236  real_t* g_new = new real_t[nV];
237  real_t* lb_new = ( lb_file != 0 ) ? new real_t[nV] : 0;
238  real_t* ub_new = ( ub_file != 0 ) ? new real_t[nV] : 0;
239  real_t* lbA_new = ( lbA_file != 0 ) ? new real_t[nC] : 0;
240  real_t* ubA_new = ( ubA_file != 0 ) ? new real_t[nC] : 0;
241 
242  returnvalue = loadQPvectorsFromFile( g_file,lb_file,ub_file,lbA_file,ubA_file,
243  g_new,lb_new,ub_new,lbA_new,ubA_new
244  );
245  if ( returnvalue != SUCCESSFUL_RETURN )
246  {
247  if ( ubA_file != 0 )
248  delete[] ubA_new;
249  if ( lbA_file != 0 )
250  delete[] lbA_new;
251  if ( ub_file != 0 )
252  delete[] ub_new;
253  if ( lb_file != 0 )
254  delete[] lb_new;
255  delete[] g_new;
256  delete[] A_new;
257  delete[] H_new;
258 
260  }
261 
262  /* 3) Actually perform hotstart. */
263  returnvalue = hotstart( H_new,g_new,A_new,lb_new,ub_new,lbA_new,ubA_new,
264  nWSR,cputime,
265  guessedBounds,guessedConstraints
266  );
267 
268  if ( ubA_file != 0 )
269  delete[] ubA_new;
270  if ( lbA_file != 0 )
271  delete[] lbA_new;
272  if ( ub_file != 0 )
273  delete[] ub_new;
274  if ( lb_file != 0 )
275  delete[] lb_new;
276  delete[] g_new;
277  delete[] A_new;
278  delete[] H_new;
279 
280  return returnvalue;
281 }
282 
283 
284 /*
285  * h o t s t a r t
286  */
288  const real_t* const lb_new, const real_t* const ub_new,
289  const real_t* const lbA_new, const real_t* const ubA_new,
290  int_t& nWSR, real_t* const cputime,
291  const Bounds* const guessedBounds, const Constraints* const guessedConstraints
292  )
293 {
294  /* Call to hotstart function for fixed QP matrices. */
295  return QProblem::hotstart( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,cputime, guessedBounds,guessedConstraints );
296 }
297 
298 
299 /*
300  * h o t s t a r t
301  */
302 returnValue SQProblem::hotstart( const char* const g_file,
303  const char* const lb_file, const char* const ub_file,
304  const char* const lbA_file, const char* const ubA_file,
305  int_t& nWSR, real_t* const cputime,
306  const Bounds* const guessedBounds, const Constraints* const guessedConstraints
307  )
308 {
309  /* Call to hotstart function for fixed QP matrices. */
310  return QProblem::hotstart( g_file,lb_file,ub_file,lbA_file,ubA_file, nWSR,cputime, guessedBounds,guessedConstraints );
311 }
312 
313 
314 
315 #ifdef __MATLAB__
316 returnValue SQProblem::resetMatrixPointers( )
317 {
318  H = 0;
319  A = 0;
320 
321  return SUCCESSFUL_RETURN;
322 }
323 #endif
324 
325 
326 
327 /*****************************************************************************
328  * P R O T E C T E D *
329  *****************************************************************************/
330 
331 /*
332  * s e t u p N e w A u x i l i a r y Q P
333  */
335  const real_t *lb_new, const real_t *ub_new, const real_t *lbA_new, const real_t *ubA_new
336  )
337 {
338  int_t i;
339  int_t nV = getNV( );
340  int_t nC = getNC( );
341  returnValue returnvalue;
342 
343  if ( ( getStatus( ) == QPS_NOTINITIALISED ) ||
346  {
348  }
349 
351 
352 
353  /* I) SETUP NEW QP MATRICES AND VECTORS: */
354  /* 1) Shift constraints' bounds vectors by (A_new - A)'*x_opt to ensure
355  * that old optimal solution remains feasible for new QP data. */
356  /* Firstly, shift by -A'*x_opt and ... */
357  if ( nC > 0 )
358  {
359  if ( A_new == 0 )
361 
362  for ( i=0; i<nC; ++i )
363  {
364  lbA[i] = -Ax_l[i];
365  ubA[i] = Ax_u[i];
366  }
367 
368  /* Set constraint matrix as well as ... */
369  setA( A_new );
370 
371  /* ... secondly, shift by +A_new'*x_opt. */
372  for ( i=0; i<nC; ++i )
373  {
374  lbA[i] += Ax[i];
375  ubA[i] += Ax[i];
376  }
377 
378  /* update constraint products. */
379  for ( i=0; i<nC; ++i )
380  {
381  Ax_u[i] = ubA[i] - Ax[i];
382  Ax_l[i] = Ax[i] - lbA[i];
383  }
384  }
385 
386  /* 2) Set new Hessian matrix, determine Hessian type and
387  * regularise new Hessian matrix if necessary. */
388  /* a) Setup new Hessian matrix and determine its type. */
389  if ( H_new != 0 )
390  {
391  setH( H_new );
392 
396 
397  /* b) Regularise new Hessian if necessary. */
398  if ( ( hessianType == HST_ZERO ) ||
399  ( hessianType == HST_SEMIDEF ) ||
400  ( usingRegularisation( ) == BT_TRUE ) )
401  {
402  regVal = 0.0; /* reset previous regularisation */
403 
406  }
407  }
408  else
409  {
410  /* if no Hessian is specified, keep previous trivial Hessian (HST_ZERO or HST_IDENTITY),
411  otherwise abort */
412  if ( H != 0 )
414  }
415 
416  /* 3) Setup QP gradient. */
419 
420 
421  /* II) SETUP WORKING SETS AND MATRIX FACTORISATIONS: */
422  /* 1) Make a copy of current bounds/constraints ... */
423  Bounds oldBounds = bounds;
424  Constraints oldConstraints = constraints;
425 
426  /* we're trying to find an active set with positive definite null
427  * space Hessian twice:
428  * - first for the current active set including all equalities
429  * - second after moving all inactive variables to a bound
430  * (depending on Options). This creates an empty null space and
431  * is guaranteed to succeed. Thus this loop will exit after n_try=1.
432  */
433  int_t n_try;
434  for (n_try = 0; n_try < 2; ++n_try) {
435 
436  if (n_try > 0) {
437  // the current active set leaves an indefinite null space Hessian
438  // move all inactive variables to a bound, creating an empty null space
439  for (int_t ii = 0; ii < nV; ++ii)
440  if (oldBounds.getStatus (ii) == ST_INACTIVE)
441  oldBounds.setStatus (ii, options.initialStatusBounds);
442  }
443 
444  /* ... reset them ... */
445  bounds.init( nV );
446  constraints.init( nC );
447 
448  /* ... and set them up afresh. */
449  if ( setupSubjectToType(lb_new,ub_new,lbA_new,ubA_new ) != SUCCESSFUL_RETURN )
451 
454 
457 
458  /* 2) Setup TQ factorisation. */
461 
462  // check for equalities that have become bounds ...
463  for (int_t ii = 0; ii < nC; ++ii) {
464  if (oldConstraints.getType (ii) == ST_EQUALITY && constraints.getType (ii) == ST_BOUNDED) {
465  if (oldConstraints.getStatus (ii) == ST_LOWER && y[nV+ii] < 0.0)
466  oldConstraints.setStatus (ii, ST_UPPER);
467  else if (oldConstraints.getStatus (ii) == ST_UPPER && y[nV+ii] > 0.0)
468  oldConstraints.setStatus (ii, ST_LOWER);
469  }
470  }
471 
472  // ... and do the same also for the bounds!
473  for (int_t ii = 0; ii < nV; ++ii) {
474  if (oldBounds.getType(ii) == ST_EQUALITY
475  && bounds.getType(ii) == ST_BOUNDED) {
476  if (oldBounds.getStatus(ii) == ST_LOWER && y[ii] < 0.0)
477  oldBounds.setStatus(ii, ST_UPPER);
478  else if (oldBounds.getStatus(ii) == ST_UPPER && y[ii] > 0.0)
479  oldBounds.setStatus(ii, ST_LOWER);
480  }
481  }
482 
483  /* 3) Setup old working sets afresh (updating TQ factorisation). */
484  if ( setupAuxiliaryWorkingSet( &oldBounds,&oldConstraints,BT_TRUE ) != SUCCESSFUL_RETURN )
486 
487  /* Factorise projected Hessian
488  * this now handles all special cases (no active bounds/constraints, no nullspace) */
489  returnvalue = computeProjectedCholesky( );
490 
491  /* leave the loop if decomposition was successful, i.e. we have
492  * found an active set with positive definite null space Hessian */
493  if ( returnvalue == SUCCESSFUL_RETURN )
494  break;
495  }
496 
497  /* adjust lb/ub if we changed the old active set in the second try
498  */
499  if (n_try > 0) {
500  // as per setupAuxiliaryQPbounds assumptions ... oh the troubles
501  for (int_t ii = 0; ii < nC; ++ii)
502  Ax_l[ii] = Ax_u[ii] = Ax[ii];
504  }
505 
507 
508  return SUCCESSFUL_RETURN;
509 }
510 
511 
512 /*
513  * s e t u p N e w A u x i l i a r y Q P
514  */
516  const real_t *lb_new, const real_t *ub_new, const real_t *lbA_new, const real_t *ubA_new
517  )
518 {
519  int_t nV = getNV( );
520  int_t nC = getNC( );
521 
522  DenseMatrix *dA = 0;
523  SymDenseMat *sH = 0;
524 
525  if ( A_new != 0 )
526  {
527  dA = new DenseMatrix(nC, nV, nV, (real_t*) A_new);
528  }
529  else
530  {
531  if ( nC > 0 )
533  }
534 
535  if ( H_new != 0 )
536  sH = new SymDenseMat(nV, nV, nV, (real_t*) H_new);
537 
538  returnValue returnvalue = setupNewAuxiliaryQP( sH,dA, lb_new,ub_new,lbA_new,ubA_new );
539 
540  if ( H_new != 0 )
543 
544  return returnvalue;
545 }
546 
547 
549 
550 
551 /*
552  * end of file
553  */
QProblemStatus status
Definition: QProblem.h:82
#define ST_LOWER
Implements the online active set strategy for QPs with varying matrices.
Allows to pass back messages to the calling function.
HessianType hessianType
Definition: QProblem.h:87
Interfaces matrix-vector operations tailored to symmetric dense matrices.
int getNV() const
returnValue setH(const real_t *const H_new)
SQProblem & operator=(const SQProblem &rhs)
returnValue hotstart(const real_t *const g_new, const real_t *const lb_new, const real_t *const ub_new, const real_t *const lbA_new, const real_t *const ubA_new, int &nWSR, real_t *const cputime)
returnValue readFromFile(real_t *data, int nrow, int ncol, const char *datafilename)
#define ST_INACTIVE
BooleanType usingRegularisation() const
real_t regVal
Definition: QProblem.h:88
SubjectToStatus getStatus(int i) const
Interfaces matrix-vector operations tailored to general dense matrices.
Bounds bounds
Definition: QProblem.h:72
virtual returnValue computeProjectedCholesky()
returnValue setupAuxiliaryQPbounds(const Bounds *const auxiliaryBounds, const Constraints *const auxiliaryConstraints, BooleanType useRelaxation)
Abstract base class for interfacing tailored matrix-vector operations.
Options options
Definition: QProblem.h:98
returnValue loadQPvectorsFromFile(const char *const g_file, const char *const lb_file, const char *const ub_file, const char *const lbA_file, const char *const ubA_file, real_t *const g_new, real_t *const lb_new, real_t *const ub_new, real_t *const lbA_new, real_t *const ubA_new) const
void rhs(const real_t *x, real_t *f)
int getNC() const
real_t y[NVMAX+NCMAX]
Definition: QProblem.h:78
SubjectToType getType(int i) const
QProblemStatus getStatus() const
#define BT_TRUE
Definition: acado_types.hpp:47
#define HST_ZERO
#define HST_SEMIDEF
#define HST_UNKNOWN
returnValue setStatus(int i, SubjectToStatus value)
DenseMatrix * H
Definition: QProblem.h:65
#define BT_FALSE
Definition: acado_types.hpp:49
virtual returnValue setupNewAuxiliaryQP(SymmetricMatrix *H_new, Matrix *A_new, const real_t *lb_new, const real_t *ub_new, const real_t *lbA_new, const real_t *ubA_new)
Manages working sets of bounds (= box constraints).
#define ST_UPPER
double real_t
Definition: AD_test.c:10
Implements the online active set strategy for QPs with general constraints.
returnValue setupAuxiliaryWorkingSet(const Bounds *const auxiliaryBounds, const Constraints *const auxiliaryConstraints, BooleanType setupAfresh)
returnValue setA(const real_t *const A_new)
Abstract base class for interfacing matrix-vector operations tailored to symmetric matrices...
returnValue hotstart(const real_t *const H_new, const real_t *const g_new, const real_t *const A_new, const real_t *const lb_new, const real_t *const ub_new, const real_t *const lbA_new, const real_t *const ubA_new, int &nWSR, real_t *const cputime)


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