qpOASES-3.2.0/interfaces/octave/qpOASES_sequence.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 #include <qpOASES.hpp>
39 
40 
42 
43 
44 #include "qpOASES_octave_utils.hpp"
45 
48 
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  */
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. */
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  */
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  */
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  */
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  */
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  */
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 
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  */
int_t 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[])
returnValue solveCurrentEQP(const int n_rhs, const real_t *g_in, const real_t *lb_in, const real_t *ub_in, const real_t *lbA_in, const real_t *ubA_in, real_t *x_out, real_t *y_out)
returnValue init(const real_t *const _H, const real_t *const _g, const real_t *const _A, const real_t *const _lb, const real_t *const _ub, const real_t *const _lbA, const real_t *const _ubA, int &nWSR, const real_t *const yOpt=0, real_t *const cputime=0)
BooleanType containsNaNorInf(const mxArray *prhs[], int_t rhs_index, bool mayContainInf)
#define ST_LOWER
returnValue setupHessianMatrix(const mxArray *prhsH, int_t nV, SymmetricMatrix **H, sparse_int_t **Hir, sparse_int_t **Hjc, real_t **Hv)
virtual returnValue getWorkingSetBounds(real_t *workingSetB)
Implements the online active set strategy for box-constrained QPs.
#define myMexErrMsgTxt
const double INFTY
Implements the online active set strategy for QPs with varying matrices.
returnValue setupOptions(Options *options, const mxArray *optionsPtr, int &nWSRin)
Allows to pass back messages to the calling function.
int getNV() const
sparse_int_t * Air
returnValue smartDimensionCheck(real_t **input, unsigned int m, unsigned int n, BooleanType emptyAllowed, const mxArray *prhs[], int idx)
returnValue init(const real_t *const _H, const real_t *const _g, const real_t *const _lb, const real_t *const _ub, int &nWSR, const real_t *const yOpt=0, real_t *const cputime=0)
returnValue hotstart(const real_t *const g_new, const real_t *const lb_new, const real_t *const ub_new, const real_t *const lbA_new, const real_t *const ubA_new, int &nWSR, real_t *const cputime)
returnValue convertFortranToC(const real_t *const A_for, int nV, int nC, real_t *const A)
#define PL_NONE
virtual returnValue getWorkingSetConstraints(real_t *workingSetC)
#define ST_INACTIVE
sparse_int_t * Hjc
static QProblemB * globalQPB
returnValue setupBound(int _number, SubjectToStatus _status)
returnValue setOptions(const Options &_options)
static std::vector< QPInstance * > g_instances
#define PL_HIGH
returnValue hotstart(const real_t *const g_new, const real_t *const lb_new, const real_t *const ub_new, int &nWSR, real_t *const cputime)
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
void allocateOutputs(int nlhs, mxArray *plhs[], int nV, int nC=0, int nP=1)
static SQProblem * globalSQP
returnValue setupAuxiliaryInputs(const mxArray *auxInput, uint_t nV, uint_t nC, HessianType *hessianType, double **x0, double **guessedBounds, double **guessedConstraints, double **R)
returnValue setupConstraint(int _number, SubjectToStatus _status)
Provides a generic way to set and pass user-specified options.
Definition: options.hpp:65
int_t allocateQPInstance(int_t nV, int_t nC, HessianType hessianType, BooleanType isSimplyBounded, const Options *options)
int_t 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)
Abstract base class for interfacing tailored matrix-vector operations.
#define PL_LOW
int_t 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[])
int getNC() const
int_t 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)
void obtainOutputs(int k, QProblemB *qp, returnValue returnvalue, int nWSRin, int nlhs, mxArray *plhs[], int nV, int nC=0)
#define BT_TRUE
Definition: acado_types.hpp:47
#define HST_UNKNOWN
SymmetricMatrix * H
returnValue setupConstraintMatrix(const mxArray *prhsA, int_t nV, int_t nC, Matrix **A, sparse_int_t **Air, sparse_int_t **Ajc, real_t **Av)
static int_t s_nexthandle
#define BT_FALSE
Definition: acado_types.hpp:49
Manages working sets of bounds (= box constraints).
#define ST_UPPER
double real_t
Definition: AD_test.c:10
Implements the online active set strategy for QPs with general constraints.
QPInstance * getQPInstance(int_t handle)
sparse_int_t * Hir
BooleanType isEqual(real_t x, real_t y, real_t TOL=ZERO)
#define R
int_t 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[])
sparse_int_t * Ajc
Abstract base class for interfacing matrix-vector operations tailored to symmetric matrices...
returnValue hotstart(const real_t *const H_new, const real_t *const g_new, const real_t *const A_new, const real_t *const lb_new, const real_t *const ub_new, const real_t *const lbA_new, const real_t *const ubA_new, int &nWSR, real_t *const cputime)


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