qpOASES_e_matlab_utils.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 
37 void QPInstanceCON( QPInstance* _THIS,
38  int _nV,
39  int _nC,
40  HessianType _hessianType,
41  BooleanType _isSimplyBounded
42  )
43 {
44  if ( _nV > NVMAX )
45  {
46  myMexErrMsgTxt( "ERROR (qpOASES_e): Too many primal variables (try increasing QPOASES_NVMAX)!" );
47  return;
48  }
49 
50  if ( _nC > NCMAX )
51  {
52  myMexErrMsgTxt( "ERROR (qpOASES_e): Too many constraints (try increasing QPOASES_NCMAX)!" );
53  return;
54  }
55 
56  _THIS->handle = QPInstance_nexthandle;
57 
58  if ( _nC > 0 )
59  _THIS->isSimplyBounded = BT_FALSE;
60  else
61  _THIS->isSimplyBounded = _isSimplyBounded;
62 
63  if ( _THIS->isSimplyBounded == BT_FALSE )
64  QProblemCON( &(_THIS->sqp),_nV,_nC,_hessianType );
65  else
66  QProblemBCON( &(_THIS->qpb),_nV,_hessianType );
67 }
68 
69 
70 int QPInstance_getNV( QPInstance* _THIS )
71 {
72  if ( _THIS->isSimplyBounded == BT_FALSE )
73  return QProblem_getNV( &(_THIS->sqp) );
74  else
75  return QProblemB_getNV( &(_THIS->qpb) );
76 }
77 
78 
79 int QPInstance_getNC( QPInstance* _THIS )
80 {
81  if ( _THIS->isSimplyBounded == BT_FALSE )
82  return QProblem_getNC( &(_THIS->sqp) );
83  else
84  return 0;
85 }
86 
87 
88 
89 /*
90  * m x I s S c a l a r
91  */
92 bool mxIsScalar( const mxArray *pm )
93 {
94  if ( ( mxGetM(pm) == 1 ) && ( mxGetN(pm) == 1 ) )
95  return true;
96  else
97  return false;
98 }
99 
100 
101 
102 /*
103  * a l l o c a t e Q P r o b l e m I n s t a n c e
104  */
105 int allocateQPInstance( int nV, int nC, HessianType hessianType,
106  BooleanType isSimplyBounded, const Options* options
107  )
108 {
109  QPInstance* inst = 0;
110 
111  QPInstance_nexthandle = (QPInstance_nexthandle+1) % MAX_NUM_QPINSTANCES;
112 
113  inst = &(QPInstances[QPInstance_nexthandle]);
114  QPInstanceCON( inst, nV,nC,hessianType, isSimplyBounded );
115 
116  if ( options != 0 )
117  {
118  if ( isSimplyBounded == BT_FALSE )
119  QProblem_setOptions( &(inst->sqp),*options );
120  else
121  QProblemB_setOptions( &(inst->qpb),*options );
122  }
123 
124  return inst->handle;
125 }
126 
127 
128 /*
129  * g e t Q P r o b l e m I n s t a n c e
130  */
131 QPInstance* getQPInstance( int handle )
132 {
133  if ( handle < MAX_NUM_QPINSTANCES )
134  return &(QPInstances[handle]);
135  else
136  return 0;
137 }
138 
139 
140 
141 /*
142  * s m a r t D i m e n s i o n C h e c k
143  */
144 returnValue smartDimensionCheck( real_t** input, unsigned int m, unsigned int n, BooleanType emptyAllowed,
145  const mxArray* prhs[], int idx
146  )
147 {
148  char msg[QPOASES_MAX_STRING_LENGTH];
149 
150  /* If index is negative, the input does not exist. */
151  if ( idx < 0 )
152  {
153  *input = 0;
154  return SUCCESSFUL_RETURN;
155  }
156 
157  /* Otherwise the input has been passed by the user. */
158  if ( mxIsEmpty( prhs[ idx ] ) )
159  {
160  /* input is empty */
161  if ( ( emptyAllowed == BT_TRUE ) || ( idx == 0 ) ) /* idx==0 used for auxInput */
162  {
163  *input = 0;
164  return SUCCESSFUL_RETURN;
165  }
166  else
167  {
168  if ( idx > 0 )
169  snprintf(msg, QPOASES_MAX_STRING_LENGTH, "ERROR (qpOASES_e): Empty argument %d not allowed!", idx+1);
170  myMexErrMsgTxt( msg );
171  return RET_INVALID_ARGUMENTS;
172  }
173  }
174  else
175  {
176  /* input is non-empty */
177  if ( mxIsSparse( prhs[ idx ] ) == 0 )
178  {
179  if ( ( mxGetM( prhs[ idx ] ) == m ) && ( mxGetN( prhs[ idx ] ) == n ) )
180  {
181  *input = (real_t*) mxGetPr( prhs[ idx ] );
182  return SUCCESSFUL_RETURN;
183  }
184  else
185  {
186  if ( idx > 0 )
187  snprintf(msg, QPOASES_MAX_STRING_LENGTH, "ERROR (qpOASES_e): Input dimension mismatch for argument %d ([%ld,%ld] ~= [%d,%d]).",
188  idx+1, (long int)mxGetM(prhs[idx]), (long int)mxGetN(prhs[idx]), m, n);
189  else /* idx==0 used for auxInput */
190  snprintf(msg, QPOASES_MAX_STRING_LENGTH, "ERROR (qpOASES_e): Input dimension mismatch for some auxInput entry ([%ld,%ld] ~= [%d,%d]).",
191  (long int)mxGetM(prhs[idx]), (long int)mxGetN(prhs[idx]), m, n);
192  myMexErrMsgTxt( msg );
193  return RET_INVALID_ARGUMENTS;
194  }
195  }
196  else
197  {
198  if ( idx > 0 )
199  snprintf(msg, QPOASES_MAX_STRING_LENGTH, "ERROR (qpOASES_e): Vector argument %d must not be in sparse format!", idx+1);
200  else /* idx==0 used for auxInput */
201  snprintf(msg, QPOASES_MAX_STRING_LENGTH, "ERROR (qpOASES_e): auxInput entries must not be in sparse format!" );
202  myMexErrMsgTxt( msg );
203  return RET_INVALID_ARGUMENTS;
204  }
205  }
206 
207  return SUCCESSFUL_RETURN;
208 }
209 
210 
211 
212 /*
213  * c o n t a i n s N a N
214  */
215 BooleanType containsNaN( const real_t* const data, unsigned int dim )
216 {
217  unsigned int i;
218 
219  if ( data == 0 )
220  return BT_FALSE;
221 
222  for ( i = 0; i < dim; ++i )
223  if ( mxIsNaN(data[i]) == 1 )
224  return BT_TRUE;
225 
226  return BT_FALSE;
227 }
228 
229 
230 /*
231  * c o n t a i n s I n f
232  */
233 BooleanType containsInf( const real_t* const data, unsigned int dim )
234 {
235  unsigned int i;
236 
237  if ( data == 0 )
238  return BT_FALSE;
239 
240  for ( i = 0; i < dim; ++i )
241  if ( mxIsInf(data[i]) == 1 )
242  return BT_TRUE;
243 
244  return BT_FALSE;
245 }
246 
247 
248 /*
249  * c o n t a i n s N a N o r I n f
250  */
251 BooleanType containsNaNorInf( const mxArray* prhs[], int rhs_index,
252  BooleanType mayContainInf
253  )
254 {
255  unsigned int dim;
256  char msg[QPOASES_MAX_STRING_LENGTH];
257 
258  if ( rhs_index < 0 )
259  return BT_FALSE;
260 
261  /* overwrite dim for sparse matrices */
262  if (mxIsSparse(prhs[rhs_index]) == 1)
263  dim = (unsigned int)mxGetNzmax(prhs[rhs_index]);
264  else
265  dim = mxGetM(prhs[rhs_index]) * mxGetN(prhs[rhs_index]);
266 
267  if (containsNaN((real_t*) mxGetPr(prhs[rhs_index]), dim) == BT_TRUE) {
268  snprintf(msg, QPOASES_MAX_STRING_LENGTH,
269  "ERROR (qpOASES_e): Argument %d contains 'NaN' !", rhs_index + 1);
270  myMexErrMsgTxt(msg);
271  return BT_TRUE;
272  }
273 
274  if ( mayContainInf == BT_FALSE ) {
275  if (containsInf((real_t*) mxGetPr(prhs[rhs_index]), dim) == BT_TRUE) {
276  snprintf(msg, QPOASES_MAX_STRING_LENGTH,
277  "ERROR (qpOASES_e): Argument %d contains 'Inf' !",
278  rhs_index + 1);
279  myMexErrMsgTxt(msg);
280  return BT_TRUE;
281  }
282  }
283 
284  return BT_FALSE;
285 }
286 
287 
288 /*
289  * c o n v e r t F o r t r a n T o C
290  */
291 returnValue convertFortranToC( const real_t* const M_for, int nV, int nC, real_t* const M )
292 {
293  int i,j;
294 
295  if ( ( M_for == 0 ) || ( M == 0 ) )
296  return RET_INVALID_ARGUMENTS;
297 
298  if ( ( nV < 0 ) || ( nC < 0 ) )
299  return RET_INVALID_ARGUMENTS;
300 
301  for ( i=0; i<nC; ++i )
302  for ( j=0; j<nV; ++j )
303  M[i*nV + j] = M_for[j*nC + i];
304 
305  return SUCCESSFUL_RETURN;
306 }
307 
308 
309 /*
310  * h a s O p t i o n s V a l u e
311  */
312 BooleanType hasOptionsValue( const mxArray* optionsPtr, const char* const optionString, double** optionValue )
313 {
314  mxArray* optionName = mxGetField( optionsPtr,0,optionString );
315 
316  if ( optionName == 0 )
317  {
318  char msg[QPOASES_MAX_STRING_LENGTH];
319  snprintf(msg, QPOASES_MAX_STRING_LENGTH, "Option struct does not contain entry '%s', using default value instead!", optionString );
320  mexWarnMsgTxt( msg );
321  return BT_FALSE;
322  }
323 
324  if ( ( mxIsEmpty(optionName) == false ) && ( mxIsScalar( optionName ) == true ) )
325  {
326  *optionValue = mxGetPr( optionName );
327  return BT_TRUE;
328  }
329  else
330  {
331  char msg[QPOASES_MAX_STRING_LENGTH];
332  snprintf(msg, QPOASES_MAX_STRING_LENGTH, "Option '%s' is not a scalar, using default value instead!", optionString );
333  mexWarnMsgTxt( msg );
334  return BT_FALSE;
335  }
336 }
337 
338 
339 /*
340  * s e t u p O p t i o n s
341  */
342 returnValue setupOptions( Options* options, const mxArray* optionsPtr, int* nWSRin, real_t* maxCpuTime )
343 {
344  double* optionValue;
345  int optionValueInt;
346 
347  /* Check for correct number of option entries;
348  * may occur, e.g., if user types options.<misspelledName> = <someValue>; */
349  if ( mxGetNumberOfFields(optionsPtr) != 31 )
350  mexWarnMsgTxt( "Options might be set incorrectly as struct has wrong number of entries!\n Type 'help qpOASES_options' for further information." );
351 
352 
353  if ( hasOptionsValue( optionsPtr,"maxIter",&optionValue ) == BT_TRUE )
354  if ( *optionValue >= 0.0 )
355  *nWSRin = (int)*optionValue;
356 
357  if ( hasOptionsValue( optionsPtr,"maxCpuTime",&optionValue ) == BT_TRUE )
358  if ( *optionValue >= 0.0 )
359  *maxCpuTime = *optionValue;
360 
361  if ( hasOptionsValue( optionsPtr,"printLevel",&optionValue ) == BT_TRUE )
362  {
363  #ifdef __SUPPRESSANYOUTPUT__
364  options->printLevel = PL_NONE;
365  #else
366  optionValueInt = (int)*optionValue;
367  options->printLevel = (REFER_NAMESPACE_QPOASES PrintLevel)optionValueInt;
368  if ( options->printLevel < PL_DEBUG_ITER )
369  options->printLevel = PL_DEBUG_ITER;
370  if ( options->printLevel > PL_HIGH )
371  options->printLevel = PL_HIGH;
372  #endif
373  }
374 
375  if ( hasOptionsValue( optionsPtr,"enableRamping",&optionValue ) == BT_TRUE )
376  {
377  optionValueInt = (int)*optionValue;
378  options->enableRamping = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
379  }
380 
381  if ( hasOptionsValue( optionsPtr,"enableFarBounds",&optionValue ) == BT_TRUE )
382  {
383  optionValueInt = (int)*optionValue;
384  options->enableFarBounds = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
385  }
386 
387  if ( hasOptionsValue( optionsPtr,"enableFlippingBounds",&optionValue ) == BT_TRUE )
388  {
389  optionValueInt = (int)*optionValue;
390  options->enableFlippingBounds = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
391  }
392 
393  if ( hasOptionsValue( optionsPtr,"enableRegularisation",&optionValue ) == BT_TRUE )
394  {
395  optionValueInt = (int)*optionValue;
396  options->enableRegularisation = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
397  }
398 
399  if ( hasOptionsValue( optionsPtr,"enableFullLITests",&optionValue ) == BT_TRUE )
400  {
401  optionValueInt = (int)*optionValue;
402  options->enableFullLITests = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
403  }
404 
405  if ( hasOptionsValue( optionsPtr,"enableNZCTests",&optionValue ) == BT_TRUE )
406  {
407  optionValueInt = (int)*optionValue;
408  options->enableNZCTests = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
409  }
410 
411  if ( hasOptionsValue( optionsPtr,"enableDriftCorrection",&optionValue ) == BT_TRUE )
412  options->enableDriftCorrection = (int)*optionValue;
413 
414  if ( hasOptionsValue( optionsPtr,"enableCholeskyRefactorisation",&optionValue ) == BT_TRUE )
415  options->enableCholeskyRefactorisation = (int)*optionValue;
416 
417  if ( hasOptionsValue( optionsPtr,"enableEqualities",&optionValue ) == BT_TRUE )
418  {
419  optionValueInt = (int)*optionValue;
420  options->enableEqualities = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
421  }
422 
423 
424  if ( hasOptionsValue( optionsPtr,"terminationTolerance",&optionValue ) == BT_TRUE )
425  options->terminationTolerance = *optionValue;
426 
427  if ( hasOptionsValue( optionsPtr,"boundTolerance",&optionValue ) == BT_TRUE )
428  options->boundTolerance = *optionValue;
429 
430  if ( hasOptionsValue( optionsPtr,"boundRelaxation",&optionValue ) == BT_TRUE )
431  options->boundRelaxation = *optionValue;
432 
433  if ( hasOptionsValue( optionsPtr,"epsNum",&optionValue ) == BT_TRUE )
434  options->epsNum = *optionValue;
435 
436  if ( hasOptionsValue( optionsPtr,"epsDen",&optionValue ) == BT_TRUE )
437  options->epsDen = *optionValue;
438 
439  if ( hasOptionsValue( optionsPtr,"maxPrimalJump",&optionValue ) == BT_TRUE )
440  options->maxPrimalJump = *optionValue;
441 
442  if ( hasOptionsValue( optionsPtr,"maxDualJump",&optionValue ) == BT_TRUE )
443  options->maxDualJump = *optionValue;
444 
445 
446  if ( hasOptionsValue( optionsPtr,"initialRamping",&optionValue ) == BT_TRUE )
447  options->initialRamping = *optionValue;
448 
449  if ( hasOptionsValue( optionsPtr,"finalRamping",&optionValue ) == BT_TRUE )
450  options->finalRamping = *optionValue;
451 
452  if ( hasOptionsValue( optionsPtr,"initialFarBounds",&optionValue ) == BT_TRUE )
453  options->initialFarBounds = *optionValue;
454 
455  if ( hasOptionsValue( optionsPtr,"growFarBounds",&optionValue ) == BT_TRUE )
456  options->growFarBounds = *optionValue;
457 
458  if ( hasOptionsValue( optionsPtr,"initialStatusBounds",&optionValue ) == BT_TRUE )
459  {
460  optionValueInt = (int)*optionValue;
461  if ( optionValueInt < -1 )
462  optionValueInt = -1;
463  if ( optionValueInt > 1 )
464  optionValueInt = 1;
465  options->initialStatusBounds = (REFER_NAMESPACE_QPOASES SubjectToStatus)optionValueInt;
466  }
467 
468  if ( hasOptionsValue( optionsPtr,"epsFlipping",&optionValue ) == BT_TRUE )
469  options->epsFlipping = *optionValue;
470 
471  if ( hasOptionsValue( optionsPtr,"numRegularisationSteps",&optionValue ) == BT_TRUE )
472  options->numRegularisationSteps = (int)*optionValue;
473 
474  if ( hasOptionsValue( optionsPtr,"epsRegularisation",&optionValue ) == BT_TRUE )
475  options->epsRegularisation = *optionValue;
476 
477  if ( hasOptionsValue( optionsPtr,"numRefinementSteps",&optionValue ) == BT_TRUE )
478  options->numRefinementSteps = (int)*optionValue;
479 
480  if ( hasOptionsValue( optionsPtr,"epsIterRef",&optionValue ) == BT_TRUE )
481  options->epsIterRef = *optionValue;
482 
483  if ( hasOptionsValue( optionsPtr,"epsLITests",&optionValue ) == BT_TRUE )
484  options->epsLITests = *optionValue;
485 
486  if ( hasOptionsValue( optionsPtr,"epsNZCTests",&optionValue ) == BT_TRUE )
487  options->epsNZCTests = *optionValue;
488 
489  return SUCCESSFUL_RETURN;
490 }
491 
492 
493 
494 /*
495  * s e t u p A u x i l i a r y I n p u t s
496  */
497 returnValue setupAuxiliaryInputs( const mxArray* auxInput, unsigned int nV, unsigned int nC,
498  HessianType* hessianType, double** x0, double** guessedBounds, double** guessedConstraints, double** R
499  )
500 {
501  mxArray* curField = 0;
502  double* hessianTypeTmp;
503  int hessianTypeInt;
504 
505  /* hessianType */
506  curField = mxGetField( auxInput,0,"hessianType" );
507  if ( curField == NULL )
508  mexWarnMsgTxt( "auxInput struct does not contain entry 'hessianType'!\n Type 'help qpOASES_auxInput' for further information." );
509  else
510  {
511  if ( mxIsEmpty(curField) == true )
512  {
513  *hessianType = HST_UNKNOWN;
514  }
515  else
516  {
517  if ( mxIsScalar(curField) == false )
518  return RET_INVALID_ARGUMENTS;
519 
520  hessianTypeTmp = mxGetPr(curField);
521  hessianTypeInt = (int)*hessianTypeTmp;
522  if ( hessianTypeInt < 0 )
523  hessianTypeInt = 6; /* == HST_UNKNOWN */
524  if ( hessianTypeInt > 5 )
525  hessianTypeInt = 6; /* == HST_UNKNOWN */
526  *hessianType = (REFER_NAMESPACE_QPOASES HessianType)hessianTypeInt;
527  }
528  }
529 
530  /* x0 */
531  curField = mxGetField( auxInput,0,"x0" );
532  if ( curField == NULL )
533  mexWarnMsgTxt( "auxInput struct does not contain entry 'x0'!\n Type 'help qpOASES_auxInput' for further information." );
534  else
535  {
536  *x0 = mxGetPr(curField);
537  if ( smartDimensionCheck( x0,nV,1, BT_TRUE,((const mxArray**)&curField),0 ) != SUCCESSFUL_RETURN )
538  return RET_INVALID_ARGUMENTS;
539  }
540 
541  /* guessedWorkingSetB */
542  curField = mxGetField( auxInput,0,"guessedWorkingSetB" );
543  if ( curField == NULL )
544  mexWarnMsgTxt( "auxInput struct does not contain entry 'guessedWorkingSetB'!\n Type 'help qpOASES_auxInput' for further information." );
545  else
546  {
547  *guessedBounds = mxGetPr(curField);
548  if ( smartDimensionCheck( guessedBounds,nV,1, BT_TRUE,((const mxArray**)&curField),0 ) != SUCCESSFUL_RETURN )
549  return RET_INVALID_ARGUMENTS;
550  }
551 
552  /* guessedWorkingSetC */
553  curField = mxGetField( auxInput,0,"guessedWorkingSetC" );
554  if ( curField == NULL )
555  mexWarnMsgTxt( "auxInput struct does not contain entry 'guessedWorkingSetC'!\n Type 'help qpOASES_auxInput' for further information." );
556  else
557  {
558  *guessedConstraints = mxGetPr(curField);
559  if ( smartDimensionCheck( guessedConstraints,nC,1, BT_TRUE,((const mxArray**)&curField),0 ) != SUCCESSFUL_RETURN )
560  return RET_INVALID_ARGUMENTS;
561  }
562 
563  /* R */
564  curField = mxGetField( auxInput,0,"R" );
565  if ( curField == NULL )
566  mexWarnMsgTxt( "auxInput struct does not contain entry 'R'!\n Type 'help qpOASES_auxInput' for further information." );
567  else
568  {
569  *R = mxGetPr(curField);
570  if ( smartDimensionCheck( R,nV,nV, BT_TRUE,((const mxArray**)&curField),0 ) != SUCCESSFUL_RETURN )
571  return RET_INVALID_ARGUMENTS;
572  }
573 
574  return SUCCESSFUL_RETURN;
575 }
576 
577 
578 
579 /*
580  * a l l o c a t e O u t p u t s
581  */
582 returnValue allocateOutputs( int nlhs, mxArray* plhs[], int nV, int nC, int nP, int handle
583  )
584 {
585  /* Create output vectors and assign pointers to them. */
586  int curIdx = 0;
587 
588  /* handle */
589  if ( handle >= 0 )
590  plhs[curIdx++] = mxCreateDoubleMatrix( 1, 1, mxREAL );
591 
592  /* x */
593  plhs[curIdx++] = mxCreateDoubleMatrix( nV, nP, mxREAL );
594 
595  if ( nlhs > curIdx )
596  {
597  /* fval */
598  plhs[curIdx++] = mxCreateDoubleMatrix( 1, nP, mxREAL );
599 
600  if ( nlhs > curIdx )
601  {
602  /* exitflag */
603  plhs[curIdx++] = mxCreateDoubleMatrix( 1, nP, mxREAL );
604 
605  if ( nlhs > curIdx )
606  {
607  /* iter */
608  plhs[curIdx++] = mxCreateDoubleMatrix( 1, nP, mxREAL );
609 
610  if ( nlhs > curIdx )
611  {
612  /* lambda */
613  plhs[curIdx++] = mxCreateDoubleMatrix( nV+nC, nP, mxREAL );
614 
615  if ( nlhs > curIdx )
616  {
617  /* setup auxiliary output struct */
618  mxArray* auxOutput = mxCreateStructMatrix( 1,1,0,0 );
619  int curFieldNum;
620 
621  /* working set */
622  curFieldNum = mxAddField( auxOutput,"workingSetB" );
623  if ( curFieldNum >= 0 )
624  mxSetFieldByNumber( auxOutput,0,curFieldNum,mxCreateDoubleMatrix( nV, nP, mxREAL ) );
625 
626  curFieldNum = mxAddField( auxOutput,"workingSetC" );
627  if ( curFieldNum >= 0 )
628  mxSetFieldByNumber( auxOutput,0,curFieldNum,mxCreateDoubleMatrix( nC, nP, mxREAL ) );
629 
630  curFieldNum = mxAddField( auxOutput,"cpuTime" );
631  if ( curFieldNum >= 0 )
632  mxSetFieldByNumber( auxOutput,0,curFieldNum,mxCreateDoubleMatrix( 1, nP, mxREAL ) );
633 
634  plhs[curIdx] = auxOutput;
635  }
636  }
637  }
638  }
639  }
640 
641  return SUCCESSFUL_RETURN;
642 }
643 
644 
645 /*
646  * o b t a i n O u t p u t s
647  */
648 returnValue obtainOutputs( int k, QProblem* qp, returnValue returnvalue, int _nWSRout, double _cpuTime,
649  int nlhs, mxArray* plhs[], int nV, int nC, int handle
650  )
651 {
652  double *x=0, *obj=0, *status=0, *nWSRout=0, *y=0, *workingSetB=0, *workingSetC=0, *cpuTime;
653  mxArray *auxOutput=0, *curField=0;
654 
655  /* Create output vectors and assign pointers to them. */
656  int curIdx = 0;
657 
658  /* handle */
659  if ( handle >= 0 )
660  plhs[curIdx++] = mxCreateDoubleScalar( handle );
661 
662  /* x */
663  x = mxGetPr( plhs[curIdx++] );
664  QProblem_getPrimalSolution( qp,&(x[k*nV]) );
665 
666  if ( nlhs > curIdx )
667  {
668  /* fval */
669  obj = mxGetPr( plhs[curIdx++] );
670  obj[k] = QProblem_getObjVal( qp );
671 
672  if ( nlhs > curIdx )
673  {
674  /* exitflag */
675  status = mxGetPr( plhs[curIdx++] );
676  status[k] = (double)qpOASES_getSimpleStatus( returnvalue,0 );
677 
678  if ( nlhs > curIdx )
679  {
680  /* iter */
681  nWSRout = mxGetPr( plhs[curIdx++] );
682  nWSRout[k] = (double) _nWSRout;
683 
684  if ( nlhs > curIdx )
685  {
686  /* lambda */
687  y = mxGetPr( plhs[curIdx++] );
688  QProblem_getDualSolution( qp,&(y[k*(nV+nC)]) );
689 
690  /* auxOutput */
691  if ( nlhs > curIdx )
692  {
693  auxOutput = plhs[curIdx];
694  curField = 0;
695 
696  /* working set bounds */
697  if ( nV > 0 )
698  {
699  curField = mxGetField( auxOutput,0,"workingSetB" );
700  workingSetB = mxGetPr(curField);
701  QProblem_getWorkingSetBounds( qp,&(workingSetB[k*nV]) );
702  }
703 
704  /* working set constraints */
705  if ( nC > 0 )
706  {
707  curField = mxGetField( auxOutput,0,"workingSetC" );
708  workingSetC = mxGetPr(curField);
709  QProblem_getWorkingSetConstraints( qp,&(workingSetC[k*nC]) );
710  }
711 
712  /* cpu time */
713  curField = mxGetField( auxOutput,0,"cpuTime" );
714  cpuTime = mxGetPr(curField);
715  cpuTime[0] = (double) _cpuTime;
716  }
717  }
718  }
719  }
720  }
721 
722  return SUCCESSFUL_RETURN;
723 }
724 
725 
726 /*
727  * o b t a i n O u t p u t s S B
728  */
729 returnValue obtainOutputsSB( int k, QProblemB* qp, returnValue returnvalue, int _nWSRout, double _cpuTime,
730  int nlhs, mxArray* plhs[], int nV, int handle
731  )
732 {
733  double *x=0, *obj=0, *status=0, *nWSRout=0, *y=0, *workingSetB=0, *workingSetC=0, *cpuTime;
734  mxArray *auxOutput=0, *curField=0;
735 
736  /* Create output vectors and assign pointers to them. */
737  int curIdx = 0;
738 
739  /* handle */
740  if ( handle >= 0 )
741  plhs[curIdx++] = mxCreateDoubleScalar( handle );
742 
743  /* x */
744  x = mxGetPr( plhs[curIdx++] );
745  QProblemB_getPrimalSolution( qp,&(x[k*nV]) );
746 
747  if ( nlhs > curIdx )
748  {
749  /* fval */
750  obj = mxGetPr( plhs[curIdx++] );
751  obj[k] = QProblemB_getObjVal( qp );
752 
753  if ( nlhs > curIdx )
754  {
755  /* exitflag */
756  status = mxGetPr( plhs[curIdx++] );
757  status[k] = (double)qpOASES_getSimpleStatus( returnvalue,0 );
758 
759  if ( nlhs > curIdx )
760  {
761  /* iter */
762  nWSRout = mxGetPr( plhs[curIdx++] );
763  nWSRout[k] = (double) _nWSRout;
764 
765  if ( nlhs > curIdx )
766  {
767  /* lambda */
768  y = mxGetPr( plhs[curIdx++] );
769  QProblemB_getDualSolution( qp,&(y[k*nV]) );
770 
771  /* auxOutput */
772  if ( nlhs > curIdx )
773  {
774  auxOutput = plhs[curIdx];
775  curField = 0;
776 
777  /* working set bounds */
778  if ( nV > 0 )
779  {
780  curField = mxGetField( auxOutput,0,"workingSetB" );
781  workingSetB = mxGetPr(curField);
782  QProblemB_getWorkingSetBounds( qp,&(workingSetB[k*nV]) );
783  }
784 
785  /* cpu time */
786  curField = mxGetField( auxOutput,0,"cpuTime" );
787  cpuTime = mxGetPr(curField);
788  cpuTime[0] = (double) _cpuTime;
789  }
790  }
791  }
792  }
793  }
794 
795  return SUCCESSFUL_RETURN;
796 }
797 
798 
799 
800 /*
801  * s e t u p H e s s i a n M a t r i x
802  */
803 returnValue setupHessianMatrix( const mxArray* prhsH, int nV,
804  DenseMatrix* H
805  )
806 {
807  real_t *H_for = 0;
808  /*static real_t H_mem[NVMAX*NVMAX];*/
809 
810  if ( prhsH == 0 )
811  return SUCCESSFUL_RETURN;
812 
813  if ( mxIsSparse( prhsH ) != 0 )
814  {
815  myMexErrMsgTxt( "ERROR (qpOASES_e): Cannot handle sparse matrices!" );
816  return RET_INVALID_ARGUMENTS;
817  }
818  else
819  {
820  /* make a deep-copy in order to avoid modifying input data when regularising */
821  H_for = (real_t*) mxGetPr( prhsH );
822  /*qpOASES_printM( H_for,nV,nV );*/
823  /*convertFortranToC( H_for, nV,nV,H_mem ); not neccessary as H is symmetric! */
824  DenseMatrixCON( H, nV,nV,nV, H_for );
825  }
826 
827  return SUCCESSFUL_RETURN;
828 }
829 
830 
831 /*
832  * s e t u p C o n s t r a i n t M a t r i x
833  */
834 returnValue setupConstraintMatrix( const mxArray* prhsA, int nV, int nC,
835  DenseMatrix* A
836  )
837 {
838  real_t *A_for = 0;
839  static real_t A_mem[NCMAX*NVMAX];
840 
841  if ( prhsA == 0 )
842  return SUCCESSFUL_RETURN;
843 
844  if ( mxIsSparse( prhsA ) != 0 )
845  {
846  myMexErrMsgTxt( "ERROR (qpOASES_e): Cannot handle sparse matrices!" );
847  return RET_INVALID_ARGUMENTS;
848  }
849  else
850  {
851  /* make a deep-copy in order to avoid modifying input data when regularising */
852  A_for = (real_t*) mxGetPr( prhsA );
853  convertFortranToC( A_for, nV,nC,A_mem );
854  DenseMatrixCON( A, nC,nV,nV, A_mem );
855  }
856 
857  return SUCCESSFUL_RETURN;
858 }
859 
860 
861 /*
862  * end of file
863  */
QProblemBCON
void QProblemBCON(QProblemB *_THIS, int _nV, HessianType _hessianType)
Definition: QProblemB.c:45
QProblem_getPrimalSolution
returnValue QProblem_getPrimalSolution(QProblem *_THIS, real_t *const xOpt)
Definition: QProblem.c:1168
QProblemB_getWorkingSetBounds
returnValue QProblemB_getWorkingSetBounds(QProblemB *_THIS, real_t *workingSetB)
Definition: QProblemB.c:763
M
#define M
Definition: rocket_with_templates_ocp.hpp:43
QProblemB_getNV
static int QProblemB_getNV(QProblemB *_THIS)
Definition: QProblemB.h:1172
allocateQPInstance
int allocateQPInstance(int nV, int nC, HessianType hessianType, BooleanType isSimplyBounded, const Options *options)
Definition: qpOASES_e_matlab_utils.c:105
getQPInstance
QPInstance * getQPInstance(int handle)
Definition: qpOASES_e_matlab_utils.c:131
containsNaN
BooleanType containsNaN(const real_t *const data, unsigned int dim)
Definition: qpOASES_e_matlab_utils.c:215
QPInstance_getNV
int QPInstance_getNV(QPInstance *_THIS)
Definition: qpOASES_e_matlab_utils.c:70
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
QProblemB_getDualSolution
returnValue QProblemB_getDualSolution(QProblemB *_THIS, real_t *const yOpt)
Definition: QProblemB.c:905
myMexErrMsgTxt
#define myMexErrMsgTxt
Definition: qpOASES_matlab_utils.hpp:42
SUCCESSFUL_RETURN
Definition: acado_types.hpp:836
setupHessianMatrix
returnValue setupHessianMatrix(const mxArray *prhsH, int nV, DenseMatrix *H)
Definition: qpOASES_e_matlab_utils.c:803
RET_INVALID_ARGUMENTS
Definition: acado_types.hpp:839
QPInstance::isSimplyBounded
BooleanType isSimplyBounded
Definition: qpOASES_matlab_utils.hpp:78
qpOASES_getSimpleStatus
int qpOASES_getSimpleStatus(returnValue returnvalue, BooleanType doPrintStatus)
Definition: Utils.c:882
QPInstances
static QPInstance QPInstances[MAX_NUM_QPINSTANCES]
Definition: qpOASES_e_matlab_utils.h:84
NCMAX
#define NCMAX
Definition: qpoases3/include/qpOASES_e/Constants.h:74
returnValue
Allows to pass back messages to the calling function.
Definition: acado_types.hpp:1139
setupOptions
returnValue setupOptions(Options *options, const mxArray *optionsPtr, int *nWSRin, real_t *maxCpuTime)
Definition: qpOASES_e_matlab_utils.c:342
QPInstance_getNC
int QPInstance_getNC(QPInstance *_THIS)
Definition: qpOASES_e_matlab_utils.c:79
NVMAX
#define NVMAX
Definition: qpoases3/include/qpOASES_e/Constants.h:70
QProblemB_getObjVal
real_t QProblemB_getObjVal(QProblemB *_THIS)
Definition: QProblemB.c:807
HessianType
HessianType
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/Types.hpp:116
PL_NONE
#define PL_NONE
Definition: qpOASES_wrapper.h:83
containsInf
BooleanType containsInf(const real_t *const data, unsigned int dim)
Definition: qpOASES_e_matlab_utils.c:233
obtainOutputsSB
returnValue obtainOutputsSB(int k, QProblemB *qp, returnValue returnvalue, int _nWSRout, double _cpuTime, int nlhs, mxArray *plhs[], int nV, int handle)
Definition: qpOASES_e_matlab_utils.c:729
QProblem_getNV
static int QProblem_getNV(QProblem *_THIS)
Definition: QProblem.h:1691
DenseMatrixCON
void DenseMatrixCON(DenseMatrix *_THIS, int m, int n, int lD, real_t *v)
Definition: Matrices.c:47
setupConstraintMatrix
returnValue setupConstraintMatrix(const mxArray *prhsA, int nV, int nC, DenseMatrix *A)
Definition: qpOASES_e_matlab_utils.c:834
PL_HIGH
#define PL_HIGH
Definition: qpOASES_wrapper.h:86
DenseMatrix
Interfaces matrix-vector operations tailored to general dense matrices.
Definition: qpOASES-3.0beta/include/qpOASES/Matrices.hpp:273
BooleanType
BooleanType
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/Types.hpp:44
mxIsScalar
bool mxIsScalar(const mxArray *pm)
Definition: qpOASES_e_matlab_utils.c:92
Options
Provides a generic way to set and pass user-specified options.
Definition: options.hpp:65
PL_DEBUG_ITER
#define PL_DEBUG_ITER
Definition: qpOASES_wrapper.h:81
QProblem_getObjVal
real_t QProblem_getObjVal(QProblem *_THIS)
Definition: QProblem.c:1095
obtainOutputs
returnValue obtainOutputs(int k, QProblem *qp, returnValue returnvalue, int _nWSRout, double _cpuTime, int nlhs, mxArray *plhs[], int nV, int nC, int handle)
Definition: qpOASES_e_matlab_utils.c:648
allocateOutputs
returnValue allocateOutputs(int nlhs, mxArray *plhs[], int nV, int nC, int nP, int handle)
Definition: qpOASES_e_matlab_utils.c:582
hasOptionsValue
BooleanType hasOptionsValue(const mxArray *optionsPtr, const char *const optionString, double **optionValue)
Definition: qpOASES_e_matlab_utils.c:312
SubjectToStatus
SubjectToStatus
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/Types.hpp:97
QProblemB_setOptions
static returnValue QProblemB_setOptions(QProblemB *_THIS, Options _options)
Definition: QProblemB.h:1299
PrintLevel
PrintLevel
Definition: acado_types.hpp:292
QProblem_getNC
static int QProblem_getNC(QProblem *_THIS)
Definition: QProblem.h:2061
QProblemB_getPrimalSolution
returnValue QProblemB_getPrimalSolution(QProblemB *_THIS, real_t *const xOpt)
Definition: QProblemB.c:880
QProblem_getDualSolution
returnValue QProblem_getDualSolution(QProblem *_THIS, real_t *const yOpt)
Definition: QProblem.c:1058
BT_TRUE
#define BT_TRUE
Definition: acado_types.hpp:47
QProblem_getWorkingSetConstraints
returnValue QProblem_getWorkingSetConstraints(QProblem *_THIS, real_t *workingSetC)
Definition: QProblem.c:1022
QPInstanceCON
void QPInstanceCON(QPInstance *_THIS, int _nV, int _nC, HessianType _hessianType, BooleanType _isSimplyBounded)
Definition: qpOASES_e_matlab_utils.c:37
setupAuxiliaryInputs
returnValue setupAuxiliaryInputs(const mxArray *auxInput, unsigned int nV, unsigned int nC, HessianType *hessianType, double **x0, double **guessedBounds, double **guessedConstraints, double **R)
Definition: qpOASES_e_matlab_utils.c:497
HST_UNKNOWN
#define HST_UNKNOWN
Definition: qpOASES_wrapper.h:70
convertFortranToC
returnValue convertFortranToC(const real_t *const M_for, int nV, int nC, real_t *const M)
Definition: qpOASES_e_matlab_utils.c:291
QPOASES_MAX_STRING_LENGTH
#define QPOASES_MAX_STRING_LENGTH
Definition: qpoases3/include/qpOASES_e/Constants.h:123
BT_FALSE
#define BT_FALSE
Definition: acado_types.hpp:49
real_t
double real_t
Definition: AD_test.c:10
QProblem
Implements the online active set strategy for QPs with general constraints.
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/QProblem.hpp:53
QPInstance_nexthandle
static int QPInstance_nexthandle
Definition: qpOASES_e_matlab_utils.h:52
QProblemCON
void QProblemCON(QProblem *_THIS, int _nV, int _nC, HessianType _hessianType)
Definition: QProblem.c:51
MAX_NUM_QPINSTANCES
#define MAX_NUM_QPINSTANCES
Definition: qpOASES_e_matlab_utils.h:83
R
#define R
QProblem_setOptions
static returnValue QProblem_setOptions(QProblem *_THIS, Options _options)
Definition: QProblem.h:1818
containsNaNorInf
BooleanType containsNaNorInf(const mxArray *prhs[], int rhs_index, BooleanType mayContainInf)
Definition: qpOASES_e_matlab_utils.c:251
REFER_NAMESPACE_QPOASES
#define REFER_NAMESPACE_QPOASES
Definition: external_packages/qpOASES-3.0beta/include/qpOASES/Types.hpp:126
smartDimensionCheck
returnValue smartDimensionCheck(real_t **input, unsigned int m, unsigned int n, BooleanType emptyAllowed, const mxArray *prhs[], int idx)
Definition: qpOASES_e_matlab_utils.c:144
QProblem_getWorkingSetBounds
returnValue QProblem_getWorkingSetBounds(QProblem *_THIS, real_t *workingSetB)
Definition: QProblem.c:1001

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