MPC feasibility problem

MPC Discussions
1

Hello,
I am currently working on a crosswind awe system using acados on matlab. It is a rigid-wing aircraft which is supposed to move on a sphere in a figure of 8 trajectory. I am trying to generate these trajectories using an MPC similar to what was done in this paper:(https://www.sciencedirect.com/science/article/abs/pii/S0360544219302592).

Unfortunately, I keep getting qp solver failure no matter what I do. I tried changing the weights, hyperparameters, but it appears the problem is never able to be solved. I would like to know if it is possible to know what exactly is the problem with the solver which causes its failure.

2

Hi,

Great to hear that you are trying this.
In the paper, they are solving the problem with IPOPT, which has significantly better globalization techniques compared to acados (for now).
What do you mean with “feasibility problem”?
It guess this problem is quite nonlinear and the constraints seem nontrivial, or did you do some reformulation to avoid them?
Also, what kind of QP solver are you using and what error do you get?
Does this happen in the first SQP iteration or somewhen later?
What tolerance are you using?
And did you try to carefully initialize the solver?

Best,
Jonathan

3

Hello Mr. Frey,
Thank you for your reply.

  1. By feasibility problem I mean that the with the state and non-linear constraints described in the paper mentioned in the previous post, I almost always get an acados solver status of 4 and the consistency condition; which requires the aircraft move on a sphere of time-varying radius; described in the paper is also violated.
  2. I use a standard 6-DOF aircraft model hence the dynamics are non-linear. There are also constraints on the states and non-linear constraints bounding the angle of attack and the angle of sideslip. Regarding the consistency condition which requires the aircraft moves on the sphere, the equation representing this is c(t)= pT*p-l^2 where p is the position in the inertial frame and l is the tether line length. It is described in the paper that placing the 2nd derivative of c as an algebraic state will guarantee that c(t) and c_dot(t) are satisfied (always equal to 0) if they are initially equal to zero. Hence c_dotdot makes the problem an index1-DAE.
  3. Regarding the qp solver, I am using partial_condensing_hpipm. The error I get is an acados solver status of 4.
  4. This usually happens in the first iteration if I use the same constraints described in the paper or even when add c(t) and c_dot(t) as algebraic states.
  5. I am using a tolerance of 1e-8.
  6. I did initialize the solver with a feasible initial condition. I made sure the AOA and side-slip angles as well as all the other constraints are satisfied. Furthermore, I calculated the required line length (l) and line velocity (vl) such that c(t0)=c_dot(t0)=0.
4

Hello Mr. Frey,
Below is the code for the acados model for reference:

function model =ampyx_model_crosswind()

import casadi.*

%define number of states and number of controls

nx=15; % 1. three abs positions 2.velocities in body-fixed frame 3.Euler angles 4.angular velocities 5. ground stations

nu=4; % 1.aileron 2.elevator 3.rudder 4.acceleration of winch

%0.Tether and winch parameters

Cdt=1.2; %tether drag coefficient

pt=0.0046; %linear density

dt=0.002; %diameter of tether

%%%Define Model parameters

%1.Aircraft parameters

mass_aircraft=36.8; %mass of glider %ampyx plane%

Izx=0.47; %product of inertia (zx) %ampyx plane%

Ixx=25; %mass MOI about body-fixed x-axis %ampyx plane%

Iyy=32; %mass MOI about body-fixed y-axis %ampyx plane%

Izz=56; %mass MOI about body-fixed z-axis %ampyx plane%

g=9.81; %gravitational constant %ampyx plane%

%2. aerodynamic parameters

b=5.5; %reference wing span %ampyx plane%

c_bar=0.55; %reference chord %ampyx plane%

S=3; %reference wing area% %ampyx plane%

AR=10; %aspect ratio %ampyx plane%

%%%Name 12 states using scalar valued expression type in CasADi (SI units)

%States

px_abs=SX.sym(‘px_abs’);

py_abs=SX.sym(‘py_abs’);

pz_abs=SX.sym(‘pz_abs’); %cartesian coordinates relative to the inertial reference frame

pxb_dot=SX.sym(‘pxb_dot’);

pyb_dot=SX.sym(‘pyb_dot’);

pzb_dot=SX.sym(‘pzb_dot’);%linear velocities in the body-fixed frame

roll=SX.sym(‘roll’);

pitch=SX.sym(‘pitch’);

yaw=SX.sym(‘yaw’);%Eulerian angles ZYX rotations

wxb=SX.sym(‘wxb’);

wyb=SX.sym(‘wyb’);

wzb=SX.sym(‘wzb’);%angular velocities in the body fixed-frame

l=SX.sym(‘l’); %length of tether

vl=SX.sym(‘vl’); %velocity of tether

E=SX.sym(‘E’); %energy extracted by system

%Controls

ua=SX.sym(‘ua’); %aileron

ue=SX.sym(‘ue’); %elevator

ur=SX.sym(‘ur’); %rudder

al=SX.sym(‘al’); %acceleration of winch

%algebraic states

c=SX.sym(‘c’);%%%%%

c_dot=SX.sym(‘c_dot’);%%%%

c_dotdot=SX.sym(‘c_dotdot’); %c_dotdot

%Parameters

rho=SX.sym(‘rho’);%air density

wind_X=SX.sym(‘wind_X’);

wind_Y=SX.sym(‘wind_Y’);

wind_Z=SX.sym(‘wind_Z’); %wind components in the body-fixed frame

lambda=SX.sym(‘lambda’); %denotes tether force

sym_x=vertcat(px_abs,py_abs,pz_abs,pxb_dot,pyb_dot,pzb_dot,roll,pitch,yaw,wxb,wyb,wzb,l,vl,E);

sym_xdot=SX.sym(‘xdot’,nx,1);%Create derivative of state vector

sym_u=vertcat(ua,ue,ur,al);%control signals

sym_p=vertcat(rho,wind_X,wind_Y,wind_Z,lambda);

sym_z=vertcat(c_dotdot);

%%%State expressions

%Calculate Fa_b and Ma_b%%%%%%%%%%%%%%

%-1.wind vector change

L1_roll=[1 0 0; 0 cos(roll) sin(roll); 0 -sin(roll) cos(roll)];

L2_pitch=[cos(pitch) 0 -sin(pitch); 0 1 0; sin(pitch) 0 cos(pitch)];

L3_yaw=[cos(yaw) sin(yaw) 0; -sin(yaw) cos(yaw) 0; 0 0 1];

LBV=L1_rollL2_pitchL3_yaw;

LVB=transpose(L3_yaw)*transpose(L2_pitch)*transpose(L1_roll);

wind_bodyfixed=LBV*[wind_X;wind_Y;wind_Z];

wind_xb=wind_bodyfixed(1);

wind_yb=wind_bodyfixed(2);

wind_zb=wind_bodyfixed(3);

%0. shared constants

vr_xb=pxb_dot-wind_xb;

vr_yb=pyb_dot-wind_yb;

vr_zb=pzb_dot-wind_zb;

%vr_xb=wind_xb-pxb_dot;

%vr_yb=wind_yb-pyb_dot;

%vr_zb=wind_zb-pzb_dot;

Vt=sqrt(vr_xb^2+vr_yb^2+vr_zb^2);

side_slip=asin(vr_yb/Vt);

AOA_alpha=atan(vr_zb/vr_xb);

q_bar=0.5rhoVt^2;

p_hat=(wxbb)/(2Vt);

q_hat=(wybc_bar)/(2Vt);

r_hat=(wzbb)/(2Vt);

%%Calculate tether forces

%a. Tether tension force

Ftension_inertial=-1lambda[px_abs;py_abs;pz_abs];

Ftension_bodyfixed=LBV*Ftension_inertial;

Ftension_xb=Ftension_bodyfixed(1);

Ftension_yb=Ftension_bodyfixed(2);

Ftension_zb=Ftension_bodyfixed(3);

mag_Ftether_tension=sqrt(Ftension_xb^2+Ftension_yb^2+Ftension_zb^2);

%b. Tether weight

Fweight_inertial=[0;0;ptgl]; %l is state and it is the tether length and equal to mag_p (supposed check)

Fweight_bodyfixed=LBV*Fweight_inertial;

Fweight_xb=Fweight_bodyfixed(1);

Fweight_yb=Fweight_bodyfixed(2);

Fweight_zb=Fweight_bodyfixed(3);

%c. Tether drag

drag_const=(dtCdtq_bar*l)/4;

Fdrag_xb=-1drag_constcos(AOA_alpha);

Fdrag_yb=0;

Fdrag_zb=-1drag_constsin(AOA_alpha);

%d. calculate total tether force

Ftether_xb=Ftension_xb+Fweight_xb+Fdrag_xb;

Ftether_yb=Ftension_yb+Fweight_yb+Fdrag_yb;

Ftether_zb=Ftension_zb+Fweight_zb+Fdrag_zb;

%1. Calculate Fa_xb

Cx_alpha=8.32*AOA_alpha;

Cx_q_hat=4.412*AOA_alpha-0.603;

Cx_ue=0.112*AOA_alpha-0.011;

Cx_0=0.456;

Cx=Cx_alphaAOA_alpha+Cx_q_hatq_hat+Cx_ue*ue+Cx_0;

Fa_xb=q_barSCx;

%2. Calculate Fa_yb

Cy_beta=-0.186;

Cy_p_hat=-0.102;

Cy_r_hat=0.137AOA_alpha0.169;

Cy_ua=-0.05;

Cy_ur=0.103;

Cy=Cy_betaside_slip+Cy_p_hatp_hat+Cy_r_hatr_hat+Cy_uaua+Cy_ur*ur;

Fa_yb=q_barSCy;

%3. Calculate Fa_zb

Cz_alpha=10.203AOA_alpha^2+1.226AOA_alpha;

Cz_q_hat=6.149AOA_alpha^2+0.125AOA_alpha-7.556;

Cz_ue=0.292AOA_alpha^2-0.001AOA_alpha-0.315;

Cz_0=-5.4;

Cz=Cz_alphaAOA_alpha+Cz_q_hatq_hat+Cz_ue*ue+Cz_0;

Fa_zb=q_barSCz;

%4. Calcuate Ma_xb

Cl_beta=-0.062;

Cl_p_hat=-0.559;

Cl_r_hat=0.645*AOA_alpha+0.181;

Cl_ua=0.041*AOA_alpha-0.248;

Cl_ur=0.004;

Cl=Cl_betaside_slip+Cl_p_hatp_hat+Cl_r_hatr_hat+Cl_uaua+Cl_ur*ur;

Ma_xb=q_barSb*Cl;

%5. Calculate Ma_yb

Cm_alpha=0.205*AOA_alpha;

Cm_q_hat=5.289AOA_alpha^2-0.003AOA_alpha-11.302;

Cm_ue=-1.019;

Cm_0=-0.315;

Cm=Cm_alphaAOA_alpha+Cm_q_hatq_hat+Cm_ue*ue+Cm_0;

Ma_yb=q_barSc_bar*Cm;

%6. Calculate Ma_zb

Cn_beta=-0.085*AOA_alpha+0.058;

Cn_p_hat=-0.913*AOA_alpha-0.057;

Cn_r_hat=-0.052;

Cn_ua=-0.115*AOA_alpha+0.019;

Cn_ur=-0.041;

Cn=Cn_betaside_slip+Cn_p_hatp_hat+Cn_r_hatr_hat+Cn_uaua+Cn_ur*ur;

Ma_zb=q_barSb*Cn;

%7.calculate total forces and moments (Fa+Fw+Ft)

Fxb=Fa_xb-mass_aircraftgsin(pitch)+Ftether_xb;

Fyb=Fa_yb+mass_aircraftgsin(roll)*cos(pitch)+Ftether_yb;

Fzb=Fa_zb+mass_aircraftgcos(roll)*cos(pitch)+Ftether_zb;

Mxb=Ma_xb;

Myb=Ma_yb;

Mzb=Ma_zb;

%%Need to add aerodynamic force to thrust,weight,tether forces

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%1.First set three states expression(px_abs,py_abs,pz_abs)

LVB=transpose(L3_yaw)*transpose(L2_pitch)*transpose(L1_roll);

inertial_pos=LVB*[pxb_dot;pyb_dot;pzb_dot];

px_abs_dot=inertial_pos(1);

py_abs_dot=inertial_pos(2);

pz_abs_dot=inertial_pos(3);

%2.Second set of three states expression (pxb_dot,pyb_dot,pzb_dot)

pxb_dotdot=Fxb/mass_aircraft+wzbpyb_dot-wybpzb_dot;

pyb_dotdot=Fyb/mass_aircraft-wzbpxb_dot+wxbpzb_dot;

pzb_dotdot=Fzb/mass_aircraft+wybpxb_dot-wxbpyb_dot;

%3.Third set of three states expressions (roll,pitch,yaw)

roll_dot=wxb+(wybsin(roll)+wzbcos(pitch))*tan(pitch);

pitch_dot=wybcos(roll)-wzbsin(roll);

yaw_dot=(1/cos(pitch))(wybsin(roll)+wzb*cos(roll));

%4. Fourth set of three states expressions (wxb,wyb,wzb)

Ib=[Ixx 0 -Izx; 0 Iyy 0; -Izx 0 Izz];

wb=[wxb;wyb;wzb];

Mb=[Mxb;Myb;Mzb];

wb_dot=inv(Ib)Mb-inv(Ib)(cross(wb,Ib*wb));

wxb_dot=wb_dot(1);

wyb_dot=wb_dot(2);

wzb_dot=wb_dot(3);

%5. winch dynamics

l_dot=vl;

vl_dot=al;

E_dot=mag_Ftether_tensionvl; %wdrd=l_dot=vl

%6. consistency conditions

p_inertial=[px_abs;py_abs;pz_abs];

v_inertial=[px_abs_dot;py_abs_dot;pz_abs_dot];

v_dot_inertial=LVB*[pxb_dotdot;pyb_dotdot;pzb_dotdot];

c_expr=0.5*(transpose(p_inertial)*p_inertial-l^2);

c_dot_expr=transpose(v_inertial)p_inertial-lvl;

c_dotdot_expr=transpose(v_dot_inertial)*p_inertial+transpose(v_inertial)v_inertial-vl^2-lal; %l_dotdot=al,l_dot=vl

%state expression

stateExpression=vertcat(px_abs_dot,py_abs_dot,pz_abs_dot,pxb_dotdot,pyb_dotdot,pzb_dotdot, …

roll_dot,pitch_dot,yaw_dot,wxb_dot,wyb_dot,wzb_dot,l_dot,vl_dot,E_dot);

implicitStateExpression=[stateExpression-sym_xdot;c_dotdot-c_dotdot_expr];

%constraints on controls

constr_Jbu=eye(4);

ua_max=(20*pi)/180;

ue_max=(30*pi)/180;

ur_max=(30*pi)/180;

al_min=-2.3;

al_max=2.4;

constr_lbu=[-ua_max;-ue_max;-ur_max;al_min];

constr_ubu=[ua_max;ue_max;ur_max;al_max];

%constraints on states

constr_Jbx=[zeros(1,6),1,zeros(1,8);%roll

zeros(1,7),1,zeros(1,7);%pitch

zeros(1,9),1,zeros(1,5);%wxb

zeros(1,10),1,zeros(1,4);%wyb

zeros(1,11),1,zeros(1,3);%wzb

zeros(1,12),1,zeros(1,2);%tether length (l)

zeros(1,13),1,zeros(1,1)];%tether velocity (vl)

min_roll=(-50*pi)/180;

max_roll=(50*pi)/180;

min_pitch=(-40*pi)/180;

max_pitch=(40*pi)/180;

min_wb=(-50*pi)/180;

max_wb=(50*pi)/180;

min_l=10;

max_l=700;

min_vl=-15;

max_vl=20;

constr_lbx=[min_roll;min_pitch;min_wb;min_wb;min_wb;min_l;min_vl];

constr_ubx=[max_roll;max_pitch;max_wb;max_wb;max_wb;max_l;max_vl];

%non-linear constraints

altitude=-pz_abs;

constr_expr_h=[AOA_alpha;side_slip;Vt;altitude;mag_Ftether_tension];

min_AOA_alpha=(-6*pi)/180;

max_AOA_alpha=(9*pi)/180;

min_side_slip=(-20*pi)/180;

max_side_slip=(20*pi)/180;

min_Vt=13;

max_Vt=32;

min_altitude=100;

max_altitude=1e6;

min_mag_tether_tension=50;

max_mag_tether_tension=1800;

constr_lh=[min_AOA_alpha;min_side_slip;min_Vt;min_altitude;min_mag_tether_tension];

constr_uh=[max_AOA_alpha;max_side_slip;max_Vt;max_altitude;max_mag_tether_tension];

constr_expr_h_e=constr_expr_h;

constr_lh_e=constr_lh;

constr_uh_e=constr_uh;

%costfunction

Q_wb=diag([1e1 1e1 1e1]);

Q_sideslip=1e3;

Q_u=diag([1e4 1e4 1e4 1e1]);%1e7 for controls if use c,c_dot in DAE

cost_expr_ext_cost=-1E_dot+transpose(wb_dot)Q_wbwb_dot+Q_sideslipside_slip^2+sym_u’Q_usym_u;

cost_expr_ext_cost_e=-1E_dot+Q_sideslipside_slip^2;

%Defining model struct parameters

model.nx=nx; %%%

model.nu=nu; %%%

model.sym_x=sym_x; %%%

model.sym_xdot=sym_xdot; %%%

model.sym_u=sym_u; %%%

model.sym_p=sym_p;%%%%%%

model.sym_z=sym_z;

model.stateExpression=stateExpression; %%%

model.implicitStateExpression=implicitStateExpression; %%%

model.constr_Jbu=constr_Jbu;

model.constr_lbu=constr_lbu;

model.constr_ubu=constr_ubu;

model.constr_Jbx=constr_Jbx;

model.constr_lbx=constr_lbx;

model.constr_ubx=constr_ubx;

model.constr_expr_h=constr_expr_h;

model.constr_lh=constr_lh;

model.constr_uh=constr_uh;

model.constr_expr_h_e=constr_expr_h_e;

model.constr_lh_e=constr_lh_e;

model.constr_uh_e=constr_uh_e;

model.cost_expr_ext_cost=cost_expr_ext_cost;

model.cost_expr_ext_cost_e=cost_expr_ext_cost_e;

end

5

Hello Mr. Frey,
Also attached is the OCP folder to open the simulink model. This contains all solver details:

clear all

model_path=fullfile(pwd,‘…’,‘ampyx_model_crosswind’)

addpath(model_path)

check_acados_requirements()

%2. initial state values

px_abs_init=300;

py_abs_init=0;

pz_abs_init=-400;

pxb_dot_init=25;

pyb_dot_init=0;

pzb_dot_init=0;

roll_init=(-30*pi)/180; %initial roll angle

pitch_init=(-5*pi)/180; %initial pitch angle

yaw_init=(10*pi)/180; %initial yaw angle

omega_xb_init=0;

omega_yb_init=0;

omega_zb_init=0;

%calculated initial conditions

L1_roll_init=[1 0 0; 0 cos(roll_init) sin(roll_init); 0 -sin(roll_init) cos(roll_init)];

L2_pitch_init=[cos(pitch_init) 0 -sin(pitch_init); 0 1 0; sin(pitch_init) 0 cos(pitch_init)];

L3_yaw_init=[cos(yaw_init) sin(yaw_init) 0; -sin(yaw_init) cos(yaw_init) 0; 0 0 1];

LVB_init=transpose(L3_yaw_init)*transpose(L2_pitch_init)*transpose(L1_roll_init);

v_abs_init=LVB_init*[pxb_dot_init;pyb_dot_init;pzb_dot_init];

p_abs_init=[px_abs_init;py_abs_init;pz_abs_init];

l_init=sqrt(px_abs_init^2+py_abs_init^2+pz_abs_init^2);

vl_init=(transpose(v_abs_init)*p_abs_init)/l_init;

E_init=0;

% OCP parameters and designated solvers

N=20;

T=0.5;

Ts=T/N; %every time period a control signal is outputed for OCP and currently for MPC too (uniform time grid)

%We will consider currently that MPC outputs control input at this rate too

x0=[px_abs_init;py_abs_init;pz_abs_init;pxb_dot_init;pyb_dot_init;pzb_dot_init;

roll_init;pitch_init;yaw_init;omega_xb_init;omega_yb_init;omega_zb_init;l_init;vl_init;E_init];%Initial state is that pendulum is stationary and in equilibrium position

nlp_solver=‘sqp’;

nlp_solver_max_iter = 50;%%%

regularize_method = ‘convexify’;

tol = 1e-8;

qp_solver=‘partial_condensing_hpipm’;

qp_solver_cond_N = 5; % for partial condensing

qp_solver_iter_max = 50; % default is 50; OSQP needs a lot sometimes.%%%%%%

sim_method = ‘irk’; %%%implicit runge kutta since DAE

sim_method_num_stages = 3;

sim_method_num_steps = 3;

%model

model=ampyx_model_crosswind;

nx=model.nx;

nu=model.nu;

%Create new acados solver for cart-pole model

ocp_model=acados_ocp_model();

%Set characteristics of ocp (problem formulation)

ocp_model.set(‘name’,‘aircraft_winch’);

ocp_model.set(‘T’,T); %Refers to how much time simulate open-loop system

ocp_model.set(‘sym_x’,model.sym_x);

ocp_model.set(‘sym_u’, model.sym_u);

ocp_model.set(‘sym_xdot’, model.sym_xdot);

ocp_model.set(‘sym_p’,model.sym_p);

ocp_model.set(‘sym_z’,model.sym_z);%%%

ocp_model.set(‘cost_type’, ‘ext_cost’);%%%

ocp_model.set(‘cost_type_e’, ‘ext_cost’);%%

ocp_model.set(‘cost_expr_ext_cost’,model.cost_expr_ext_cost);%%

ocp_model.set(‘cost_expr_ext_cost_e’,model.cost_expr_ext_cost_e);%%

%using explicit RK method

ocp_model.set(‘dyn_type’, ‘implicit’);%%%

ocp_model.set(‘dyn_expr_f’, model.implicitStateExpression);%%%

%state and input constraints

ocp_model.set(‘constr_x0’, x0);%initial state must be prescribed initial state

ocp_model.set(‘constr_Jbu’,model.constr_Jbu); %next 3 lines describe constraints on control inputs

ocp_model.set(‘constr_lbu’,model.constr_lbu);

ocp_model.set(‘constr_ubu’,model.constr_ubu);

ocp_model.set(‘constr_Jbx’,model.constr_Jbx);

ocp_model.set(‘constr_lbx’,model.constr_lbx);

ocp_model.set(‘constr_ubx’,model.constr_ubx);

ocp_model.set(‘constr_expr_h’,model.constr_expr_h);

ocp_model.set(‘constr_lh’,model.constr_lh);

ocp_model.set(‘constr_uh’,model.constr_uh);

ocp_model.set(‘constr_expr_h_e’,model.constr_expr_h_e);

ocp_model.set(‘constr_lh_e’,model.constr_lh_e);

ocp_model.set(‘constr_uh_e’,model.constr_uh_e);

%Set characteristics of ocp operations

ocp_opts = acados_ocp_opts();

ocp_opts.set(‘param_scheme_N’, N); %prediction horizon in context of MPC

ocp_opts.set(‘nlp_solver’, nlp_solver);

ocp_opts.set(‘sim_method’, sim_method);

ocp_opts.set(‘sim_method_num_stages’, sim_method_num_stages);

ocp_opts.set(‘sim_method_num_steps’, sim_method_num_steps);

ocp_opts.set(‘qp_solver’, qp_solver);

ocp_opts.set(‘qp_solver_cond_N’, qp_solver_cond_N);

ocp_opts.set(‘ext_fun_compile_flags’, ‘’); % ‘-O2’

ocp_opts.set(‘qp_solver_iter_max’, qp_solver_iter_max);

ocp_opts.set(‘nlp_solver_max_iter’, nlp_solver_max_iter);

ocp_opts.set(‘nlp_solver_tol_stat’, tol);

ocp_opts.set(‘nlp_solver_tol_eq’, tol);

ocp_opts.set(‘nlp_solver_tol_ineq’, tol);

ocp_opts.set(‘nlp_solver_tol_comp’, tol);

ocp_opts.set(‘regularize_method’, regularize_method);

%Create OCP SOLVER (open-loop solver)

ocp = acados_ocp(ocp_model, ocp_opts);

x_traj_init = zeros(nx, N+1);

u_traj_init = zeros(nu, N);%as last state due to (N-1)th control input

%initialize reference trajectory

%yref=zeros(17,1);

%yref_e=zeros(12,1);

%%%%Note: with OCP you are giving reference trajectory across the full prediction horizon

%%%%Note: in MPC only the current reference as determined by tau is given.

%plotYRef=zeros(1,N+1);

%Setting reference trajectory for the OCP problem

%ocp.set(‘p’,1);

%for j=0:N-1

% ocp.set(‘cost_y_ref’,yref,j);%set reference trajectory across horizon (have reference for x and u)

%end

%ocp.set(‘cost_y_ref_e’,yref_e,N);%set terminal state reference

%ocp.solve();

%utraj = ocp.get(‘u’);

%xtraj = ocp.get(‘x’);

%status = ocp.get(‘status’); % 0 - success

%ocp.print(‘stat’)

%%%%Plots

%ts = linspace(0, T, N+1);

%figure; hold on;

%States = {‘p’, ‘theta’, ‘v’, ‘dtheta’};

%for i=1:length(States)

% subplot(length(States), 1, i);

% plot(ts, xtraj(i,:)); grid on; hold on;

% plot(ts,plotYRef);

% ylabel(States{i});

% xlabel(‘t [s]’)

%end

%Plot control input trajectory

%figure

%stairs(ts, [utraj’; utraj(end)])

%ylabel(‘F [N]’)

%xlabel(‘t [s]’)

%grid on

%%output position reference trajectory

%referenceVariables=xtraj;%get position trajectory

%tOCP=T;

%time_Vect=linspace(0,tOCP,length(referenceVariables(1,:))); %must both OCP and MPC have same T and N such consistent timing

%% get available simulink_opts with default options

simulink_opts = get_acados_simulink_opts;

%manipulate simulink_opts

%% inputs

%simulink_opts.inputs.cost_W_0 = 1;

%simulink_opts.inputs.cost_W = 1;

%simulink_opts.inputs.cost_W_e = 1;

ocp.generate_c_code(simulink_opts);

%% Compile Sfunctions

cd c_generated_code

make_sfun_sim; % integrator

make_sfun; % ocp solver

%% Copy Simulink example blocks into c_generated_code

source_folder = fullfile(pwd, ‘…’);

target_folder = pwd;

copyfile( fullfile(source_folder, ‘ampyx_simulink_MPC.slx’), target_folder );

%% Open Simulink example blocks

open_system(fullfile(target_folder, ‘ampyx_simulink_MPC’))

%%

disp(‘Press play in Simulink!’);

6

Dear Dr. Frey,
Please let me know if you require the simulink file too.
Thank you

7

Hi Mohammed,

  1. The term “[structure-preserving] feasibility problem” is also used for problems derived from an OCP where all constraints, but the dynamics, are formulated as costs, see https://arxiv.org/pdf/2403.10115
    So there was some confusion on my side.

In general, I think that these AWE OCPs are very challenging and extremely nonlinear.
Even IPOPT, which has much more advanced globalization than acados can be hard to get to converge.
I am not sure if acados is the right tool for you.

Something to try if you want to do MPC for AWE is to generate a reference trajectory with something like IPOPT and then use a tracking formulation to do MPC with acados.
This could be easier to get solved with acados, but even tracking formulations with extremely nonlinear models can be very challenging.

Lastly, you can try to use a lower tolerance, it might be easier to solve the problem less accurately.