qpOASES-3.0beta/interfaces/matlab/qpOASES.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 
36 #include <qpOASES/QProblem.hpp>
37 
38 
39 using namespace qpOASES;
40 
41 #include "qpOASES_matlab_utils.cpp"
42 
43 
44 
45 /*
46  * q p O A S E S m e x _ c o n s t r a i n t s
47  */
48 void qpOASESmex_constraints( int nV, int nC, int nP,
51  int nWSRin, real_t* x0, Options* options,
52  int nOutputs, mxArray* plhs[]
53  )
54 {
55  /* 1) Setup initial QP. */
56  QProblem QP( nV,nC );
57  QP.setOptions( *options );
58 
59  /* 2) Solve initial QP. */
60  returnValue returnvalue;
61 
62  if ( x0 == 0 )
63  returnvalue = QP.init( H,g,A,lb,ub,lbA,ubA, nWSRin,0 );
64  else
65  returnvalue = QP.init( H,g,A,lb,ub,lbA,ubA, nWSRin,0, x0,0,0,0 );
66 
67  /* 3) Solve remaining QPs and assign lhs arguments. */
68  /* Set up pointers to the current QP vectors */
69  real_t* g_current = g;
70  real_t* lb_current = lb;
71  real_t* ub_current = ub;
72  real_t* lbA_current = lbA;
73  real_t* ubA_current = ubA;
74 
75  /* Loop through QP sequence. */
76  for ( int k=0; k<nP; ++k )
77  {
78  if ( k != 0 )
79  {
80  /* update pointers to the current QP vectors */
81  g_current = &(g[k*nV]);
82  if ( lb != 0 )
83  lb_current = &(lb[k*nV]);
84  if ( ub != 0 )
85  ub_current = &(ub[k*nV]);
86  if ( lbA != 0 )
87  lbA_current = &(lbA[k*nC]);
88  if ( ubA != 0 )
89  ubA_current = &(ubA[k*nC]);
90 
91  returnvalue = QP.hotstart( g_current,lb_current,ub_current,lbA_current,ubA_current, nWSRin,0 );
92  }
93 
94  /* write results into output vectors */
95  obtainOutputs( k,&QP,returnvalue,nWSRin,
96  nOutputs,plhs,nV,nC );
97  }
98 
99  return;
100 }
101 
102 
103 /*
104  * q p O A S E S m e x _ b o u n d s
105  */
106 void qpOASESmex_bounds( int nV, int nP,
108  real_t* lb, real_t* ub,
109  int nWSRin, real_t* x0, Options* options,
110  int nOutputs, mxArray* plhs[]
111  )
112 {
113  /* 1) Setup initial QP. */
114  QProblemB QP( nV );
115  QP.setOptions( *options );
116 
117  /* 2) Solve initial QP. */
118  returnValue returnvalue;
119 
120  if ( x0 == 0 )
121  returnvalue = QP.init( H,g,lb,ub, nWSRin,0 );
122  else
123  returnvalue = QP.init( H,g,lb,ub, nWSRin,0, x0,0,0 );
124 
125  /* 3) Solve remaining QPs and assign lhs arguments. */
126  /* Set up pointers to the current QP vectors */
127  real_t* g_current = g;
128  real_t* lb_current = lb;
129  real_t* ub_current = ub;
130 
131  /* Loop through QP sequence. */
132  for ( int k=0; k<nP; ++k )
133  {
134  if ( k != 0 )
135  {
136  /* update pointers to the current QP vectors */
137  g_current = &(g[k*nV]);
138  if ( lb != 0 )
139  lb_current = &(lb[k*nV]);
140  if ( ub != 0 )
141  ub_current = &(ub[k*nV]);
142 
143  returnvalue = QP.hotstart( g_current,lb_current,ub_current, nWSRin,0 );
144  }
145 
146  /* write results into output vectors */
147  obtainOutputs( k,&QP,returnvalue,nWSRin,
148  nOutputs,plhs,nV );
149  }
150 
151  return;
152 }
153 
154 
155 
156 /*
157  * m e x F u n c t i o n
158  */
159 void mexFunction( int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[] )
160 {
161  /* inputs */
162  SymmetricMatrix *H=0;
163  Matrix *A=0;
164  real_t *g=0, *A_for=0, *A_mem=0, *lb=0, *ub=0, *lbA=0, *ubA=0, *x0=0;
165  int H_idx, g_idx, A_idx, lb_idx, ub_idx, lbA_idx, ubA_idx, x0_idx=-1, options_idx=-1;
166 
168  options.printLevel = PL_LOW;
169  #ifdef __DEBUG__
170  options.printLevel = PL_HIGH;
171  #endif
172  #ifdef __SUPPRESSANYOUTPUT__
173  options.printLevel = PL_NONE;
174  #endif
175 
176  /* dimensions */
177  unsigned int nV=0, nC=0, nP=0;
178 
179  /* sparse matrix indices */
180  long *Hdiag = 0;
181 
182  /* I) CONSISTENCY CHECKS: */
183  /* 1) Ensure that qpOASES is called with a feasible number of input arguments. */
184  if ( ( nrhs < 4 ) || ( nrhs > 9 ) )
185  mexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\n Type 'help qpOASES' for further information." );
186 
187  /* 2) Check for proper number of output arguments. */
188  if ( nlhs > 5 )
189  mexErrMsgTxt( "ERROR (qpOASES): At most five output arguments are allowed: \n [obj,x,y,status,nWSRout]!" );
190  if ( nlhs < 1 )
191  mexErrMsgTxt( "ERROR (qpOASES): At least one output argument is required: [obj,...]!" );
192 
193 
194  /* II) PREPARE RESPECTIVE QPOASES FUNCTION CALL: */
195  /* Choose between QProblem and QProblemB object and assign the corresponding
196  * indices of the input pointer array in to order to access QP data correctly. */
197  H_idx = 0;
198  g_idx = 1;
199  nV = mxGetM( prhs[ H_idx ] ); /* row number of Hessian matrix */
200  nP = mxGetN( prhs[ g_idx ] ); /* number of columns of the gradient matrix (vectors series have to be stored columnwise!) */
201 
202  /* 0) Check whether options are specified .*/
203  if ( ( !mxIsEmpty( prhs[nrhs-1] ) ) && ( mxIsStruct( prhs[nrhs-1] ) ) )
204  options_idx = nrhs-1;
205 
206  /* 1) Simply bounded QP. */
207  if ( ( ( nrhs >= 4 ) && ( nrhs <= 5 ) ) ||
208  ( ( options_idx > 0 ) && ( nrhs == 6 ) ) )
209  {
210  lb_idx = 2;
211  ub_idx = 3;
212 
213  if ( nrhs >= 5 ) /* x0 specified */
214  x0_idx = 4;
215  else
216  x0_idx = -1;
217  }
218  else
219  {
220  A_idx = 2;
221 
222  /* If constraint matrix is empty, use a QProblemB object! */
223  if ( mxIsEmpty( prhs[ A_idx ] ) )
224  {
225  lb_idx = 3;
226  ub_idx = 4;
227 
228  if ( nrhs >= 8 ) /* x0 specified */
229  x0_idx = 7;
230  else
231  x0_idx = -1;
232  }
233  else
234  {
235  lb_idx = 3;
236  ub_idx = 4;
237  lbA_idx = 5;
238  ubA_idx = 6;
239 
240  if ( nrhs >= 8 ) /* x0 specified */
241  x0_idx = 7;
242  else
243  x0_idx = -1;
244 
245  nC = mxGetM( prhs[ A_idx ] ); /* row number of constraint matrix */
246  }
247  }
248 
249 
250  /* III) ACTUALLY PERFORM QPOASES FUNCTION CALL: */
251  int nWSRin = 5*(nV+nC);
252  if ( options_idx > 0 )
253  setupOptions( &options,prhs[options_idx],nWSRin );
254 
255  /* ensure that data is given in real_t precision */
256  if ( ( mxIsDouble( prhs[ H_idx ] ) == 0 ) ||
257  ( mxIsDouble( prhs[ g_idx ] ) == 0 ) )
258  mexErrMsgTxt( "ERROR (qpOASES): All data has to be provided in double precision!" );
259 
260  /* Check inputs dimensions and assign pointers to inputs. */
261  if ( mxGetN( prhs[ H_idx ] ) != nV )
262  {
263  char msg[200];
264  snprintf(msg, 199, "ERROR (qpOASES): Hessian matrix input dimension mismatch (%ld != %d)!",
265  (long int)mxGetN(prhs[H_idx]), nV);
266  fprintf(stderr, "%s\n", msg);
267  mexErrMsgTxt(msg);
268  }
269 
270  /* check for sparsity */
271  if ( mxIsSparse( prhs[ H_idx ] ) != 0 )
272  {
273  long *ir = (long *)mxGetIr(prhs[H_idx]);
274  long *jc = (long *)mxGetJc(prhs[H_idx]);
275  real_t *v = (real_t*)mxGetPr(prhs[H_idx]);
276  /*
277  for (long col = 0; col < nV; col++)
278  for (long idx = jc[col]; idx < jc[col+1]; idx++)
279  mexPrintf(" (%ld,%ld) %12.4f\n", ir[idx]+1, col+1, v[idx]);
280  */
281  //mexPrintf( "%ld\n", ir[0] );
282  SymSparseMat *sH;
283  H = sH = new SymSparseMat(nV, nV, ir, jc, v);
284  Hdiag = sH->createDiagInfo();
285  }
286  else
287  {
288  H = new SymDenseMat(nV, nV, nV, (real_t*) mxGetPr(prhs[H_idx]));
289  }
290 
291  if ( smartDimensionCheck( &g,nV,nP, BT_FALSE,prhs,g_idx ) != SUCCESSFUL_RETURN )
292  return;
293 
294  if ( smartDimensionCheck( &lb,nV,nP, BT_TRUE,prhs,lb_idx ) != SUCCESSFUL_RETURN )
295  return;
296 
297  if ( smartDimensionCheck( &ub,nV,nP, BT_TRUE,prhs,ub_idx ) != SUCCESSFUL_RETURN )
298  return;
299 
300  if ( smartDimensionCheck( &x0,nV,1, BT_TRUE,prhs,x0_idx ) != SUCCESSFUL_RETURN )
301  return;
302 
303  if ( nC > 0 )
304  {
305  /* ensure that data is given in real_t precision */
306  if ( mxIsDouble( prhs[ A_idx ] ) == 0 )
307  mexErrMsgTxt( "ERROR (qpOASES): All data has to be provided in real_t precision!" );
308 
309  /* Check inputs dimensions and assign pointers to inputs. */
310  if ( mxGetN( prhs[ A_idx ] ) != nV )
311  {
312  char msg[200];
313  snprintf(msg, 199, "ERROR (qpOASES): Constraint matrix input dimension mismatch (%ld != %d)!",
314  (long int)mxGetN(prhs[A_idx]), nV);
315  fprintf(stderr, "%s\n", msg);
316  mexErrMsgTxt(msg);
317  }
318 
319  A_for = (real_t*) mxGetPr( prhs[ A_idx ] );
320 
321  if ( smartDimensionCheck( &lbA,nC,nP, BT_TRUE,prhs,lbA_idx ) != SUCCESSFUL_RETURN )
322  return;
323 
324  if ( smartDimensionCheck( &ubA,nC,nP, BT_TRUE,prhs,ubA_idx ) != SUCCESSFUL_RETURN )
325  return;
326 
327  /* Check for sparsity. */
328  if ( mxIsSparse( prhs[ A_idx ] ) != 0 )
329  {
330  long *ir = (long *)mxGetIr(prhs[A_idx]);
331  long *jc = (long *)mxGetJc(prhs[A_idx]);
332  real_t *v = (real_t*)mxGetPr(prhs[A_idx]);
333  A = new SparseMatrix(nC, nV, ir, jc, v);
334  }
335  else
336  {
337  /* Convert constraint matrix A from FORTRAN to C style
338  * (not necessary for H as it should be symmetric!). */
339  A_mem = new real_t[nC*nV];
340  convertFortranToC( A_for,nV,nC, A_mem );
341  A = new DenseMatrix(nC, nV, nV, A_mem );
342  A->doFreeMemory();
343  }
344  }
345 
346  allocateOutputs( nlhs,plhs,nV,nC,nP );
347 
348  if ( nC == 0 )
349  {
350  /* call qpOASES */
351  qpOASESmex_bounds( nV,nP,
352  H,g,
353  lb,ub,
354  nWSRin,x0,&options,
355  nlhs,plhs
356  );
357 
358  delete H;
359  if (Hdiag) delete[] Hdiag;
360  return;
361  /* 2) Call usual version including constraints (using QProblem class) */
362  }
363  else
364  {
365  /* Call qpOASES. */
366  qpOASESmex_constraints( nV,nC,nP,
367  H,g,A,
368  lb,ub,lbA,ubA,
369  nWSRin,x0,&options,
370  nlhs,plhs
371  );
372 
373  delete H; delete A;
374  if (Hdiag) delete[] Hdiag;
375  return;
376  }
377 }
378 
379 /*
380  * end of file
381  */
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)
Implements the online active set strategy for box-constrained QPs.
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.
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)
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
#define PL_NONE
returnValue setOptions(const Options &_options)
#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)
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
void qpOASESmex_bounds(int nV, int nP, SymmetricMatrix *H, real_t *g, real_t *lb, real_t *ub, int nWSRin, real_t *x0, Options *options, int nOutputs, mxArray *plhs[])
Abstract base class for interfacing tailored matrix-vector operations.
#define PL_LOW
#define v
void obtainOutputs(int k, QProblemB *qp, returnValue returnvalue, int nWSRin, int nlhs, mxArray *plhs[], int nV, int nC=0)
void qpOASESmex_constraints(int nV, int nC, int nP, SymmetricMatrix *H, real_t *g, Matrix *A, real_t *lb, real_t *ub, real_t *lbA, real_t *ubA, int nWSRin, real_t *x0, Options *options, int nOutputs, mxArray *plhs[])
#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.
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:00