qpOASES-3.0beta/interfaces/simulink/qpOASES_QProblem.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 
37 #include <stdlib.h>
38 
39 #include <qpOASES/QProblem.hpp>
40 
41 
42 #ifdef __cplusplus
43 extern "C" {
44 #endif
45 
46 
47 #define S_FUNCTION_NAME qpOASES_QProblem
48 #define S_FUNCTION_LEVEL 2
50 #define MDL_START
52 #include "simstruc.h"
53 
54 
55 /* SETTINGS: */
56 #define SAMPLINGTIME 0.1
57 #define NCONTROLINPUTS 2
58 #define NWSR 10
61 static void mdlInitializeSizes (SimStruct *S) /* Init sizes array */
62 {
63  int nU = NCONTROLINPUTS;
64 
65  /* Specify the number of continuous and discrete states */
66  ssSetNumContStates(S, 0);
67  ssSetNumDiscStates(S, 0);
68 
69  /* Specify the number of intput ports */
70  if ( !ssSetNumInputPorts(S, 7) )
71  return;
72 
73  /* Specify the number of output ports */
74  if ( !ssSetNumOutputPorts(S, 4) )
75  return;
76 
77  /* Specify dimension information for the input ports */
78  ssSetInputPortVectorDimension(S, 0, DYNAMICALLY_SIZED);
79  ssSetInputPortVectorDimension(S, 1, DYNAMICALLY_SIZED);
80  ssSetInputPortVectorDimension(S, 2, DYNAMICALLY_SIZED);
81  ssSetInputPortVectorDimension(S, 3, DYNAMICALLY_SIZED);
82  ssSetInputPortVectorDimension(S, 4, DYNAMICALLY_SIZED);
83  ssSetInputPortVectorDimension(S, 5, DYNAMICALLY_SIZED);
84  ssSetInputPortVectorDimension(S, 6, DYNAMICALLY_SIZED);
85 
86  /* Specify dimension information for the output ports */
87  ssSetOutputPortVectorDimension(S, 0, 1 );
88  ssSetOutputPortVectorDimension(S, 1, nU );
89  ssSetOutputPortVectorDimension(S, 2, 1 );
90  ssSetOutputPortVectorDimension(S, 3, 1 );
91 
92  /* Specify the direct feedthrough status */
93  ssSetInputPortDirectFeedThrough(S, 0, 1);
94  ssSetInputPortDirectFeedThrough(S, 1, 1);
95  ssSetInputPortDirectFeedThrough(S, 2, 1);
96  ssSetInputPortDirectFeedThrough(S, 3, 1);
97  ssSetInputPortDirectFeedThrough(S, 4, 1);
98  ssSetInputPortDirectFeedThrough(S, 5, 1);
99  ssSetInputPortDirectFeedThrough(S, 6, 1);
100 
101  /* One sample time */
102  ssSetNumSampleTimes(S, 1);
103 
104 
105  /* global variables:
106  * 0: problem
107  * 1: H
108  * 2: g
109  * 3: A
110  * 4: lb
111  * 5: ub
112  * 6: lbA
113  * 7: ubA
114  * 8: count
115  */
116 
117  /* Specify the size of the block's pointer work vector */
118  ssSetNumPWork(S, 9);
119 }
120 
121 
122 #if defined(MATLAB_MEX_FILE)
123 
124 #define MDL_SET_INPUT_PORT_DIMENSION_INFO
125 #define MDL_SET_OUTPUT_PORT_DIMENSION_INFO
126 
127 static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
128 {
129  if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) )
130  return;
131 }
132 
133 static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
134 {
135  if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) )
136  return;
137 }
138 
139 #endif
140 
141 
142 static void mdlInitializeSampleTimes(SimStruct *S)
143 {
144  ssSetSampleTime(S, 0, SAMPLINGTIME);
145  ssSetOffsetTime(S, 0, 0.0);
146 }
147 
148 
149 static void mdlStart(SimStruct *S)
150 {
151  #ifndef __DSPACE__
152  using namespace qpOASES;
153  #endif
154 
155  int nU = NCONTROLINPUTS;
156  int size_H, size_g, size_A, size_lb, size_ub, size_lbA, size_ubA;
157  int nV, nC;
158 
159  QProblem* problem;
160  real_t* count;
161 
162 
163  /* get block inputs dimensions */
164  size_H = ssGetInputPortWidth(S, 0);
165  size_g = ssGetInputPortWidth(S, 1);
166  size_A = ssGetInputPortWidth(S, 2);
167  size_lb = ssGetInputPortWidth(S, 3);
168  size_ub = ssGetInputPortWidth(S, 4);
169  size_lbA = ssGetInputPortWidth(S, 5);
170  size_ubA = ssGetInputPortWidth(S, 6);
171 
172 
173  /* dimension checks */
174  nV = size_g;
175  nC = (int) ( ((real_t) size_A) / ((real_t) nV) );
176 
177  if ( nV == 0 )
178  {
179  #ifndef __DSPACE__
180  mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
181  #endif
182  return;
183  }
184 
185  if ( size_H != nV*nV )
186  {
187  #ifndef __DSPACE__
188  mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
189  #endif
190  return;
191  }
192 
193  if ( nU > nV )
194  {
195  #ifndef __DSPACE__
196  mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
197  #endif
198  return;
199  }
200 
201  if ( size_lb != nV )
202  {
203  #ifndef __DSPACE__
204  mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
205  #endif
206  return;
207  }
208 
209  if ( size_ub != nV )
210  {
211  #ifndef __DSPACE__
212  mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
213  #endif
214  return;
215  }
216 
217  if ( size_lbA != nC )
218  {
219  #ifndef __DSPACE__
220  mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
221  #endif
222  return;
223  }
224 
225  if ( size_ubA != nC )
226  {
227  #ifndef __DSPACE__
228  mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
229  #endif
230  return;
231  }
232 
233 
234  /* allocate QProblem object */
235  problem = new QProblem( nV,nC );
236  if ( problem == 0 )
237  {
238  #ifndef __DSPACE__
239  mexErrMsgTxt( "ERROR (qpOASES): Unable to create QProblem object!" );
240  #endif
241  return;
242  }
243 
244  #ifndef __DEBUG__
245  problem->setPrintLevel( PL_LOW );
246  #endif
247  #ifdef __SUPPRESSANYOUTPUT__
248  problem->setPrintLevel( PL_NONE );
249  #endif
250  #ifdef __DSPACE__
251  problem->setPrintLevel( PL_NONE );
252  #endif
253 
254  ssGetPWork(S)[0] = (void *) problem;
255 
256  /* allocate memory for QP data ... */
257  ssGetPWork(S)[1] = (void *) calloc( size_H, sizeof(real_t) ); /* H */
258  ssGetPWork(S)[2] = (void *) calloc( size_g, sizeof(real_t) ); /* g */
259  ssGetPWork(S)[3] = (void *) calloc( size_A, sizeof(real_t) ); /* A */
260  ssGetPWork(S)[4] = (void *) calloc( size_lb, sizeof(real_t) ); /* lb */
261  ssGetPWork(S)[5] = (void *) calloc( size_ub, sizeof(real_t) ); /* ub */
262  ssGetPWork(S)[6] = (void *) calloc( size_lbA, sizeof(real_t) ); /* lbA */
263  ssGetPWork(S)[7] = (void *) calloc( size_ubA, sizeof(real_t) ); /* ubA */
264  ssGetPWork(S)[8] = (void *) calloc( 1, sizeof(real_t) ); /* count */
265 
266  /* reset counter */
267  count = (real_t *) ssGetPWork(S)[8];
268  count[0] = 0.0;
269 }
270 
271 
272 static void mdlOutputs(SimStruct *S, int_T tid)
273 {
274  #ifndef __DSPACE__
275  using namespace qpOASES;
276  #endif
277 
278  int i;
279  int nV, nC, status;
280 
281  int nWSR = NWSR;
282  int nU = NCONTROLINPUTS;
283 
284  InputRealPtrsType in_H, in_g, in_A, in_lb, in_ub, in_lbA, in_ubA;
285 
286  QProblem* problem;
287  real_t *H, *g, *A, *lb, *ub, *lbA, *ubA, *count;
288 
289  real_t *xOpt;
290 
291  real_T *out_objVal, *out_xOpt, *out_status, *out_nWSR;
292 
293 
294  /* get pointers to block inputs ... */
295  in_H = ssGetInputPortRealSignalPtrs(S, 0);
296  in_g = ssGetInputPortRealSignalPtrs(S, 1);
297  in_A = ssGetInputPortRealSignalPtrs(S, 2);
298  in_lb = ssGetInputPortRealSignalPtrs(S, 3);
299  in_ub = ssGetInputPortRealSignalPtrs(S, 4);
300  in_lbA = ssGetInputPortRealSignalPtrs(S, 5);
301  in_ubA = ssGetInputPortRealSignalPtrs(S, 6);
302 
303 
304  /* ... and to the QP data */
305  problem = (QProblem *) ssGetPWork(S)[0];
306 
307  H = (real_t *) ssGetPWork(S)[1];
308  g = (real_t *) ssGetPWork(S)[2];
309  A = (real_t *) ssGetPWork(S)[3];
310  lb = (real_t *) ssGetPWork(S)[4];
311  ub = (real_t *) ssGetPWork(S)[5];
312  lbA = (real_t *) ssGetPWork(S)[6];
313  ubA = (real_t *) ssGetPWork(S)[7];
314 
315  count = (real_t *) ssGetPWork(S)[8];
316 
317 
318  /* setup QP data */
319  nV = ssGetInputPortWidth(S, 1); /* nV = size_g */
320  nC = (int) ( ((real_t) ssGetInputPortWidth(S, 2)) / ((real_t) nV) ); /* nC = size_A / size_g */
321 
322  for ( i=0; i<nV*nV; ++i )
323  H[i] = (*in_H)[i];
324 
325  for ( i=0; i<nC*nV; ++i )
326  A[i] = (*in_A)[i];
327 
328  for ( i=0; i<nV; ++i )
329  {
330  g[i] = (*in_g)[i];
331  lb[i] = (*in_lb)[i];
332  ub[i] = (*in_ub)[i];
333  }
334 
335  for ( i=0; i<nC; ++i )
336  {
337  lbA[i] = (*in_lbA)[i];
338  ubA[i] = (*in_ubA)[i];
339  }
340 
341  xOpt = new real_t[nV];
342 
343  if ( count[0] == 0 )
344  {
345  /* initialise and solve first QP */
346  status = problem->init( H,g,A,lb,ub,lbA,ubA, nWSR,0 );
347  ssGetPWork(S)[0] = ( void* ) problem;
348  problem->getPrimalSolution( xOpt );
349  }
350  else
351  {
352  /* solve neighbouring QP using hotstart technique */
353  status = problem->hotstart( g,lb,ub,lbA,ubA, nWSR,0 );
354  if ( ( status != SUCCESSFUL_RETURN ) && ( status != RET_MAX_NWSR_REACHED ) )
355  {
356  /* if an error occurs, reset problem data structures and initialise again */
357  problem->reset( );
358  problem->init( H,g,A,lb,ub,lbA,ubA, nWSR,0 );
359  }
360  else
361  {
362  /* otherwise obtain optimal solution */
363  problem->getPrimalSolution( xOpt );
364  }
365  }
366 
367  /* generate block output: status information ... */
368  out_objVal = ssGetOutputPortRealSignal(S, 0);
369  out_xOpt = ssGetOutputPortRealSignal(S, 1);
370  out_status = ssGetOutputPortRealSignal(S, 2);
371  out_nWSR = ssGetOutputPortRealSignal(S, 3);
372 
373  out_objVal[0] = ((real_T) problem->getObjVal( ));
374 
375  for ( i=0; i<nU; ++i )
376  out_xOpt[i] = ((real_T) xOpt[i]);
377 
378  switch ( status )
379  {
380  case SUCCESSFUL_RETURN:
381  out_status[0] = 0.0;
382  break;
383 
385  out_status[0] = 1.0;
386  break;
387 
390  out_status[0] = -2.0;
391  break;
392 
395  out_status[0] = -3.0;
396  break;
397 
398  default:
399  out_status[0] = -1.0;
400  break;
401  }
402 
403  out_nWSR[0] = ((real_T) nWSR);
404 
405  /* increase counter */
406  count[0] = count[0] + 1;
407 
408  delete[] xOpt;
409 }
410 
411 
412 static void mdlTerminate(SimStruct *S)
413 {
414  #ifndef __DSPACE__
415  using namespace qpOASES;
416  #endif
417 
418  /* reset global message handler */
420 
421  int i;
422  for ( i=0; i<9; ++i )
423  {
424  if ( ssGetPWork(S)[i] != 0 )
425  free( ssGetPWork(S)[i] );
426  }
427 }
428 
429 
430 #ifdef MATLAB_MEX_FILE
431 #include "simulink.c"
432 #else
433 #include "cg_sfun.h"
434 #endif
435 
436 
437 #ifdef __cplusplus
438 }
439 #endif
440 
441 
442 /*
443  * end of file
444  */
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 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)
#define PL_NONE
#define PL_LOW
double real_t
Definition: AD_test.c:10
Implements the online active set strategy for QPs with general constraints.


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