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

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