ACADO in Simulink

User Questions
1

Recently, I use ACADO to solve the OCP of NMPC in Simulink. During the simulation, we receive the error information as follow “NMPC step (error value: 33)”. And, I don’t know how to check and remove the error. Could you give me some suggestion?

2

Hi,

not sure if anyone can answer that here.
Is there a particular reason you are using ACADO instead of acados?
ACADO is not actively maintained since around 2018, see update README with note on current development and acados reference by FreyJo · Pull Request #304 · acado/acado · GitHub

Of course if anyone knows about ACADO, feel free to answer.

3

Thank your reply! I learned about the ACADOs when I first applied the optimizer, but because I couldn’t find a good code generation example, I applied the ACADO solver. Now, I don’t know to obtain the optimal solution. I am attaching my source code and the code generation code now, please help me check if there are any errors in the settings. Thank you very much!
EXPORT = 1;
DifferentialState LambdaF LambdaR BrakeTorF BrakeTorR;
Control DeltaTorF DeltaTorR;

OnlineData PedalTorqueF PedalTorqueR;
OnlineData Vx Ax MiuF MiuR;

%% System model
global VehicleModelParameter TireForce Control_Para
Wf = TireForce.Fzf;
Wr = TireForce.Fzr;
DFz = TireForce.DFz;
Rf = VehicleModelParameter.Rdius;
Rr = Rf;
Jf = VehicleModelParameter.Jw;
Jr = Jf;
Tao_f = VehicleModelParameter.Tao_f;
Tao_r = VehicleModelParameter.Tao_r;
Ts = 0.001;
N_Steps = Control_Para.Nsteps;

Phi_F = -RfMiuF(Wf - DFzAx) + Jf(1 - LambdaF)Ax/Rf;
Phi_r = -RrMiuR*(Wr + DFzAx) + Jr(1 - LambdaR)*Ax/Rr;

f = [dot(LambdaF) == Rf*(Phi_F + PedalTorqueF + DeltaTorF)/(JfVx);…
dot(LambdaR) == Rr(Phi_r + PedalTorqueR + DeltaTorR)/(Jr*Vx);…
dot(BrakeTorF) == (PedalTorqueF + DeltaTorF - BrakeTorF)/Tao_f;…
dot(BrakeTorR) == (PedalTorqueR + DeltaTorR - BrakeTorR)/Tao_r];

%% SIMexport
fprintf(‘-----------------------\n SIMexport \n-----------------------\n’);
acadoSet(‘problemname’,‘sim’);
sim = acado.SIMexport(Ts);
numSteps = 1;
sim.setModel(f);
sim.set(‘INTEGRATOR_TYPE’,‘INT_IRK_GL4’);
sim.set(‘NUM_INTEGRATOR_STEPS’,numSteps);

if EXPORT
sim.exportCode(‘export_sim’);
cd export_sim
make_acado_integrator(‘…/integrate_Brake’)
cd …
end

%% MPCexport
fprintf(‘-----------------------\n MPCexport \n----------------------\n’);

Opt_Vari = [LambdaF;LambdaR];
h = [Opt_Vari;controls];
hN = Opt_Vari;

W_mat = eye(length(h));
WN_mat = eye(length(hN));
W = acado.BMatrix(W_mat);
WN = acado.BMatrix(WN_mat);

ocp = acado.OCP( 0.0,N_Steps*Ts,N_Steps);
ocp.minimizeLSQ(W,h);
ocp.minimizeLSQEndTerm(WN,hN);

Slip_LB = 0.15;
Slip_UB = 0.3;
DeltaTorF_LB = -1000;
DeltaTorR_LB = -1000;
Control_Torque_UB = 0;
Wheel_Torque_UB = 2000;
Wheel_Torque_LB = 0;

