Hi,
I am new to Acados and I am trying to generate the C code for my MPC for trajectory planning to a goal pose. My MPC with the Casadi works correct and my designed constraints are also satisfied while finding the path and reaching to the goal pose. I need to deploy my MPC to the hardware through S -function. I am unable to find the cause of the QP solver failure, I have followed the examples of Acados with the Matlab interface. I would appreciate any guidance.
typmodel = AcadosModel();
model.name = 'car_mpc';
% State (x)
x_pos = SX.sym('x_pos'); y_pos = SX.sym('y_pos'); theta = SX.sym('theta');
z_x = SX.sym('z_x'); z_y = SX.sym('z_y'); z_theta = SX.sym('z_theta');
x = [x_pos; y_pos; theta; z_x; z_y; z_theta];
% Control (u)
v = SX.sym('v'); delta = SX.sym('delta');
u = [v; delta];
nu = length(u);
% Parameters (p)
p = SX.sym('p', 3 + 1); % [xs_x, xs_y, xs_theta, delta_previous]
% -- Dynamics --
slip = atan((param.lr/param.L) * tan(delta));
f_expl = [ v * cos(theta + slip); ...
v * sin(theta + slip); ...
(v / param.lr) * sin(slip); ...
x_pos - p(1); ...
y_pos - p(2); ...
theta - p(3) ];
model.f_expl_expr = f_expl;
xdot = SX.sym('xdot', nx, 1);
model.f_impl_expr = xdot - f_expl;
model.xdot = xdot;
model.x = x;
model.u = u;
model.p = p;
% -- Cost Function --
state_orig = x(1:3);
state_int = x(4:6);
target_state = p(1:3);
prev_delta = p(4);
% Stage cost
error_stage = state_orig - target_state;
model.cost_expr_ext_cost = 0.5*error_stage' * param.Q * error_stage ...
+ 0.5* u' * param.R * u ...
+ 0.5* state_int' * param.Qi * state_int ...
+ 0.5* steer_delta_weight * (delta - prev_delta)^2;
% Terminal cost
error_terminal = state_orig - target_state;
model.cost_expr_ext_cost_e = 0.5* error_terminal' * param.P * error_terminal ...
+ 0.5* state_int' * param.Qi * state_int;
model.cost_expr_ext_cost_0 = model.cost_expr_ext_cost;
% -- Constraints --
% Path constraints
slope = tan(param.cone_half_ang);
d = [x_pos; y_pos] - param.cone_apex;
Rm = [cos(-param.orientation), -sin(-param.orientation);
sin(-param.orientation), cos(-param.orientation)];
dr = Rm * d;
Xr = dr(1); Yr = dr(2);
vel_term = param.eta * ( abs(v*cos(theta) - param.Vg(1)) ...
+ abs(v*sin(theta) - param.Vg(2)) ...
- param.slack ) - param.beta;
model.con_h_expr = [ Yr + slope*Xr; ...
-Yr + slope*Xr; ...
Xr - param.hyperplane; ...
vel_term - (abs(Xr) + abs(Yr)) ];
ocp_dims = AcadosOcp();
ocp_dims.model = model;
ocp_dims.dims.N = param.N;
ocp_dims.dims.nx = size(x, 1);
ocp_dims.dims.nu = size(u, 1);
ocp_dims.dims.np = size(p, 1);
ocp_dims.dims.nh = size(model.con_h_expr, 1); % Number of path constraints
ocp_dims.dims.nh_e = size(model.con_h_expr_e, 1); % Number of terminal constraints
% -- Control bounds --
ocp_dims.constraints.lbu = [param.v_min; param.omega_min];
ocp_dims.constraints.ubu = [param.v_max; param.omega_max];
ocp_dims.constraints.idxbu = (0:ocp_dims.dims.nu-1)';
ocp_dims.constraints.idxbx_0 = (0:ocp_dims.dims.nx-1)';
ocp_dims.constraints.lbx_0 = zeros(ocp_dims.dims.nx, 1);
ocp_dims.constraints.ubx_0 = zeros(ocp_dims.dims.nx, 1);
ocp_dims.constraints.lh = [-inf; -inf; -inf; -inf];
ocp_dims.constraints.uh = [0; 0; 0; 0];
ocp_dims.solver_options.qp_solver = 'FULL_CONDENSING_HPIPM';
ocp_dims.solver_options.hessian_approx = 'GAUSS_NEWTON';
ocp_dims.solver_options.integrator_type= 'IRK';
ocp_dims.solver_options.nlp_solver_type= 'SQP_RTI';
ocp_cost = AcadosOcpCost();
ocp_cost.cost_type_0 = 'EXTERNAL'; % For initial stage k=0
ocp_cost.cost_type = 'EXTERNAL'; % For k=1...N-1
ocp_cost.cost_type_e = 'EXTERNAL'; % For terminal stage k=N
ocp_dims.cost = ocp_cost;
ocp_opts = acados_ocp_opts();
ocp_dims.solver_options.tf = param.T * param.N;
acados_solver = AcadosOcpSolver(ocp_dims, struct('print_level',2, 'qp_print_level', 2));
% MPC Loop
x_c = x0_real; % Current state of the "real" system
x0_ocp = [x_c; 0; 0; 0]; % Initial state for the OCP (physical + integral states)
X_init = repmat(x0_ocp, 1, param.N+1);
U_init = zeros(nu, param.N);
% Logging variables
time_log = 0;
state_log = x_c;
control_log = [];
u_prev_delta = 0; % Initialize previous steering angle
iter = 0;
tic;
while(norm(x_c - xs, 2) > 1e-3)
x0_ocp = [x_c; 0; 0; 0]; % Reset integral states to 0 at each step
acados_solver.set('constr_x0', x0_ocp);
p_val = [xs; u_prev_delta];
for k = 0:param.N
acados_solver.set('p', p_val, k);
end
acados_solver.solve();
status = acados_solver.get('status');
if status ~= 0
warning(['Acados solver returned status ' num2str(status) ' at iteration ' num2str(iter)]);
end
% Get the optimal control input
u_ap = acados_solver.get('u', 0);
% Apply control and simulate
x_c = shift_state(x_c, u_ap, param.T, param);
% Log data and update for next iteration
iter = iter + 1;
control_log = [control_log, u_ap];
state_log = [state_log, x_c];
time_log = [time_log, time_log(end) + param.T];
u_prev_delta = u_ap(2); % Update for the next iteration's cost function
% Update initial guess for the next MPC step (shifting)
X_opt = acados_solver.get('x');
U_opt = acados_solver.get('u');
X_init = [X_opt(:, 2:end), X_opt(:, end)];
U_init = [U_opt(:, 2:end), U_opt(:, end)];
end
