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/octave/qpOASES_sequence.cpp
27 * \author Hans Joachim Ferreau, Christian Kirches, Andreas Potschka, Alexander Buchner
28 * \version 3.2
29 * \date 2007-2017
30 *
31 * Interface for octave that enables to call qpOASES as a MEX function
32 * (variant for solving QP sequences).
33 *
34 */
35
36
37
38 #include <qpOASES.hpp>
39
40
41 USING_NAMESPACE_QPOASES
42
43
44 #include "qpOASES_octave_utils.hpp"
45
46 /** initialise handle counter of QPInstance class */
47 int_t QPInstance::s_nexthandle = 1;
48
49 /** global pointer to QP objects */
50 static std::vector<QPInstance *> g_instances;
51
52 #include "qpOASES_octave_utils.cpp"
53
54
55 /*
56 * Q P r o b l e m B _ i n i t
57 */
QProblemB_init(int_t handle,SymmetricMatrix * H,real_t * g,const real_t * const lb,const real_t * const ub,int_t nWSRin,real_t maxCpuTimeIn,const double * const x0,Options * options,int_t nOutputs,mxArray * plhs[],const double * const guessedBounds,const double * const _R)58 int_t QProblemB_init( int_t handle,
59 SymmetricMatrix* H, real_t* g,
60 const real_t* const lb, const real_t* const ub,
61 int_t nWSRin, real_t maxCpuTimeIn,
62 const double* const x0, Options* options,
63 int_t nOutputs, mxArray* plhs[],
64 const double* const guessedBounds,
65 const double* const _R
66 )
67 {
68 int_t nWSRout = nWSRin;
69 real_t maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
70
71 /* 1) setup initial QP. */
72 QProblemB* globalQPB = getQPInstance(handle)->qpb;
73
74 if ( globalQPB == 0 )
75 {
76 myMexErrMsgTxt( "ERROR (qpOASES): Invalid handle to QP instance!" );
77 return -1;
78 }
79
80 globalQPB->setOptions( *options );
81
82 /* 2) Solve initial QP. */
83 returnValue returnvalue;
84 int_t nV = globalQPB->getNV();
85
86 /* 3) Fill the working set. */
87 Bounds bounds(nV);
88 if (guessedBounds != 0) {
89 for (int_t i = 0; i < nV; i++) {
90 if ( isEqual(guessedBounds[i],-1.0) == BT_TRUE ) {
91 bounds.setupBound(i, ST_LOWER);
92 } else if ( isEqual(guessedBounds[i],1.0) == BT_TRUE ) {
93 bounds.setupBound(i, ST_UPPER);
94 } else if ( isEqual(guessedBounds[i],0.0) == BT_TRUE ) {
95 bounds.setupBound(i, ST_INACTIVE);
96 } else {
97 char msg[MAX_STRING_LENGTH];
98 snprintf(msg, MAX_STRING_LENGTH,
99 "ERROR (qpOASES): Only {-1, 0, 1} allowed for status of bounds!");
100 myMexErrMsgTxt(msg);
101 return -1;
102 }
103 }
104 }
105
106 returnvalue = globalQPB->init( H,g,lb,ub,
107 nWSRout,&maxCpuTimeOut,
108 x0,0,
109 (guessedBounds != 0) ? &bounds : 0,
110 _R
111 );
112
113 /* 3) Assign lhs arguments. */
114 obtainOutputs( 0,globalQPB,returnvalue,nWSRout,maxCpuTimeOut,
115 nOutputs,plhs,nV,0,handle );
116
117 return 0;
118 }
119
120
121 /*
122 * S Q P r o b l e m _ i n i t
123 */
SQProblem_init(int_t handle,SymmetricMatrix * H,real_t * g,Matrix * A,const real_t * const lb,const real_t * const ub,const real_t * const lbA,const real_t * const ubA,int_t nWSRin,real_t maxCpuTimeIn,const double * const x0,Options * options,int_t nOutputs,mxArray * plhs[],const double * const guessedBounds,const double * const guessedConstraints,const double * const _R)124 int_t SQProblem_init( int_t handle,
125 SymmetricMatrix* H, real_t* g, Matrix* A,
126 const real_t* const lb, const real_t* const ub,
127 const real_t* const lbA, const real_t* const ubA,
128 int_t nWSRin, real_t maxCpuTimeIn,
129 const double* const x0, Options* options,
130 int_t nOutputs, mxArray* plhs[],
131 const double* const guessedBounds, const double* const guessedConstraints,
132 const double* const _R
133 )
134 {
135 int_t nWSRout = nWSRin;
136 real_t maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
137
138 /* 1) setup initial QP. */
139 SQProblem* globalSQP = getQPInstance(handle)->sqp;
140
141 if ( globalSQP == 0 )
142 {
143 myMexErrMsgTxt( "ERROR (qpOASES): Invalid handle to QP instance!" );
144 return -1;
145 }
146
147 globalSQP->setOptions( *options );
148
149 /* 2) Solve initial QP. */
150 returnValue returnvalue;
151 int_t nV = globalSQP->getNV();
152 int_t nC = globalSQP->getNC();
153
154 /* 3) Fill the working set. */
155 Bounds bounds(nV);
156 Constraints constraints(nC);
157 if (guessedBounds != 0) {
158 for (int_t i = 0; i < nV; i++) {
159 if ( isEqual(guessedBounds[i],-1.0) == BT_TRUE ) {
160 bounds.setupBound(i, ST_LOWER);
161 } else if ( isEqual(guessedBounds[i],1.0) == BT_TRUE ) {
162 bounds.setupBound(i, ST_UPPER);
163 } else if ( isEqual(guessedBounds[i],0.0) == BT_TRUE ) {
164 bounds.setupBound(i, ST_INACTIVE);
165 } else {
166 char msg[MAX_STRING_LENGTH];
167 snprintf(msg, MAX_STRING_LENGTH,
168 "ERROR (qpOASES): Only {-1, 0, 1} allowed for status of bounds!");
169 myMexErrMsgTxt(msg);
170 return -1;
171 }
172 }
173 }
174
175 if (guessedConstraints != 0) {
176 for (int_t i = 0; i < nC; i++) {
177 if ( isEqual(guessedConstraints[i],-1.0) == BT_TRUE ) {
178 constraints.setupConstraint(i, ST_LOWER);
179 } else if ( isEqual(guessedConstraints[i],1.0) == BT_TRUE ) {
180 constraints.setupConstraint(i, ST_UPPER);
181 } else if ( isEqual(guessedConstraints[i],0.0) == BT_TRUE ) {
182 constraints.setupConstraint(i, ST_INACTIVE);
183 } else {
184 char msg[MAX_STRING_LENGTH];
185 snprintf(msg, MAX_STRING_LENGTH,
186 "ERROR (qpOASES): Only {-1, 0, 1} allowed for status of constraints!");
187 myMexErrMsgTxt(msg);
188 return -1;
189 }
190 }
191 }
192
193 returnvalue = globalSQP->init( H,g,A,lb,ub,lbA,ubA,
194 nWSRout,&maxCpuTimeOut,
195 x0,0,
196 (guessedBounds != 0) ? &bounds : 0, (guessedConstraints != 0) ? &constraints : 0,
197 _R
198 );
199
200 /* 3) Assign lhs arguments. */
201 obtainOutputs( 0,globalSQP,returnvalue,nWSRout,maxCpuTimeOut,
202 nOutputs,plhs,nV,nC,handle );
203
204 return 0;
205 }
206
207
208
209 /*
210 * Q P r o b l e m B _ h o t s t a r t
211 */
QProblemB_hotstart(int_t handle,const real_t * const g,const real_t * const lb,const real_t * const ub,int_t nWSRin,real_t maxCpuTimeIn,Options * options,int_t nOutputs,mxArray * plhs[])212 int_t QProblemB_hotstart( int_t handle,
213 const real_t* const g,
214 const real_t* const lb, const real_t* const ub,
215 int_t nWSRin, real_t maxCpuTimeIn,
216 Options* options,
217 int_t nOutputs, mxArray* plhs[]
218 )
219 {
220 int_t nWSRout = nWSRin;
221 real_t maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
222
223 QProblemB* globalQPB = getQPInstance(handle)->qpb;
224
225 if ( globalQPB == 0 )
226 {
227 myMexErrMsgTxt( "ERROR (qpOASES): QP needs to be initialised first!" );
228 return -1;
229 }
230
231 int_t nV = globalQPB->getNV();
232
233 /* 1) Solve QP with given options. */
234 globalQPB->setOptions( *options );
235 returnValue returnvalue = globalQPB->hotstart( g,lb,ub, nWSRout,&maxCpuTimeOut );
236
237 /* 2) Assign lhs arguments. */
238 obtainOutputs( 0,globalQPB,returnvalue,nWSRout,maxCpuTimeOut,
239 nOutputs,plhs,nV,0 );
240
241 return 0;
242 }
243
244
245 /*
246 * Q P r o b l e m _ h o t s t a r t
247 */
QProblem_hotstart(int_t handle,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 nWSRin,real_t maxCpuTimeIn,Options * options,int_t nOutputs,mxArray * plhs[])248 int_t QProblem_hotstart( int_t handle,
249 const real_t* const g,
250 const real_t* const lb, const real_t* const ub,
251 const real_t* const lbA, const real_t* const ubA,
252 int_t nWSRin, real_t maxCpuTimeIn,
253 Options* options,
254 int_t nOutputs, mxArray* plhs[]
255 )
256 {
257 int_t nWSRout = nWSRin;
258 real_t maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
259
260 QProblem* globalSQP = getQPInstance(handle)->sqp;
261
262 if ( globalSQP == 0 )
263 {
264 myMexErrMsgTxt( "ERROR (qpOASES): QP needs to be initialised first!" );
265 return -1;
266 }
267
268 int_t nV = globalSQP->getNV();
269 int_t nC = globalSQP->getNC();
270
271 /* 1) Solve QP with given options. */
272 globalSQP->setOptions( *options );
273 returnValue returnvalue = globalSQP->hotstart( g,lb,ub,lbA,ubA, nWSRout,&maxCpuTimeOut );
274
275 /* 2) Assign lhs arguments. */
276 obtainOutputs( 0,globalSQP,returnvalue,nWSRout,maxCpuTimeOut,
277 nOutputs,plhs,nV,nC );
278
279 return 0;
280 }
281
282
283 /*
284 * S Q P r o b l e m _ h o t s t a r t
285 */
SQProblem_hotstart(int_t handle,SymmetricMatrix * H,real_t * g,Matrix * A,const real_t * const lb,const real_t * const ub,const real_t * const lbA,const real_t * const ubA,int_t nWSRin,real_t maxCpuTimeIn,Options * options,int_t nOutputs,mxArray * plhs[])286 int_t SQProblem_hotstart( int_t handle,
287 SymmetricMatrix* H, real_t* g, Matrix* A,
288 const real_t* const lb, const real_t* const ub, const real_t* const lbA, const real_t* const ubA,
289 int_t nWSRin, real_t maxCpuTimeIn,
290 Options* options,
291 int_t nOutputs, mxArray* plhs[]
292 )
293 {
294 int_t nWSRout = nWSRin;
295 real_t maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
296
297 SQProblem* globalSQP = getQPInstance(handle)->sqp;
298
299 if ( globalSQP == 0 )
300 {
301 myMexErrMsgTxt( "ERROR (qpOASES): QP needs to be initialised first!" );
302 return -1;
303 }
304
305 int_t nV = globalSQP->getNV();
306 int_t nC = globalSQP->getNC();
307
308 /* 1) Solve QP. */
309 globalSQP->setOptions( *options );
310 returnValue returnvalue = globalSQP->hotstart( H,g,A,lb,ub,lbA,ubA, nWSRout,&maxCpuTimeOut );
311
312 switch (returnvalue)
313 {
314 case SUCCESSFUL_RETURN:
315 case RET_QP_UNBOUNDED:
316 case RET_QP_INFEASIBLE:
317 break;
318
319 default:
320 myMexErrMsgTxt( "ERROR (qpOASES): Hotstart failed." );
321 return -1;
322 }
323
324 /* 2) Assign lhs arguments. */
325 obtainOutputs( 0,globalSQP,returnvalue,nWSRout,maxCpuTimeOut,
326 nOutputs,plhs,nV,nC );
327
328 return 0;
329 }
330
331
332
333 /*
334 * m e x F u n c t i o n
335 */
mexFunction(int nlhs,mxArray * plhs[],int nrhs,const mxArray * prhs[])336 void mexFunction( int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[] )
337 {
338 /* inputs */
339 char typeString[2];
340
341 real_t *g=0, *lb=0, *ub=0, *lbA=0, *ubA=0;
342 HessianType hessianType = HST_UNKNOWN;
343 double *x0=0, *R=0, *R_for=0;
344 double *guessedBounds=0, *guessedConstraints=0;
345
346 int_t H_idx=-1, g_idx=-1, A_idx=-1, lb_idx=-1, ub_idx=-1, lbA_idx=-1, ubA_idx=-1;
347 int_t x0_idx=-1, auxInput_idx=-1;
348
349 BooleanType isSimplyBoundedQp = BT_FALSE;
350
351 Options options;
352 options.printLevel = PL_LOW;
353 #ifdef __DEBUG__
354 options.printLevel = PL_HIGH;
355 #endif
356 #ifdef __SUPPRESSANYOUTPUT__
357 options.printLevel = PL_NONE;
358 #endif
359
360 /* dimensions */
361 uint_t nV=0, nC=0, handle=0;
362 int_t nWSRin;
363 real_t maxCpuTimeIn = -1.0;
364 QPInstance* globalQP = 0;
365
366 /* I) CONSISTENCY CHECKS: */
367 /* 1) Ensure that qpOASES is called with a feasible number of input arguments. */
368 if ( ( nrhs < 5 ) || ( nrhs > 10 ) )
369 {
370 if ( nrhs != 2 )
371 {
372 myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
373 return;
374 }
375 }
376
377 /* 2) Ensure that first input is a string ... */
378 if ( mxIsChar( prhs[0] ) != 1 )
379 {
380 myMexErrMsgTxt( "ERROR (qpOASES): First input argument must be a string!" );
381 return;
382 }
383
384 mxGetString( prhs[0], typeString, 2 );
385
386 /* ... and if so, check if it is an allowed one. */
387 if ( ( strcmp( typeString,"i" ) != 0 ) && ( strcmp( typeString,"I" ) != 0 ) &&
388 ( strcmp( typeString,"h" ) != 0 ) && ( strcmp( typeString,"H" ) != 0 ) &&
389 ( strcmp( typeString,"m" ) != 0 ) && ( strcmp( typeString,"M" ) != 0 ) &&
390 ( strcmp( typeString,"e" ) != 0 ) && ( strcmp( typeString,"E" ) != 0 ) &&
391 ( strcmp( typeString,"c" ) != 0 ) && ( strcmp( typeString,"C" ) != 0 ) )
392 {
393 myMexErrMsgTxt( "ERROR (qpOASES): Undefined first input argument!\nType 'help qpOASES_sequence' for further information." );
394 return;
395 }
396
397
398 /* II) SELECT RESPECTIVE QPOASES FUNCTION CALL: */
399 /* 1) Init (without or with initial guess for primal solution). */
400 if ( ( strcmp( typeString,"i" ) == 0 ) || ( strcmp( typeString,"I" ) == 0 ) )
401 {
402 /* consistency checks */
403 if ( ( nlhs < 1 ) || ( nlhs > 7 ) )
404 {
405 myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
406 return;
407 }
408
409 if ( ( nrhs < 5 ) || ( nrhs > 10 ) )
410 {
411 myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
412 return;
413 }
414
415 g_idx = 2;
416
417 if ( mxIsEmpty(prhs[1]) == 1 )
418 {
419 H_idx = -1;
420 nV = (uint_t)mxGetM( prhs[ g_idx ] ); /* row number of Hessian matrix */
421 }
422 else
423 {
424 H_idx = 1;
425 nV = (uint_t)mxGetM( prhs[ H_idx ] ); /* row number of Hessian matrix */
426 }
427
428
429 /* ensure that data is given in double precision */
430 if ( ( ( H_idx >= 0 ) && ( mxIsDouble( prhs[ H_idx ] ) == 0 ) ) ||
431 ( mxIsDouble( prhs[ g_idx ] ) == 0 ) )
432 {
433 myMexErrMsgTxt( "ERROR (qpOASES): All data has to be provided in double precision!" );
434 return;
435 }
436
437 if ( ( H_idx >= 0 ) && ( ( mxGetN( prhs[ H_idx ] ) != nV ) || ( mxGetM( prhs[ H_idx ] ) != nV ) ) )
438 {
439 myMexErrMsgTxt( "ERROR (qpOASES): Hessian matrix dimension mismatch!" );
440 return;
441 }
442
443
444 /* Check for 'Inf' and 'Nan' in Hessian */
445 if (containsNaNorInf( prhs,H_idx, 0 ) == BT_TRUE)
446 return;
447
448 /* Check for 'Inf' and 'Nan' in gradient */
449 if (containsNaNorInf(prhs,g_idx, 0 ) == BT_TRUE)
450 return;
451
452 /* determine whether is it a simply bounded QP */
453 if ( nrhs <= 7 )
454 isSimplyBoundedQp = BT_TRUE;
455 else
456 isSimplyBoundedQp = BT_FALSE;
457
458 if ( isSimplyBoundedQp == BT_TRUE )
459 {
460 lb_idx = 3;
461 ub_idx = 4;
462
463 if (containsNaNorInf( prhs,lb_idx, 1 ) == BT_TRUE)
464 return;
465
466 if (containsNaNorInf( prhs,ub_idx, 1 ) == BT_TRUE)
467 return;
468
469 /* Check inputs dimensions and assign pointers to inputs. */
470 nC = 0; /* row number of constraint matrix */
471
472
473 if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,2 ) != SUCCESSFUL_RETURN )
474 return;
475
476 if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,3 ) != SUCCESSFUL_RETURN )
477 return;
478
479 if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,4 ) != SUCCESSFUL_RETURN )
480 return;
481
482 /* default value for nWSR */
483 nWSRin = 5*nV;
484
485 /* Check whether x0 and options are specified .*/
486 if ( nrhs >= 6 )
487 {
488 if ((!mxIsEmpty(prhs[5])) && (mxIsStruct(prhs[5])))
489 setupOptions( &options,prhs[5],nWSRin,maxCpuTimeIn );
490
491 if ( ( nrhs >= 7 ) && ( !mxIsEmpty(prhs[6]) ) )
492 {
493 /* auxInput specified */
494 if ( mxIsStruct(prhs[6]) )
495 {
496 auxInput_idx = 6;
497 x0_idx = -1;
498 }
499 else
500 {
501 auxInput_idx = -1;
502 x0_idx = 6;
503 }
504 }
505 else
506 {
507 auxInput_idx = -1;
508 x0_idx = -1;
509 }
510 }
511 }
512 else
513 {
514 A_idx = 3;
515
516 /* ensure that data is given in double precision */
517 if ( mxIsDouble( prhs[ A_idx ] ) == 0 )
518 {
519 myMexErrMsgTxt( "ERROR (qpOASES): All data has to be provided in double precision!" );
520 return;
521 }
522
523 /* Check inputs dimensions and assign pointers to inputs. */
524 nC = (uint_t)mxGetM( prhs[ A_idx ] ); /* row number of constraint matrix */
525
526 lb_idx = 4;
527 ub_idx = 5;
528 lbA_idx = 6;
529 ubA_idx = 7;
530
531 if (containsNaNorInf( prhs,A_idx, 0 ) == BT_TRUE)
532 return;
533
534 if (containsNaNorInf( prhs,lb_idx, 1 ) == BT_TRUE)
535 return;
536
537 if (containsNaNorInf( prhs,ub_idx, 1 ) == BT_TRUE)
538 return;
539
540 if (containsNaNorInf( prhs,lbA_idx, 1 ) == BT_TRUE)
541 return;
542
543 if (containsNaNorInf( prhs,ubA_idx, 1 ) == BT_TRUE)
544 return;
545
546 if ( ( mxGetN( prhs[ A_idx ] ) != 0 ) && ( mxGetN( prhs[ A_idx ] ) != nV ) )
547 {
548 myMexErrMsgTxt( "ERROR (qpOASES): Constraint matrix dimension mismatch!" );
549 return;
550 }
551
552 if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,g_idx ) != SUCCESSFUL_RETURN )
553 return;
554
555 if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,lb_idx ) != SUCCESSFUL_RETURN )
556 return;
557
558 if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,ub_idx ) != SUCCESSFUL_RETURN )
559 return;
560
561 if ( smartDimensionCheck( &lbA,nC,1, BT_TRUE,prhs,lbA_idx ) != SUCCESSFUL_RETURN )
562 return;
563
564 if ( smartDimensionCheck( &ubA,nC,1, BT_TRUE,prhs,ubA_idx ) != SUCCESSFUL_RETURN )
565 return;
566
567 /* default value for nWSR */
568 nWSRin = 5*(nV+nC);
569
570 /* Check whether x0 and options are specified .*/
571 if ( nrhs >= 9 )
572 {
573 if ((!mxIsEmpty(prhs[8])) && (mxIsStruct(prhs[8])))
574 setupOptions( &options,prhs[8],nWSRin,maxCpuTimeIn );
575
576 if ( ( nrhs >= 10 ) && ( !mxIsEmpty(prhs[9]) ) )
577 {
578 /* auxInput specified */
579 if ( mxIsStruct(prhs[9]) )
580 {
581 auxInput_idx = 9;
582 x0_idx = -1;
583 }
584 else
585 {
586 auxInput_idx = -1;
587 x0_idx = 9;
588 }
589 }
590 else
591 {
592 auxInput_idx = -1;
593 x0_idx = -1;
594 }
595 }
596 }
597
598
599 /* check dimensions and copy auxInputs */
600 if ( smartDimensionCheck( &x0,nV,1, BT_TRUE,prhs,x0_idx ) != SUCCESSFUL_RETURN )
601 return;
602
603 if ( auxInput_idx >= 0 )
604 setupAuxiliaryInputs( prhs[auxInput_idx],nV,nC, &hessianType,&x0,&guessedBounds,&guessedConstraints,&R_for );
605
606 /* convert Cholesky factor to C storage format */
607 if ( R_for != 0 )
608 {
609 R = new real_t[nV*nV];
610 convertFortranToC( R_for, nV,nV, R );
611 }
612
613 /* allocate instance */
614 handle = allocateQPInstance( nV,nC,hessianType, isSimplyBoundedQp,&options );
615 globalQP = getQPInstance( handle );
616
617 /* make a deep-copy of the user-specified Hessian matrix (possibly sparse) */
618 if ( H_idx >= 0 )
619 setupHessianMatrix( prhs[H_idx],nV, &(globalQP->H),&(globalQP->Hir),&(globalQP->Hjc),&(globalQP->Hv) );
620
621 /* make a deep-copy of the user-specified constraint matrix (possibly sparse) */
622 if ( ( nC > 0 ) && ( A_idx >= 0 ) )
623 setupConstraintMatrix( prhs[A_idx],nV,nC, &(globalQP->A),&(globalQP->Air),&(globalQP->Ajc),&(globalQP->Av) );
624
625 /* Create output vectors and assign pointers to them. */
626 allocateOutputs( nlhs,plhs, nV,nC,1,handle );
627
628 /* Call qpOASES. */
629 if ( isSimplyBoundedQp == BT_TRUE )
630 {
631 QProblemB_init( handle,
632 globalQP->H,g,
633 lb,ub,
634 nWSRin,maxCpuTimeIn,
635 x0,&options,
636 nlhs,plhs,
637 guessedBounds,R
638 );
639 }
640 else
641 {
642 SQProblem_init( handle,
643 globalQP->H,g,globalQP->A,
644 lb,ub,lbA,ubA,
645 nWSRin,maxCpuTimeIn,
646 x0,&options,
647 nlhs,plhs,
648 guessedBounds,guessedConstraints,R
649 );
650 }
651
652 if (R != 0) delete R;
653 return;
654 }
655
656 /* 2) Hotstart. */
657 if ( ( strcmp( typeString,"h" ) == 0 ) || ( strcmp( typeString,"H" ) == 0 ) )
658 {
659 /* consistency checks */
660 if ( ( nlhs < 1 ) || ( nlhs > 6 ) )
661 {
662 myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
663 return;
664 }
665
666 if ( ( nrhs < 5 ) || ( nrhs > 8 ) )
667 {
668 myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
669 return;
670 }
671
672 /* determine whether is it a simply bounded QP */
673 if ( nrhs < 7 )
674 isSimplyBoundedQp = BT_TRUE;
675 else
676 isSimplyBoundedQp = BT_FALSE;
677
678
679 if ( ( mxIsDouble( prhs[1] ) == false ) || ( mxIsScalar( prhs[1] ) == false ) )
680 {
681 myMexErrMsgTxt( "ERROR (qpOASES): Expecting a handle to QP object as second argument!\nType 'help qpOASES_sequence' for further information." );
682 return;
683 }
684
685 /* get QP instance */
686 handle = (uint_t)mxGetScalar( prhs[1] );
687 globalQP = getQPInstance( handle );
688 if ( globalQP == 0 )
689 {
690 myMexErrMsgTxt( "ERROR (qpOASES): Invalid handle to QP instance!" );
691 return;
692 }
693
694 nV = globalQP->getNV();
695
696 g_idx = 2;
697 lb_idx = 3;
698 ub_idx = 4;
699
700 if (containsNaNorInf( prhs,g_idx, 0 ) == BT_TRUE)
701 return;
702
703 if (containsNaNorInf( prhs,lb_idx, 1 ) == BT_TRUE)
704 return;
705
706 if (containsNaNorInf( prhs,ub_idx, 1 ) == BT_TRUE)
707 return;
708
709
710 /* Check inputs dimensions and assign pointers to inputs. */
711 if ( isSimplyBoundedQp == BT_TRUE )
712 {
713 nC = 0;
714
715 if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,g_idx ) != SUCCESSFUL_RETURN )
716 return;
717
718 if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,lb_idx ) != SUCCESSFUL_RETURN )
719 return;
720
721 if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,ub_idx ) != SUCCESSFUL_RETURN )
722 return;
723
724 /* default value for nWSR */
725 nWSRin = 5*nV;
726
727 /* Check whether options are specified .*/
728 if ( nrhs == 6 )
729 if ( ( !mxIsEmpty( prhs[5] ) ) && ( mxIsStruct( prhs[5] ) ) )
730 setupOptions( &options,prhs[5],nWSRin,maxCpuTimeIn );
731 }
732 else
733 {
734 nC = globalQP->getNC( );
735
736 lbA_idx = 5;
737 ubA_idx = 6;
738
739 if (containsNaNorInf( prhs,lbA_idx, 1 ) == BT_TRUE)
740 return;
741
742 if (containsNaNorInf( prhs,ubA_idx, 1 ) == BT_TRUE)
743 return;
744
745 if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,g_idx ) != SUCCESSFUL_RETURN )
746 return;
747
748 if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,lb_idx ) != SUCCESSFUL_RETURN )
749 return;
750
751 if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,ub_idx ) != SUCCESSFUL_RETURN )
752 return;
753
754 if ( smartDimensionCheck( &lbA,nC,1, BT_TRUE,prhs,lbA_idx ) != SUCCESSFUL_RETURN )
755 return;
756
757 if ( smartDimensionCheck( &ubA,nC,1, BT_TRUE,prhs,ubA_idx ) != SUCCESSFUL_RETURN )
758 return;
759
760 /* default value for nWSR */
761 nWSRin = 5*(nV+nC);
762
763 /* Check whether options are specified .*/
764 if ( nrhs == 8 )
765 if ( ( !mxIsEmpty( prhs[7] ) ) && ( mxIsStruct( prhs[7] ) ) )
766 setupOptions( &options,prhs[7],nWSRin,maxCpuTimeIn );
767 }
768
769 /* Create output vectors and assign pointers to them. */
770 allocateOutputs( nlhs,plhs, nV,nC );
771
772 /* call qpOASES */
773 if ( isSimplyBoundedQp == BT_TRUE )
774 {
775 QProblemB_hotstart( handle, g,
776 lb,ub,
777 nWSRin,maxCpuTimeIn,
778 &options,
779 nlhs,plhs
780 );
781 }
782 else
783 {
784 QProblem_hotstart( handle, g,
785 lb,ub,lbA,ubA,
786 nWSRin,maxCpuTimeIn,
787 &options,
788 nlhs,plhs
789 );
790 }
791
792 return;
793 }
794
795 /* 3) Modify matrices. */
796 if ( ( strcmp( typeString,"m" ) == 0 ) || ( strcmp( typeString,"M" ) == 0 ) )
797 {
798 /* consistency checks */
799 if ( ( nlhs < 1 ) || ( nlhs > 6 ) )
800 {
801 myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
802 return;
803 }
804
805 if ( ( nrhs < 9 ) || ( nrhs > 10 ) )
806 {
807 myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
808 return;
809 }
810
811 if ( ( mxIsDouble( prhs[1] ) == false ) || ( mxIsScalar( prhs[1] ) == false ) )
812 {
813 myMexErrMsgTxt( "ERROR (qpOASES): Expecting a handle to QP object as second argument!\nType 'help qpOASES_sequence' for further information." );
814 return;
815 }
816
817
818 /* get QP instance */
819 handle = (uint_t)mxGetScalar( prhs[1] );
820 globalQP = getQPInstance( handle );
821 if ( globalQP == 0 )
822 {
823 myMexErrMsgTxt( "ERROR (qpOASES): Invalid handle to QP instance!" );
824 return;
825 }
826
827 /* Check inputs dimensions and assign pointers to inputs. */
828 g_idx = 3;
829
830 if ( mxIsEmpty(prhs[2]) == 1 )
831 {
832 H_idx = -1;
833 nV = (uint_t)mxGetM( prhs[ g_idx ] ); /* if Hessian is empty, row number of gradient vector */
834 }
835 else
836 {
837 H_idx = 2;
838 nV = (uint_t)mxGetM( prhs[ H_idx ] ); /* row number of Hessian matrix */
839 }
840
841 A_idx = 4;
842 nC = (uint_t)mxGetM( prhs[ A_idx ] ); /* row number of constraint matrix */
843
844 lb_idx = 5;
845 ub_idx = 6;
846 lbA_idx = 7;
847 ubA_idx = 8;
848
849
850 /* ensure that data is given in double precision */
851 if ( ( ( H_idx >= 0 ) && ( mxIsDouble( prhs[H_idx] ) == 0 ) ) ||
852 ( mxIsDouble( prhs[g_idx] ) == 0 ) ||
853 ( mxIsDouble( prhs[A_idx] ) == 0 ) )
854 {
855 myMexErrMsgTxt( "ERROR (qpOASES): All data has to be provided in real_t precision!" );
856 return;
857 }
858
859 /* check if supplied data contains 'NaN' or 'Inf' */
860 if (containsNaNorInf(prhs,H_idx, 0) == BT_TRUE)
861 return;
862
863 if (containsNaNorInf( prhs,g_idx, 0 ) == BT_TRUE)
864 return;
865
866 if (containsNaNorInf( prhs,A_idx, 0 ) == BT_TRUE)
867 return;
868
869 if (containsNaNorInf( prhs,lb_idx, 1 ) == BT_TRUE)
870 return;
871
872 if (containsNaNorInf( prhs,ub_idx, 1 ) == BT_TRUE)
873 return;
874
875 if (containsNaNorInf( prhs,lbA_idx, 1 ) == BT_TRUE)
876 return;
877
878 if (containsNaNorInf( prhs,ubA_idx, 1 ) == BT_TRUE)
879 return;
880
881 /* Check that dimensions are consistent with existing QP instance */
882 if (nV != (uint_t) globalQP->getNV () || nC != (uint_t) globalQP->getNC ())
883 {
884 myMexErrMsgTxt( "ERROR (qpOASES): QP dimensions must be constant during a sequence! Try creating a new QP instance instead." );
885 return;
886 }
887
888 if ( ( H_idx >= 0 ) && ( ( mxGetN( prhs[ H_idx ] ) != nV ) || ( mxGetM( prhs[ H_idx ] ) != nV ) ) )
889 {
890 myMexErrMsgTxt( "ERROR (qpOASES): Hessian matrix dimension mismatch!" );
891 return;
892 }
893
894 if ( ( mxGetN( prhs[ A_idx ] ) != 0 ) && ( mxGetN( prhs[ A_idx ] ) != nV ) )
895 {
896 myMexErrMsgTxt( "ERROR (qpOASES): Constraint matrix dimension mismatch!" );
897 return;
898 }
899
900 if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,g_idx ) != SUCCESSFUL_RETURN )
901 return;
902
903 if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,lb_idx ) != SUCCESSFUL_RETURN )
904 return;
905
906 if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,ub_idx ) != SUCCESSFUL_RETURN )
907 return;
908
909 if ( smartDimensionCheck( &lbA,nC,1, BT_TRUE,prhs,lbA_idx ) != SUCCESSFUL_RETURN )
910 return;
911
912 if ( smartDimensionCheck( &ubA,nC,1, BT_TRUE,prhs,ubA_idx ) != SUCCESSFUL_RETURN )
913 return;
914
915 /* default value for nWSR */
916 nWSRin = 5*(nV+nC);
917
918 /* Check whether options are specified .*/
919 if ( nrhs > 9 )
920 if ( ( !mxIsEmpty( prhs[9] ) ) && ( mxIsStruct( prhs[9] ) ) )
921 setupOptions( &options,prhs[9],nWSRin,maxCpuTimeIn );
922
923 globalQP->deleteQPMatrices( );
924
925 /* make a deep-copy of the user-specified Hessian matrix (possibly sparse) */
926 if ( H_idx >= 0 )
927 setupHessianMatrix( prhs[H_idx],nV, &(globalQP->H),&(globalQP->Hir),&(globalQP->Hjc),&(globalQP->Hv) );
928
929 /* make a deep-copy of the user-specified constraint matrix (possibly sparse) */
930 if ( ( nC > 0 ) && ( A_idx >= 0 ) )
931 setupConstraintMatrix( prhs[A_idx],nV,nC, &(globalQP->A),&(globalQP->Air),&(globalQP->Ajc),&(globalQP->Av) );
932
933 /* Create output vectors and assign pointers to them. */
934 allocateOutputs( nlhs,plhs, nV,nC );
935
936 /* Call qpOASES */
937 SQProblem_hotstart( handle, globalQP->H,g,globalQP->A,
938 lb,ub,lbA,ubA,
939 nWSRin,maxCpuTimeIn,
940 &options,
941 nlhs,plhs
942 );
943
944 return;
945 }
946
947 /* 4) Solve current equality constrained QP. */
948 if ( ( strcmp( typeString,"e" ) == 0 ) || ( strcmp( typeString,"E" ) == 0 ) )
949 {
950 /* consistency checks */
951 if ( ( nlhs < 1 ) || ( nlhs > 4 ) )
952 {
953 myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
954 return;
955 }
956
957 if ( ( nrhs < 7 ) || ( nrhs > 8 ) )
958 {
959 myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
960 return;
961 }
962
963 if ( ( mxIsDouble( prhs[1] ) == false ) || ( mxIsScalar( prhs[1] ) == false ) )
964 {
965 myMexErrMsgTxt( "ERROR (qpOASES): Expecting a handle to QP object as second argument!\nType 'help qpOASES_sequence' for further information." );
966 return;
967 }
968
969 /* get QP instance */
970 handle = (uint_t)mxGetScalar( prhs[1] );
971 globalQP = getQPInstance( handle );
972 if ( globalQP == 0 )
973 {
974 myMexErrMsgTxt( "ERROR (qpOASES): Invalid handle to QP instance!" );
975 return;
976 }
977
978 /* Check inputs dimensions and assign pointers to inputs. */
979 int_t nRHS = (int_t)mxGetN(prhs[2]);
980 nV = globalQP->getNV( );
981 nC = globalQP->getNC( );
982 real_t *x_out, *y_out;
983
984 g_idx = 2;
985 lb_idx = 3;
986 ub_idx = 4;
987 lbA_idx = 5;
988 ubA_idx = 6;
989
990 /* check if supplied data contains 'NaN' or 'Inf' */
991 if (containsNaNorInf(prhs,g_idx, 0) == BT_TRUE)
992 return;
993
994 if (containsNaNorInf( prhs,lb_idx, 1 ) == BT_TRUE)
995 return;
996
997 if (containsNaNorInf( prhs,ub_idx, 1 ) == BT_TRUE)
998 return;
999
1000 if (containsNaNorInf( prhs,lbA_idx, 1 ) == BT_TRUE)
1001 return;
1002
1003 if (containsNaNorInf( prhs,ubA_idx, 1 ) == BT_TRUE)
1004 return;
1005
1006 if ( smartDimensionCheck( &g,nV,nRHS, BT_FALSE,prhs,g_idx ) != SUCCESSFUL_RETURN )
1007 return;
1008
1009 if ( smartDimensionCheck( &lb,nV,nRHS, BT_TRUE,prhs,lb_idx ) != SUCCESSFUL_RETURN )
1010 return;
1011
1012 if ( smartDimensionCheck( &ub,nV,nRHS, BT_TRUE,prhs,ub_idx ) != SUCCESSFUL_RETURN )
1013 return;
1014
1015 if ( smartDimensionCheck( &lbA,nC,nRHS, BT_TRUE,prhs,lbA_idx ) != SUCCESSFUL_RETURN )
1016 return;
1017
1018 if ( smartDimensionCheck( &ubA,nC,nRHS, BT_TRUE,prhs,ubA_idx ) != SUCCESSFUL_RETURN )
1019 return;
1020
1021 /* Check whether options are specified .*/
1022 if ( ( nrhs == 8 ) && ( !mxIsEmpty( prhs[7] ) ) && ( mxIsStruct( prhs[7] ) ) )
1023 {
1024 nWSRin = 5*(nV+nC);
1025 setupOptions( &options,prhs[7],nWSRin,maxCpuTimeIn );
1026 globalQP->sqp->setOptions( options );
1027 }
1028
1029 /* Create output vectors and assign pointers to them. */
1030 plhs[0] = mxCreateDoubleMatrix( nV, nRHS, mxREAL );
1031 x_out = mxGetPr(plhs[0]);
1032 if (nlhs >= 2)
1033 {
1034 plhs[1] = mxCreateDoubleMatrix( nV+nC, nRHS, mxREAL );
1035 y_out = mxGetPr(plhs[1]);
1036
1037 if (nlhs >= 3)
1038 {
1039 plhs[2] = mxCreateDoubleMatrix( nV, nRHS, mxREAL );
1040 real_t* workingSetB = mxGetPr(plhs[2]);
1041 globalQP->sqp->getWorkingSetBounds(workingSetB);
1042
1043 if ( nlhs >= 4 )
1044 {
1045 plhs[3] = mxCreateDoubleMatrix( nC, nRHS, mxREAL );
1046 real_t* workingSetC = mxGetPr(plhs[3]);
1047 globalQP->sqp->getWorkingSetConstraints(workingSetC);
1048 }
1049 }
1050 }
1051 else
1052 y_out = new real_t[nV+nC];
1053
1054 /* Solve equality constrained QP */
1055 returnValue returnvalue = globalQP->sqp->solveCurrentEQP( nRHS,g,lb,ub,lbA,ubA, x_out,y_out );
1056
1057 if (nlhs < 2)
1058 delete[] y_out;
1059
1060 if (returnvalue != SUCCESSFUL_RETURN)
1061 {
1062 char msg[MAX_STRING_LENGTH];
1063 snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): Couldn't solve current EQP (code %d)!", returnvalue);
1064 myMexErrMsgTxt(msg);
1065 return;
1066 }
1067
1068 return;
1069 }
1070
1071 /* 5) Cleanup. */
1072 if ( ( strcmp( typeString,"c" ) == 0 ) || ( strcmp( typeString,"C" ) == 0 ) )
1073 {
1074 /* consistency checks */
1075 if ( nlhs != 0 )
1076 {
1077 myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
1078 return;
1079 }
1080
1081 if ( nrhs != 2 )
1082 {
1083 myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
1084 return;
1085 }
1086
1087 if ( ( mxIsDouble( prhs[1] ) == false ) || ( mxIsScalar( prhs[1] ) == false ) )
1088 {
1089 myMexErrMsgTxt( "ERROR (qpOASES): Expecting a handle to QP object as second argument!\nType 'help qpOASES_sequence' for further information." );
1090 return;
1091 }
1092
1093 /* Cleanup SQProblem instance. */
1094 handle = (uint_t)mxGetScalar( prhs[1] );
1095 deleteQPInstance( handle );
1096
1097 return;
1098 }
1099
1100 }
1101
1102 /*
1103 * end of file
1104 */
1105