% Constraints on control inputs
ocp.subjectTo(DeltaTorF_LB <= DeltaTorF <= Control_Torque_UB);
ocp.subjectTo(DeltaTorR_LB <= DeltaTorR <= Control_Torque_UB);
% Constraints on state variables (slip ratios)
ocp.subjectTo(Slip_LB <= LambdaF <= Slip_UB);
ocp.subjectTo(Slip_LB <= LambdaR <= Slip_UB);
% Constraints on Brake Torques
ocp.subjectTo(Wheel_Torque_LB <= BrakeTorF <= Wheel_Torque_UB);
ocp.subjectTo(Wheel_Torque_LB <= BrakeTorR <= Wheel_Torque_UB);
% Model definition is correct
ocp.setModel(f);

mpc = acado.OCPexport(ocp);
mpc.set(‘HESSIAN_APPROXIMATION’,‘GAUSS_NEWTON’);
mpc.set(‘QP_SOLVER’,‘QP_QPOASES’);
mpc.set(‘DISCRETIZATION_TYPE’,‘MULTIPLE_SHOOTING’);
mpc.set(‘HOTSTART_QP’,‘YES’);
mpc.set(‘LEVENBERG_MARQUARDT’,1e-1);
mpc.set(‘CG_HARDCODE_CONSTRAINT_VALUES’,‘NO’);
mpc.set(‘SPARSE_QP_SOLUTION’,‘FULL_CONDENSING’);
mpc.set(‘INTEGRATOR_TYPE’,‘INT_IRK_GL2’);
mpc.set(‘NUM_INTEGRATOR_STEPS’,N_Steps);

if EXPORT
mpc.exportCode( ‘export_mpc’ );
copyfile(‘F:\Matlab2022A\acado-master\external_packages\qpoases’,‘export_mpc/qpoases’)
cd export_mpc
make_sfunction(‘…/ABS_NMPC’)
cd …
end

#define S_FUNCTION_NAME ABS_NMPC
#define S_FUNCTION_LEVEL 2

#define MDL_START

extern “C” {

#include “acado_common.h”
#include “acado_auxiliary_functions.h”
#include “simstruc.h”

ACADOvariables acadoVariables;
ACADOworkspace acadoWorkspace;

static void mdlInitializeSizes (SimStruct S)
{
/ Specify the number of continuous and discrete states */
ssSetNumContStates(S, 0);
ssSetNumDiscStates(S, 0);

/* Specify the number of intput ports */
if ( !ssSetNumInputPorts(S, 5) )
    return;

/* Specify the number of output ports */
if ( !ssSetNumOutputPorts(S, 4) )
    return;

/* Specify the number of parameters */
ssSetNumSFcnParams(S, 7);
if ( ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S) )
    return;

/* Specify dimension information for the input ports */
ssSetInputPortVectorDimension(S, 0, ACADO_NX);
ssSetInputPortVectorDimension(S, 1, ACADO_NY);
ssSetInputPortVectorDimension(S, 2, ACADO_NOD);
ssSetInputPortVectorDimension(S, 3, ACADO_NY*ACADO_NY);
ssSetInputPortVectorDimension(S, 4, ACADO_NYN*ACADO_NYN);    

/* Specify dimension information for the output ports */
ssSetOutputPortVectorDimension(S, 0, ACADO_NU );
ssSetOutputPortVectorDimension(S, 1, 1 );
ssSetOutputPortVectorDimension(S, 2, ACADO_NX );
ssSetOutputPortVectorDimension(S, 3, 1 );

/* Specify the direct feedthrough status */
ssSetInputPortDirectFeedThrough(S, 0, 1);
ssSetInputPortDirectFeedThrough(S, 1, 1);
ssSetInputPortDirectFeedThrough(S, 2, 1);
ssSetInputPortDirectFeedThrough(S, 3, 1);
ssSetInputPortDirectFeedThrough(S, 4, 1);

/* One sample time */
ssSetNumSampleTimes(S, 1);
}

#if defined(MATLAB_MEX_FILE)

#define MDL_SET_INPUT_PORT_DIMENSION_INFO
#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO

static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
{
if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) )
return;
}

static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
{
if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) )
return;
}

