qpOASES-3.0beta/interfaces/matlab/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-2011 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/QProblem.hpp>
39 
40 
41 using namespace qpOASES;
42 
43 #include <qpOASES_matlab_utils.cpp>
44 
45 
46 /* global pointer to QP object */
47 static QProblem* globalQP = 0;
49 static Matrix* globalQP_A = 0;
50 static long* globalQP_Hdiag = 0;
51 
52 
53 /*
54  * a l l o c a t e G l o b a l Q P r o b l e m I n s t a n c e
55  */
57  )
58 {
59  globalQP = new QProblem( nV,nC );
60  globalQP->setOptions( *options );
61 
62  return;
63 }
64 
65 
66 /*
67  * d e l e t e G l o b a l Q P r o b l e m I n s t a n c e
68  */
70 {
71  if ( globalQP != 0 )
72  {
73  delete globalQP;
74  globalQP = 0;
75  }
76 
77  return;
78 }
79 
80 
81 /*
82  * d e l e t e G l o b a l Q P r o b l e m M a t r i c e s
83  */
85 {
86  if ( globalQP_H != 0 )
87  {
88  delete globalQP_H;
89  globalQP_H = 0;
90  }
91 
92  if ( globalQP_A != 0 )
93  {
94  delete globalQP_A;
95  globalQP_A = 0;
96  }
97 
98  if ( globalQP_Hdiag != 0 )
99  {
100  delete[] globalQP_Hdiag;
101  globalQP_Hdiag = 0;
102  }
103 
104  return;
105 }
106 
107 
108 /*
109  * i n i t
110  */
111 void init( int nV, int nC,
113  const real_t* const lb, const real_t* const ub, const real_t* const lbA, const real_t* const ubA,
114  int nWSR, const real_t* const x0, Options* options,
115  int nOutputs, mxArray* plhs[]
116  )
117 {
118  /* 1) Setup initial QP. */
119  allocateGlobalQProblemInstance( nV,nC,options );
120 
121  /* 2) Solve initial QP. */
122  returnValue returnvalue;
123 
124  if ( x0 == 0 )
125  returnvalue = globalQP->init( H,g,A,lb,ub,lbA,ubA, nWSR,0 );
126  else
127  returnvalue = globalQP->init( H,g,A,lb,ub,lbA,ubA, nWSR,0, x0,0,0,0 );
128 
129  /* 3) Assign lhs arguments. */
130  obtainOutputs( 0,globalQP,returnvalue,nWSR,
131  nOutputs,plhs,0,0 );
132 
133  return;
134 }
135 
136 
137 /*
138  * h o t s t a r t
139  */
140 void hotstart( const real_t* const g,
141  const real_t* const lb, const real_t* const ub,
142  const real_t* const lbA, const real_t* const ubA,
143  int nWSR, Options* options,
144  int nOutputs, mxArray* plhs[]
145  )
146 {
147  /* 1) Solve QP with given options. */
148  globalQP->setOptions( *options );
149  returnValue returnvalue = globalQP->hotstart( g,lb,ub,lbA,ubA, nWSR,0 );
150 
151  /* 2) Assign lhs arguments. */
152  obtainOutputs( 0,globalQP,returnvalue,nWSR,
153  nOutputs,plhs,0,0 );
154 
155  return;
156 }
157 
158 
159 /*
160  * m e x F u n c t i o n
161  */
162 void mexFunction( int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[] )
163 {
164  unsigned int i,j;
165 
166  /* inputs */
167  char* typeString;
168  real_t *H_for=0, *H_mem=0, *g=0, *A_for=0, *A_mem=0, *lb=0, *ub=0, *lbA=0, *ubA=0, *x0=0;
169 
171  options.printLevel = PL_LOW;
172  #ifdef __DEBUG__
173  options.printLevel = PL_HIGH;
174  #endif
175  #ifdef __SUPPRESSANYOUTPUT__
176  options.printLevel = PL_NONE;
177  #endif
178 
179  /* dimensions */
180  unsigned int nV=0, nC=0;
181 
182 
183  /* I) CONSISTENCY CHECKS: */
184  /* 1) Ensure that qpOASES is called with a feasible number of input arguments. */
185  if ( ( nrhs < 6 ) || ( nrhs > 10 ) )
186  if ( nrhs != 1 )
187  mexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
188 
189  /* 2) Ensure that first input is a string ... */
190  if ( mxIsChar( prhs[0] ) != 1 )
191  mexErrMsgTxt( "ERROR (qpOASES): First input argument must be a string!" );
192 
193  typeString = (char*) mxGetPr( prhs[0] );
194 
195  /* ... and if so, check if it is an allowed one. */
196  if ( ( strcmp( typeString,"i" ) != 0 ) && ( strcmp( typeString,"I" ) != 0 ) &&
197  ( strcmp( typeString,"h" ) != 0 ) && ( strcmp( typeString,"H" ) != 0 ) &&
198  ( strcmp( typeString,"c" ) != 0 ) && ( strcmp( typeString,"C" ) != 0 ) )
199  {
200  mexErrMsgTxt( "ERROR (qpOASES): Undefined first input argument!\nType 'help qpOASES_sequence' for further information." );
201  }
202 
203 
204  /* II) SELECT RESPECTIVE QPOASES FUNCTION CALL: */
205  /* 1) Init (without or with initial guess for primal solution). */
206  if ( ( strcmp( typeString,"i" ) == 0 ) || ( strcmp( typeString,"I" ) == 0 ) )
207  {
208  /* consistency checks */
209  if ( ( nlhs < 1 ) || ( nlhs > 5 ) )
210  mexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
211 
212  if ( ( nrhs < 8 ) || ( nrhs > 10 ) )
213  mexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
214 
215  /* ensure that data is given in real_t precision */
216  if ( ( mxIsDouble( prhs[1] ) == 0 ) ||
217  ( mxIsDouble( prhs[2] ) == 0 ) ||
218  ( mxIsDouble( prhs[3] ) == 0 ) )
219  mexErrMsgTxt( "ERROR (qpOASES): All data has to be provided in real_t precision!" );
220 
221 
222  /* ensure that matrices are stored in dense format */
223 // if ( ( mxIsSparse( prhs[1] ) != 0 ) || ( mxIsSparse( prhs[3] ) != 0 ) )
224 // mexErrMsgTxt( "ERROR (qpOASES): Matrices must not be stored in sparse format!" );
225 
226  /* Check inputs dimensions and assign pointers to inputs. */
227  nV = mxGetM( prhs[1] ); /* row number of Hessian matrix */
228  nC = mxGetM( prhs[3] ); /* row number of constraint matrix */
229 
230  if ( ( mxGetN( prhs[1] ) != nV ) || ( ( mxGetN( prhs[3] ) != 0 ) && ( mxGetN( prhs[3] ) != nV ) ) )
231  mexErrMsgTxt( "ERROR (qpOASES): Input dimension mismatch!" );
232 
233 
234  if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,2 ) != SUCCESSFUL_RETURN )
235  return;
236 
237  if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,4 ) != SUCCESSFUL_RETURN )
238  return;
239 
240  if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,5 ) != SUCCESSFUL_RETURN )
241  return;
242 
243  if ( smartDimensionCheck( &lbA,nC,1, BT_TRUE,prhs,6 ) != SUCCESSFUL_RETURN )
244  return;
245 
246  if ( smartDimensionCheck( &ubA,nC,1, BT_TRUE,prhs,7 ) != SUCCESSFUL_RETURN )
247  return;
248 
249  /* default value for nWSR */
250  int nWSRin = 5*(nV+nC);
251 
252 
253  /* Check whether x0 and options are specified .*/
254  if ( nrhs > 8 )
255  {
256  if ( smartDimensionCheck( &x0,nV,1, BT_TRUE,prhs,8 ) != SUCCESSFUL_RETURN )
257  return;
258 
259  if ( nrhs > 9 )
260  if ( ( !mxIsEmpty( prhs[9] ) ) && ( mxIsStruct( prhs[9] ) ) )
261  setupOptions( &options,prhs[9],nWSRin );
262  }
263 
266 
267  /* check for sparsity */
268  if ( mxIsSparse( prhs[1] ) != 0 )
269  {
270  long *ir = (long *)mxGetIr(prhs[1]);
271  long *jc = (long *)mxGetJc(prhs[1]);
272  real_t *v = (real_t*)mxGetPr(prhs[1]);
273  /*
274  for (long col = 0; col < nV; col++)
275  for (long idx = jc[col]; idx < jc[col+1]; idx++)
276  mexPrintf(" (%ld,%ld) %12.4f\n", ir[idx]+1, col+1, v[idx]);
277  */
278  //mexPrintf( "%ld\n", ir[0] );
279  SymSparseMat *sH;
280  globalQP_H = sH = new SymSparseMat(nV, nV, ir, jc, v);
282  }
283  else
284  {
285  H_for = (real_t*) mxGetPr( prhs[1] );
286  H_mem = new real_t[nV*nV];
287  for( int i=0; i<nV*nV; ++i )
288  H_mem[i] = H_for[i];
289  globalQP_H = new SymDenseMat( nV, nV, nV, H_mem );
290  globalQP_H->doFreeMemory();
291  }
292 
293  /* Convert constraint matrix A from FORTRAN to C style
294  * (not necessary for H as it should be symmetric!). */
295  if ( nC > 0 )
296  {
297  /* Check for sparsity. */
298  if ( mxIsSparse( prhs[3] ) != 0 )
299  {
300  long *ir = (long *)mxGetIr(prhs[3]);
301  long *jc = (long *)mxGetJc(prhs[3]);
302  real_t *v = (real_t*)mxGetPr(prhs[3]);
303  // mind pointer offsets due to 1-based indexing in Matlab
304  globalQP_A = new SparseMatrix(nC, nV, ir, jc, v);
305  }
306  else
307  {
308  /* Convert constraint matrix A from FORTRAN to C style
309  * (not necessary for H as it should be symmetric!). */
310  A_for = (real_t*) mxGetPr( prhs[3] );
311  A_mem = new real_t[nC*nV];
312  convertFortranToC( A_for,nV,nC, A_mem );
313  globalQP_A = new DenseMatrix(nC, nV, nV, A_mem );
314  globalQP_A->doFreeMemory();
315  }
316  }
317 
318  /* Create output vectors and assign pointers to them. */
319  allocateOutputs( nlhs,plhs, nV,nC );
320 
321  /* Call qpOASES. */
322  init( nV,nC,
323  globalQP_H,g,globalQP_A,
324  lb,ub,lbA,ubA,
325  nWSRin,x0,&options,
326  nlhs,plhs
327  );
328 
329  return;
330  }
331 
332  /* 2) Hotstart. */
333  if ( ( strcmp( typeString,"h" ) == 0 ) || ( strcmp( typeString,"H" ) == 0 ) )
334  {
335  /* consistency checks */
336  if ( ( nlhs < 1 ) || ( nlhs > 5 ) )
337  mexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
338 
339  if ( ( nrhs < 6 ) || ( nrhs > 7 ) )
340  mexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
341 
342  /* has QP been initialised? */
343  if ( globalQP == 0 )
344  mexErrMsgTxt( "ERROR (qpOASES): QP sequence needs to be initialised first!" );
345 
346 
347  /* Check inputs dimensions and assign pointers to inputs. */
348  nV = globalQP->getNV( );
349  nC = globalQP->getNC( );
350 
351  if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,1 ) != SUCCESSFUL_RETURN )
352  return;
353 
354  if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,2 ) != SUCCESSFUL_RETURN )
355  return;
356 
357  if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,3 ) != SUCCESSFUL_RETURN )
358  return;
359 
360  if ( smartDimensionCheck( &lbA,nC,1, BT_TRUE,prhs,4 ) != SUCCESSFUL_RETURN )
361  return;
362 
363  if ( smartDimensionCheck( &ubA,nC,1, BT_TRUE,prhs,5 ) != SUCCESSFUL_RETURN )
364  return;
365 
366  /* default value for nWSR */
367  int nWSRin = 5*(nV+nC);
368 
369  /* Check whether options are specified .*/
370  if ( nrhs == 7 )
371  if ( ( !mxIsEmpty( prhs[6] ) ) && ( mxIsStruct( prhs[6] ) ) )
372  setupOptions( &options,prhs[6],nWSRin );
373 
374  /* Create output vectors and assign pointers to them. */
375  allocateOutputs( nlhs,plhs, nV,nC );
376 
377  /* call qpOASES */
378  hotstart( g,
379  lb,ub,lbA,ubA,
380  nWSRin,&options,
381  nlhs,plhs
382  );
383 
384  return;
385  }
386 
387  /* 3) Cleanup. */
388  if ( ( strcmp( typeString,"c" ) == 0 ) || ( strcmp( typeString,"C" ) == 0 ) )
389  {
390  /* consistency checks */
391  if ( nlhs != 0 )
392  mexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
393 
394  if ( nrhs != 1 )
395  mexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
396 
397  /* Cleanup global QProblem instance. */
400 
401  return;
402  }
403 }
404 
405 /*
406  * end of file
407  */
Interfaces matrix-vector operations tailored to symmetric sparse matrices.
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)
returnValue setupOptions(Options *options, const mxArray *optionsPtr, int &nWSRin)
Allows to pass back messages to the calling function.
Interfaces matrix-vector operations tailored to symmetric dense matrices.
void allocateGlobalQProblemInstance(int nV, int nC, Options *options)
int getNV() const
returnValue smartDimensionCheck(real_t **input, unsigned int m, unsigned int n, BooleanType emptyAllowed, const mxArray *prhs[], int idx)
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
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
returnValue setOptions(const Options &_options)
#define PL_HIGH
Interfaces matrix-vector operations tailored to general dense matrices.
void allocateOutputs(int nlhs, mxArray *plhs[], int nV, int nC=0, int nP=1)
Interfaces matrix-vector operations tailored to general sparse matrices.
Provides a generic way to set and pass user-specified options.
Definition: options.hpp:65
Abstract base class for interfacing tailored matrix-vector operations.
#define PL_LOW
#define v
void hotstart(const real_t *const g, const real_t *const lb, const real_t *const ub, const real_t *const lbA, const real_t *const ubA, int nWSR, Options *options, int nOutputs, mxArray *plhs[])
int getNC() const
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 BT_FALSE
Definition: acado_types.hpp:49
double real_t
Definition: AD_test.c:10
Implements the online active set strategy for QPs with general constraints.
void init(int nV, int nC, 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 nWSR, const real_t *const x0, Options *options, int nOutputs, mxArray *plhs[])
static SymmetricMatrix * globalQP_H
Abstract base class for interfacing matrix-vector operations tailored to symmetric matrices...


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