1 /*
2 * This file is part of qpOASES.
3 *
4 * qpOASES -- An Implementation of the Online Active Set Strategy.
5 * Copyright (C) 2007-2017 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
25 /**
26 * \file interfaces/c/qpOASES_wrapper.cpp
27 * \author Hans Joachim Ferreau
28 * \version 3.2
29 * \date 2014-2017
30 *
31 * Interface that enables to call qpOASES from plain C.
32 *
33 */
34
35
36 #include <qpOASES.hpp>
37
38 USING_NAMESPACE_QPOASES
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 */
51 int_t qpOASES_Options_init( qpOASES_Options* const options,
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 */
qpOASES_Options_copy(const qpOASES_Options * const from,Options * const to)183 int_t qpOASES_Options_copy( const qpOASES_Options* const from,
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);
194 to->enableFarBounds = (BooleanType)(from->enableFarBounds);
195 to->enableFlippingBounds = (BooleanType)(from->enableFlippingBounds);
196 to->enableRegularisation = (BooleanType)(from->enableRegularisation);
197 to->enableFullLITests = (BooleanType)(from->enableFullLITests);
198 to->enableNZCTests = (BooleanType)(from->enableNZCTests);
199 to->enableDriftCorrection = from->enableDriftCorrection;
200 to->enableCholeskyRefactorisation = from->enableCholeskyRefactorisation;
201 to->enableEqualities = (BooleanType)(from->enableEqualities);
202
203 to->terminationTolerance = from->terminationTolerance;
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;
213 to->initialFarBounds = from->initialFarBounds;
214 to->growFarBounds = from->growFarBounds;
215 to->initialStatusBounds = (SubjectToStatus)(from->initialStatusBounds);
216 to->epsFlipping = from->epsFlipping;
217 to->numRegularisationSteps = from->numRegularisationSteps;
218 to->epsRegularisation = from->epsRegularisation;
219 to->numRefinementSteps = from->numRefinementSteps;
220 to->epsIterRef = from->epsIterRef;
221 to->epsLITests = from->epsLITests;
222 to->epsNZCTests = from->epsNZCTests;
223
224 to->enableDropInfeasibles = (BooleanType)(from->enableDropInfeasibles);
225 to->dropBoundPriority = from->dropBoundPriority;
226 to->dropEqConPriority = from->dropEqConPriority;
227 to->dropIneqConPriority = from->dropIneqConPriority;
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 */
qpOASES_obtainOutputs(const QProblemB * const globalQpObject,returnValue returnvalue,real_t * const x,real_t * const y,real_t * const obj,int_t * const status)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 */
QProblem_setup(int_t nV,int_t nC,int_t hessianType)261 int_t QProblem_setup( int_t nV,
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 */
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)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 */
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)322 int_t QProblem_hotstart( const real_t* const g,
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 */
QProblem_cleanup()352 int_t QProblem_cleanup( )
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 */
QProblemB_setup(int_t nV,int_t hessianType)368 int_t QProblemB_setup( int_t nV,
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 */
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)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 */
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)425 int_t QProblemB_hotstart( const real_t* const g,
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 */
QProblemB_cleanup()453 int_t QProblemB_cleanup( )
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 */
SQProblem_setup(int_t nV,int_t nC,int_t hessianType)469 int_t SQProblem_setup( int_t nV,
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 */
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)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 */
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)530 int_t SQProblem_hotstart( const real_t* const H,
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 */
SQProblem_cleanup()562 int_t SQProblem_cleanup( )
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 */
577