#endif /* MATLAB_MEX_FILE */

static void mdlInitializeSampleTimes(SimStruct *S)
{
double SAMPLINGTIME = mxGetScalar( ssGetSFcnParam(S, 0) );

ssSetSampleTime(S, 0, SAMPLINGTIME);
ssSetOffsetTime(S, 0, 0.0);

}

static void mdlStart(SimStruct *S)
{
int i, j, k;

InputRealPtrsType in_ref, ODInit;

// double *xInit, *uInit, *ODInit, *Smat, *SNmat, *numSteps, *lbV, *ubV, *lbAV, *ubAV;
double *xInit, *uInit, *Smat, *SNmat, *lbV, *ubV;

/* get inputs and perform feedback step */
in_ref = ssGetInputPortRealSignalPtrs(S, 1);
ODInit = ssGetInputPortRealSignalPtrs(S, 3);

xInit = mxGetPr( ssGetSFcnParam(S, 1) );
uInit = mxGetPr( ssGetSFcnParam(S, 2) );

// ODInit = mxGetPr( ssGetSFcnParam(S, 3));

for( i=0; i < ACADO_N+1; ++i ) {
    for( j=0; j < ACADO_NX; ++j ) acadoVariables.x[i*ACADO_NX+j] = xInit[j];
}
for( i=0; i < ACADO_N; ++i ) {
    for( j=0; j < ACADO_NU; ++j ) acadoVariables.u[i*ACADO_NU+j] = uInit[j];
}

for( i=0; i < ACADO_N; ++i ) {
    for( j=0; j < ACADO_NY; ++j ) acadoVariables.y[i*ACADO_NY+j] = (double)(*in_ref[j]);
}

// for( i=0; i < ACADO_N+1; ++i){
// for( j=0, j < ACADO_NOD; ++j) acadoVariables.od[i*ACADO_NOD+j] = ODInit[j];
// }
for( i=0; i < ACADO_N+1; ++i){
for( j=0; j < ACADO_NOD; ++j) acadoVariables.od[i*ACADO_NOD+j] = (double)(*ODInit[j]);
}
for( i=0; i < ACADO_NYN; ++i ) acadoVariables.yN[i] = (double)(*in_ref[i]);
// for( i=0; i < ACADO_NOD; ++i ) acadoVariables.od[i] = (double)(*ODInit[i]);

Smat = mxGetPr( ssGetSFcnParam(S, 3) );
SNmat = mxGetPr( ssGetSFcnParam(S, 4) );

// numSteps = mxGetPr( ssGetSFcnParam(S, 5) );

for( i = 0; i < (ACADO_NYN); ++i )  {
    for( j = 0; j < ACADO_NYN; ++j ) {
        acadoVariables.WN[i*ACADO_NYN+j] = SNmat[i*ACADO_NYN+j];
    }
}
for( i = 0; i < (ACADO_NY); ++i )  {
    for( j = 0; j < ACADO_NY; ++j ) {
        acadoVariables.W[i*ACADO_NY+j] = Smat[i*ACADO_NY+j];
    }
}

lbV = mxGetPr( ssGetSFcnParam(S, 5) );
ubV = mxGetPr( ssGetSFcnParam(S, 6) );

// lbAV = mxGetPr( ssGetSFcnParam(S, 8) );
// ubAV = mxGetPr( ssGetSFcnParam(S, 9) );
//
for( i=0; i < ACADO_N; ++i ) {
for( j=0; j < ACADO_NU; ++j ) acadoVariables.lbValues[i*ACADO_NU+j] = lbV[j];
for( j=0; j < ACADO_NU; ++j ) acadoVariables.ubValues[i*ACADO_NU+j] = ubV[j];
//
// for( j=0; j < ACADO_NU; ++j ) acadoVariables.lbAValues[i*ACADO_NU+j] = lbAV[j];
// for( j=0; j < ACADO_NU; ++j ) acadoVariables.ubAValues[i*ACADO_NU+j] = ubAV[j];
}
acado_initializeSolver();
// acado_preparationStep( );
}

