39 #include <qpOASES/QProblem.hpp> 47 #define S_FUNCTION_NAME qpOASES_QProblem 48 #define S_FUNCTION_LEVEL 2 56 #define SAMPLINGTIME 0.1 57 #define NCONTROLINPUTS 2 61 static void mdlInitializeSizes (SimStruct *S) 66 ssSetNumContStates(
S, 0);
67 ssSetNumDiscStates(
S, 0);
70 if ( !ssSetNumInputPorts(
S, 7) )
74 if ( !ssSetNumOutputPorts(
S, 4) )
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);
87 ssSetOutputPortVectorDimension(
S, 0, 1 );
88 ssSetOutputPortVectorDimension(
S, 1, nU );
89 ssSetOutputPortVectorDimension(
S, 2, 1 );
90 ssSetOutputPortVectorDimension(
S, 3, 1 );
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);
102 ssSetNumSampleTimes(
S, 1);
122 #if defined(MATLAB_MEX_FILE) 124 #define MDL_SET_INPUT_PORT_DIMENSION_INFO 125 #define MDL_SET_OUTPUT_PORT_DIMENSION_INFO 127 static void mdlSetInputPortDimensionInfo(SimStruct *
S, int_T port,
const DimsInfo_T *dimsInfo)
129 if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) )
133 static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port,
const DimsInfo_T *dimsInfo)
135 if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) )
145 ssSetOffsetTime(S, 0, 0.0);
156 int size_H, size_g, size_A, size_lb, size_ub, size_lbA, size_ubA;
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);
180 mexErrMsgTxt(
"ERROR (qpOASES): Dimension mismatch!" );
185 if ( size_H != nV*nV )
188 mexErrMsgTxt(
"ERROR (qpOASES): Dimension mismatch!" );
196 mexErrMsgTxt(
"ERROR (qpOASES): Dimension mismatch!" );
204 mexErrMsgTxt(
"ERROR (qpOASES): Dimension mismatch!" );
212 mexErrMsgTxt(
"ERROR (qpOASES): Dimension mismatch!" );
217 if ( size_lbA != nC )
220 mexErrMsgTxt(
"ERROR (qpOASES): Dimension mismatch!" );
225 if ( size_ubA != nC )
228 mexErrMsgTxt(
"ERROR (qpOASES): Dimension mismatch!" );
239 mexErrMsgTxt(
"ERROR (qpOASES): Unable to create QProblem object!" );
247 #ifdef __SUPPRESSANYOUTPUT__ 254 ssGetPWork(S)[0] = (
void *) problem;
257 ssGetPWork(S)[1] = (
void *) calloc( size_H,
sizeof(
real_t) );
258 ssGetPWork(S)[2] = (
void *) calloc( size_g,
sizeof(
real_t) );
259 ssGetPWork(S)[3] = (
void *) calloc( size_A,
sizeof(
real_t) );
260 ssGetPWork(S)[4] = (
void *) calloc( size_lb,
sizeof(
real_t) );
261 ssGetPWork(S)[5] = (
void *) calloc( size_ub,
sizeof(
real_t) );
262 ssGetPWork(S)[6] = (
void *) calloc( size_lbA,
sizeof(
real_t) );
263 ssGetPWork(S)[7] = (
void *) calloc( size_ubA,
sizeof(
real_t) );
264 ssGetPWork(S)[8] = (
void *) calloc( 1,
sizeof(
real_t) );
267 count = (
real_t *) ssGetPWork(S)[8];
284 InputRealPtrsType in_H, in_g, in_A, in_lb, in_ub, in_lbA, in_ubA;
291 real_T *out_objVal, *out_xOpt, *out_status, *out_nWSR;
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);
305 problem = (
QProblem *) ssGetPWork(S)[0];
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];
315 count = (
real_t *) ssGetPWork(S)[8];
319 nV = ssGetInputPortWidth(S, 1);
320 nC = (int) ( ((
real_t) ssGetInputPortWidth(S, 2)) / ((
real_t) nV) );
322 for ( i=0; i<nV*nV; ++i )
325 for ( i=0; i<nC*nV; ++i )
328 for ( i=0; i<nV; ++i )
335 for ( i=0; i<nC; ++i )
337 lbA[i] = (*in_lbA)[i];
338 ubA[i] = (*in_ubA)[i];
346 status = problem->
init( H,g,A,lb,ub,lbA,ubA, nWSR,0 );
347 ssGetPWork(S)[0] = (
void* ) problem;
353 status = problem->
hotstart( g,lb,ub,lbA,ubA, nWSR,0 );
358 problem->
init( H,g,A,lb,ub,lbA,ubA, nWSR,0 );
368 out_objVal = ssGetOutputPortRealSignal(S, 0);
369 out_xOpt = ssGetOutputPortRealSignal(S, 1);
370 out_status = ssGetOutputPortRealSignal(S, 2);
371 out_nWSR = ssGetOutputPortRealSignal(S, 3);
373 out_objVal[0] = ((real_T) problem->
getObjVal( ));
375 for ( i=0; i<nU; ++i )
376 out_xOpt[i] = ((real_T) xOpt[i]);
390 out_status[0] = -2.0;
395 out_status[0] = -3.0;
399 out_status[0] = -1.0;
403 out_nWSR[0] = ((real_T) nWSR);
406 count[0] = count[0] + 1;
422 for ( i=0; i<9; ++i )
424 if ( ssGetPWork(S)[i] != 0 )
425 free( ssGetPWork(S)[i] );
430 #ifdef MATLAB_MEX_FILE 431 #include "simulink.c" returnValue getPrimalSolution(real_t *const xOpt) const
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)
static void mdlOutputs(SimStruct *S, int_T tid)
static void mdlInitializeSampleTimes(SimStruct *S)
Implements the online active set strategy for QPs with general constraints.
MessageHandling * getGlobalMessageHandler()
static void mdlTerminate(SimStruct *S)
static void mdlStart(SimStruct *S)
returnValue setPrintLevel(PrintLevel _printlevel)