qpOASES_wrapper.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 
36 #include <qpOASES.hpp>
37 
39 
40 
41 extern "C" {
42 #include "qpOASES_wrapper.h"
43 }
44 
45 
46 
47 
48 /*
49  * q p O A S E S _ O p t i o n s _ i n i t
50  */
52  int_t mode
53  )
54 {
55  if ( ( mode < 0 ) || ( mode > 2 ) )
56  return -1;
57 
58  if ( options == 0 )
59  return -1;
60 
61 
62  /* setup default */
63  options->printLevel = PL_MEDIUM;
64  #ifdef __DEBUG__
65  options->printLevel = PL_HIGH;
66  #endif
67  #ifdef __SUPPRESSANYOUTPUT__
68  options->printLevel = PL_NONE;
69  #endif
70 
71  options->enableRamping = BT_TRUE;
72  options->enableFarBounds = BT_TRUE;
73  options->enableFlippingBounds = BT_TRUE;
74  options->enableRegularisation = BT_FALSE;
75  options->enableFullLITests = BT_FALSE;
76  options->enableNZCTests = BT_TRUE;
77  options->enableDriftCorrection = 1;
78  options->enableCholeskyRefactorisation = 0;
79  options->enableEqualities = BT_FALSE;
80 
81  #ifdef __USE_SINGLE_PRECISION__
82  options->terminationTolerance = 1.0e2 * EPS;
83  options->boundTolerance = 1.0e2 * EPS;
84  #else
85  options->terminationTolerance = 5.0e6 * EPS;
86  options->boundTolerance = 1.0e6 * EPS;
87  #endif
88  options->boundRelaxation = 1.0e4;
89  #ifdef __USE_SINGLE_PRECISION__
90  options->epsNum = -1.0e2 * EPS;
91  options->epsDen = 1.0e2 * EPS;
92  #else
93  options->epsNum = -1.0e3 * EPS;
94  options->epsDen = 1.0e3 * EPS;
95  #endif
96  options->maxPrimalJump = 1.0e8;
97  options->maxDualJump = 1.0e8;
98 
99  options->initialRamping = 0.5;
100  options->finalRamping = 1.0;
101  options->initialFarBounds = 1.0e6;
102  options->growFarBounds = 1.0e3;
103  options->initialStatusBounds = ST_LOWER;
104  #ifdef __USE_SINGLE_PRECISION__
105  options->epsFlipping = 5.0e1 * EPS;
106  #else
107  options->epsFlipping = 1.0e3 * EPS;
108  #endif
109  options->numRegularisationSteps = 0;
110  #ifdef __USE_SINGLE_PRECISION__
111  options->epsRegularisation = 2.0e1 * EPS;
112  options->numRefinementSteps = 2;
113  #else
114  options->epsRegularisation = 1.0e3 * EPS;
115  options->numRefinementSteps = 1;
116  #endif
117  options->epsIterRef = 1.0e2 * EPS;
118  #ifdef __USE_SINGLE_PRECISION__
119  options->epsLITests = 5.0e1 * EPS;
120  options->epsNZCTests = 1.0e2 * EPS;
121  #else
122  options->epsLITests = 1.0e5 * EPS;
123  options->epsNZCTests = 3.0e3 * EPS;
124  #endif
125 
126  options->enableDropInfeasibles = BT_FALSE;
127  options->dropBoundPriority = 1;
128  options->dropEqConPriority = 1;
129  options->dropIneqConPriority = 1;
130 
131 
132  switch ( mode )
133  {
134  case 0:
135  /* default, already set */
136  break;
137 
138 
139  case 1:
140  /* reliable */
141  options->enableFullLITests = BT_TRUE;
142  options->enableCholeskyRefactorisation = 1;
143 
144  #ifdef __USE_SINGLE_PRECISION__
145  options->numRefinementSteps = 3;
146  #else
147  options->numRefinementSteps = 2;
148  #endif
149 
150 
151  case 2:
152  /* MPC */
153  options->enableRamping = BT_FALSE;
154  options->enableFarBounds = BT_TRUE;
155  options->enableFlippingBounds = BT_FALSE;
156  options->enableRegularisation = BT_TRUE;
157  options->enableNZCTests = BT_FALSE;
158  options->enableDriftCorrection = 0;
159  options->enableEqualities = BT_TRUE;
160 
161  #ifdef __USE_SINGLE_PRECISION__
162  options->terminationTolerance = 1.0e3 * EPS;
163  #else
164  options->terminationTolerance = 1.0e9 * EPS;
165  #endif
166 
167  options->initialStatusBounds = ST_INACTIVE;
168  options->numRegularisationSteps = 1;
169  #ifdef __USE_SINGLE_PRECISION__
170  options->numRefinementSteps = 2;
171  #else
172  options->numRefinementSteps = 0;
173  #endif
174  }
175 
176  return 0;
177 }
178 
179 
180 /*
181  * q p O A S E S _ O p t i o n s _ c o p y
182  */
184  Options* const to
185  )
186 {
187  if ( ( from == 0 ) || ( to == 0 ) )
188  return -1;
189 
190 
191  to->printLevel = (PrintLevel)(from->printLevel);
192 
193  to->enableRamping = (BooleanType)(from->enableRamping);
202 
204  to->boundTolerance = from->boundTolerance;
205  to->boundRelaxation = from->boundRelaxation;
206  to->epsNum = from->epsNum;
207  to->epsDen = from->epsDen;
208  to->maxPrimalJump = from->maxPrimalJump;
209  to->maxDualJump = from->maxDualJump;
210 
211  to->initialRamping = from->initialRamping;
212  to->finalRamping = from->finalRamping;
214  to->growFarBounds = from->growFarBounds;
216  to->epsFlipping = from->epsFlipping;
220  to->epsIterRef = from->epsIterRef;
221  to->epsLITests = from->epsLITests;
222  to->epsNZCTests = from->epsNZCTests;
223 
228 
229  return 0;
230 }
231 
232 
233 
234 /*
235  * q p O A S E S _ o b t a i n O u t p u t s
236  */
237 int_t qpOASES_obtainOutputs( const QProblemB* const globalQpObject,
238  returnValue returnvalue,
239  real_t* const x,
240  real_t* const y,
241  real_t* const obj,
242  int_t* const status
243  )
244 {
245  if ( globalQpObject == 0 )
246  return -1;
247 
248  globalQpObject->getPrimalSolution( x );
249  globalQpObject->getDualSolution( y );
250  *obj = globalQpObject->getObjVal( );
251  *status = getSimpleStatus( returnvalue );
252 
253  return 0;
254 }
255 
256 
257 
258 /*
259  * Q P r o b l e m _ s e t u p
260  */
262  int_t nC,
263  int_t hessianType
264  )
265 {
266  if ( ( nV < 1 ) || ( nC < 0 ) )
267  return -1;
268 
269  if ( ( hessianType < 0 ) || ( hessianType > 6 ) )
270  return -1;
271 
272  if ( QProblem_cleanup() != 0 )
273  return -1;
274 
275  globalQProblemObject = new QProblem( nV,nC,(HessianType)hessianType );
276 
277  return 0;
278 }
279 
280 
281 /*
282  * Q P r o b l e m _ i n i t
283  */
284 int_t QProblem_init( const real_t* const H,
285  const real_t* const g,
286  const real_t* const A,
287  const real_t* const lb,
288  const real_t* const ub,
289  const real_t* const lbA,
290  const real_t* const ubA,
291  int_t* const nWSR,
292  real_t* const cputime,
293  const qpOASES_Options* const options,
294  real_t* const x,
295  real_t* const y,
296  real_t* const obj,
297  int_t* const status
298  )
299 {
300  /* abort if QProblem_setup has not been called */
301  if ( globalQProblemObject == 0 )
302  return -1;
303 
304  /* adjust options if provided */
305  if ( options != 0 )
306  {
307  qpOASES_Options_copy( options,&globalOptionsObject );
308  globalQProblemObject->setOptions( globalOptionsObject );
309  }
310 
311  /* actually call solver */
312  returnValue returnvalue = globalQProblemObject->init( H,g,A,lb,ub,lbA,ubA, *nWSR,cputime );
313 
314  /* assign lhs arguments */
315  return qpOASES_obtainOutputs( globalQProblemObject,returnvalue, x,y,obj,status );
316 }
317 
318 
319 /*
320  * Q P r o b l e m _ h o t s t a r t
321  */
323  const real_t* const lb,
324  const real_t* const ub,
325  const real_t* const lbA,
326  const real_t* const ubA,
327  int_t* const nWSR,
328  real_t* const cputime,
329  real_t* const x,
330  real_t* const y,
331  real_t* const obj,
332  int_t* const status
333  )
334 {
335  /* abort if QProblem_setup has not been called */
336  if ( globalQProblemObject == 0 )
337  return -1;
338 
339  /* actually call solver */
340  returnValue returnvalue = globalQProblemObject->hotstart( g,lb,ub,lbA,ubA, *nWSR,cputime );
341 
342  /* assign lhs arguments */
343  return qpOASES_obtainOutputs( globalQProblemObject,returnvalue, x,y,obj,status );
344 
345  return 0;
346 }
347 
348 
349 /*
350  * Q P r o b l e m _ c l e a n u p
351  */
353 {
354  if ( globalQProblemObject != 0 )
355  {
356  delete globalQProblemObject;
357  globalQProblemObject = 0;
358  }
359 
360  return 0;
361 }
362 
363 
364 
365 /*
366  * Q P r o b l e m B _ s e t u p
367  */
369  int_t hessianType
370  )
371 {
372  if ( nV < 1 )
373  return -1;
374 
375  if ( ( hessianType < 0 ) || ( hessianType > 6 ) )
376  return -1;
377 
378  if ( QProblemB_cleanup() != 0 )
379  return -1;
380 
381  globalQProblemBObject = new QProblemB( nV,(HessianType)hessianType );
382 
383  return 0;
384 }
385 
386 
387 /*
388  * Q P r o b l e m B _ i n i t
389  */
390 int_t QProblemB_init( const real_t* const H,
391  const real_t* const g,
392  const real_t* const lb,
393  const real_t* const ub,
394  int_t* const nWSR,
395  real_t* const cputime,
396  const qpOASES_Options* const options,
397  real_t* const x,
398  real_t* const y,
399  real_t* const obj,
400  int_t* const status
401  )
402 {
403  /* abort if QProblemB_setup has not been called */
404  if ( globalQProblemBObject == 0 )
405  return -1;
406 
407  /* adjust options if provided */
408  if ( options != 0 )
409  {
410  qpOASES_Options_copy( options,&globalOptionsObject );
411  globalQProblemBObject->setOptions( globalOptionsObject );
412  }
413 
414  /* actually call solver */
415  returnValue returnvalue = globalQProblemBObject->init( H,g,lb,ub, *nWSR,cputime );
416 
417  /* assign lhs arguments */
418  return qpOASES_obtainOutputs( globalQProblemBObject,returnvalue, x,y,obj,status );
419 }
420 
421 
422 /*
423  * Q P r o b l e m B _ h o t s t a r t
424  */
426  const real_t* const lb,
427  const real_t* const ub,
428  int_t* const nWSR,
429  real_t* const cputime,
430  real_t* const x,
431  real_t* const y,
432  real_t* const obj,
433  int_t* const status
434  )
435 {
436  /* abort if QProblemB_setup has not been called */
437  if ( globalQProblemBObject == 0 )
438  return -1;
439 
440  /* actually call solver */
441  returnValue returnvalue = globalQProblemBObject->hotstart( g,lb,ub, *nWSR,cputime );
442 
443  /* assign lhs arguments */
444  return qpOASES_obtainOutputs( globalQProblemBObject,returnvalue, x,y,obj,status );
445 
446  return 0;
447 }
448 
449 
450 /*
451  * Q P r o b l e m B _ c l e a n u p
452  */
454 {
455  if ( globalQProblemBObject != 0 )
456  {
457  delete globalQProblemBObject;
458  globalQProblemBObject = 0;
459  }
460 
461  return 0;
462 }
463 
464 
465 
466 /*
467  * S Q P r o b l e m _ s e t u p
468  */
470  int_t nC,
471  int_t hessianType
472  )
473 {
474  if ( ( nV < 1 ) || ( nC < 0 ) )
475  return -1;
476 
477  if ( ( hessianType < 0 ) || ( hessianType > 6 ) )
478  return -1;
479 
480  if ( SQProblem_cleanup() != 0 )
481  return -1;
482 
483  globalSQProblemObject = new SQProblem( nV,nC,(HessianType)hessianType );
484 
485  return 0;
486 }
487 
488 
489 /*
490  * S Q P r o b l e m _ i n i t
491  */
492 int_t SQProblem_init( const real_t* const H,
493  const real_t* const g,
494  const real_t* const A,
495  const real_t* const lb,
496  const real_t* const ub,
497  const real_t* const lbA,
498  const real_t* const ubA,
499  int_t* const nWSR,
500  real_t* const cputime,
501  const qpOASES_Options* const options,
502  real_t* const x,
503  real_t* const y,
504  real_t* const obj,
505  int_t* const status
506  )
507 {
508  /* abort if SQProblem_setup has not been called */
509  if ( globalSQProblemObject == 0 )
510  return -1;
511 
512  /* adjust options if provided */
513  if ( options != 0 )
514  {
515  qpOASES_Options_copy( options,&globalOptionsObject );
516  globalSQProblemObject->setOptions( globalOptionsObject );
517  }
518 
519  /* actually call solver */
520  returnValue returnvalue = globalSQProblemObject->init( H,g,A,lb,ub,lbA,ubA, *nWSR,cputime );
521 
522  /* assign lhs arguments */
523  return qpOASES_obtainOutputs( globalSQProblemObject,returnvalue, x,y,obj,status );
524 }
525 
526 
527 /*
528  * S Q P r o b l e m _ h o t s t a r t
529  */
531  const real_t* const g,
532  const real_t* const A,
533  const real_t* const lb,
534  const real_t* const ub,
535  const real_t* const lbA,
536  const real_t* const ubA,
537  int_t* const nWSR,
538  real_t* const cputime,
539  real_t* const x,
540  real_t* const y,
541  real_t* const obj,
542  int_t* const status
543  )
544 {
545  /* abort if SQProblem_setup has not been called */
546  if ( globalSQProblemObject == 0 )
547  return -1;
548 
549  /* actually call solver */
550  returnValue returnvalue = globalSQProblemObject->hotstart( H,g,A,lb,ub,lbA,ubA, *nWSR,cputime );
551 
552  /* assign lhs arguments */
553  return qpOASES_obtainOutputs( globalSQProblemObject,returnvalue, x,y,obj,status );
554 
555  return 0;
556 }
557 
558 
559 /*
560  * S Q P r o b l e m _ c l e a n u p
561  */
563 {
564  if ( globalSQProblemObject != 0 )
565  {
566  delete globalSQProblemObject;
567  globalSQProblemObject = 0;
568  }
569 
570  return 0;
571 }
572 
573 
574 /*
575  * end of file
576  */
int_t SQProblem_init(const real_t *const H, const real_t *const g, const real_t *const A, const real_t *const lb, const real_t *const ub, const real_t *const lbA, const real_t *const ubA, int_t *const nWSR, real_t *const cputime, const qpOASES_Options *const options, real_t *const x, real_t *const y, real_t *const obj, int_t *const status)
#define ST_LOWER
int_t enableCholeskyRefactorisation
Implements the online active set strategy for box-constrained QPs.
Implements the online active set strategy for QPs with varying matrices.
BEGIN_NAMESPACE_ACADO const double EPS
Manages all user-specified options for solving QPs.
int_t qpOASES_obtainOutputs(const QProblemB *const globalQpObject, returnValue returnvalue, real_t *const x, real_t *const y, real_t *const obj, int_t *const status)
Allows to pass back messages to the calling function.
int_t QProblemB_cleanup()
USING_NAMESPACE_QPOASES int_t qpOASES_Options_init(qpOASES_Options *const options, int_t mode)
#define PL_MEDIUM
int_t QProblemB_setup(int_t nV, int_t hessianType)
int_t QProblem_cleanup()
#define PL_NONE
#define ST_INACTIVE
#define PL_HIGH
int_t qpOASES_Options_copy(const qpOASES_Options *const from, Options *const to)
int_t QProblem_setup(int_t nV, int_t nC, int_t hessianType)
int_t QProblemB_hotstart(const real_t *const g, const real_t *const lb, const real_t *const ub, int_t *const nWSR, real_t *const cputime, real_t *const x, real_t *const y, real_t *const obj, int_t *const status)
Provides a generic way to set and pass user-specified options.
Definition: options.hpp:65
int_t SQProblem_setup(int_t nV, int_t nC, int_t hessianType)
int_t QProblem_init(const real_t *const H, const real_t *const g, const real_t *const A, const real_t *const lb, const real_t *const ub, const real_t *const lbA, const real_t *const ubA, int_t *const nWSR, real_t *const cputime, const qpOASES_Options *const options, real_t *const x, real_t *const y, real_t *const obj, int_t *const status)
PrintLevel
int_t numRegularisationSteps
int_t SQProblem_cleanup()
#define BT_TRUE
Definition: acado_types.hpp:47
int_t QProblemB_init(const real_t *const H, const real_t *const g, const real_t *const lb, const real_t *const ub, int_t *const nWSR, real_t *const cputime, const qpOASES_Options *const options, real_t *const x, real_t *const y, real_t *const obj, int_t *const status)
int_t SQProblem_hotstart(const real_t *const H, const real_t *const g, const real_t *const A, const real_t *const lb, const real_t *const ub, const real_t *const lbA, const real_t *const ubA, int_t *const nWSR, real_t *const cputime, real_t *const x, real_t *const y, real_t *const obj, int_t *const status)
real_t terminationTolerance
#define BT_FALSE
Definition: acado_types.hpp:49
int_t QProblem_hotstart(const real_t *const g, const real_t *const lb, const real_t *const ub, const real_t *const lbA, const real_t *const ubA, int_t *const nWSR, real_t *const cputime, real_t *const x, real_t *const y, real_t *const obj, int_t *const status)
double real_t
Definition: AD_test.c:10
int_t getSimpleStatus(returnValue returnvalue, BooleanType doPrintStatus=BT_FALSE)
Implements the online active set strategy for QPs with general constraints.


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