static void mdlOutputs(SimStruct *S, int_T tid)
{
int i, j, iter, error;
double measurement[ACADO_NX];
acado_timer t;
double timeFdb, timePrep;

InputRealPtrsType in_x, in_ref, ol_Data, in_W, in_WN;
real_t *out_u0, *out_kktTol, *out_xTraj, *out_objVal;

/* get inputs and perform feedback step */
in_x    = ssGetInputPortRealSignalPtrs(S, 0);
in_ref = ssGetInputPortRealSignalPtrs(S, 1);
ol_Data = ssGetInputPortRealSignalPtrs(S,2);  // 实时数据
in_W  = ssGetInputPortRealSignalPtrs(S, 3);
in_WN  = ssGetInputPortRealSignalPtrs(S, 4);


for( i=0; i < ACADO_NX; ++i ) acadoVariables.x0[i] = (double)(*in_x[i]);

// for( i=0; i < ACADO_N; ++i ) {
// for( j=0; j < ACADO_NY; ++j ) acadoVariables.y[i*ACADO_NY+j] = (double)(*in_ref[j]);
// }

for (i = 0; i < ACADO_N; ++i) {
for (j = 0; j < ACADO_NY; ++j) acadoVariables.y[i * ACADO_NY + j] = acadoVariables.y[ACADO_NY + i * ACADO_NY + j];
}

for ( i=0; i < ACADO_N; ++i ){
    for ( j=0; j < ACADO_NOD; ++j ) acadoVariables.od[i*ACADO_NOD+j] = (double)(*ol_Data[j]);
}

for( i=0; i < ACADO_NYN; ++i ) acadoVariables.y[(ACADO_N-1)*ACADO_NY+i] = acadoVariables.yN[i];
// for( i=0; i < ACADO_NYN; ++i ) acadoVariables.yN[i] = (double)(*in_ref[i]);
for( i=0; i < ACADO_NY; ++i ) {
for( j=0; j < ACADO_NY; ++j ) acadoVariables.W[i*ACADO_NY+j] = (double)(*in_W[i*ACADO_NY+j]);
}
for( i=0; i < ACADO_NYN; ++i ) {
for( j=0; j < ACADO_NYN; ++j ) acadoVariables.WN[i*ACADO_NYN+j] = (double)(*in_WN[i*ACADO_NYN+j]);
}
// acado_set( “MAX_NUM_ITERATIONS”, 500 );
acado_tic( &t );
error = acado_feedbackStep( );
timeFdb = acado_toc( &t );

acado_tic( &t );
acado_preparationStep( );
timePrep = acado_toc( &t );

printf("NMPC step (error value: %d):\n", error);
printf("Timing RTI iteration:   %.3g ms   +   %.3g ms   =   %.3g ms \n", timeFdb*1e3, timePrep*1e3, (timeFdb+timePrep)*1e3);
printf("---------------------------------------------------------------\n");

/* return outputs and prepare next iteration */
out_u0     = ssGetOutputPortRealSignal(S, 0);
out_kktTol = ssGetOutputPortRealSignal(S, 1);
out_xTraj  = ssGetOutputPortRealSignal(S, 2);
out_objVal = ssGetOutputPortRealSignal(S, 3);

for( i=0; i < ACADO_NU; ++i ) out_u0[i] = acadoVariables.u[i];
out_kktTol[0] = acado_getKKT( );
for( i = 0; i < ACADO_NX; ++i ) out_xTraj[i] = acadoVariables.x[i];
out_objVal[0] = acado_getObjective( );

}

static void mdlTerminate(SimStruct *S)
{
}

#ifdef MATLAB_MEX_FILE
#include “simulink.c”
#else
#include “cg_sfun.h”
#endif

}

4

Hi,

again, I cannot help with ACADO.
However, Simulink examples based on code generation for acados can be found here: acados/examples/acados_matlab_octave/getting_started at master · acados/acados · GitHub

Best,
Jonathan