Problem with nonlinear constraints

Hi guys!
I have a problem that I would like to bring to your attention. I’m trying to solve a path planning problem using the classic kinematic model.
I have imposed linear constraints on the absolute Cartesian coordinates, on the speed, on the wheel angle and on the inputs (acceleration and wheel speed).
I therefore tried to insert fictitious obstacles represented by circles (x^2+y^2=r^2) in the non-linear constraints. No problem so far.
So I tried to represent the same obstacles with an equation like: x^4 +y^4=r^4. In this case the solver returns the QP failure status immediately after the 1st iteration.
I also tried:

  • to initialize the states with a “likely maneuver”,
  • to change the prediction horizon and tolerances of the solver, but the result is the same.
    What could be the problem? Do you have any ideas on how to fix it or “get around” the problem?
clear  all
cd('D:\Acados\acados\examples\acados_matlab_octave')
acados_env_variables_windows
cd('D:\Projects\PathPlanning');
check_acados_requirements()
import casadi.*

%% system dimensions
nx = 5;
nu = 2;
ny = nu+nx; % number of outputs in lagrange term
ny_e = nx; % number of outputs in mayer term

%% discretization
N = 500;     % Prediction Horizon
Ts = .25;    % sampling time [s]
T = N*Ts;    % time horizon length
L = 2.57;    % [m] vehicle wheelbase 

%% named symbolic variables
x = SX.sym('x');         % x cartesian position  [m]
y = SX.sym('y');         % y cartesian position  [m]
psi = SX.sym('psi');     % hading angle         [rad]
v = SX.sym('v');         % vehicle speed        [m/s]
a = SX.sym('a');         % vehicle acceleration        [m/s]
delta = SX.sym('delta');        % steer wheel angle    [rad]
d_delta = SX.sym('d_delta');    % steer wheel speed    [rad/s]

%% (unnamed) symbolic variables
sym_x = vertcat(x, y, psi,v,delta);
sym_xdot = SX.sym('xdot', nx, 1);
sym_u = vertcat(a,d_delta);

%% dynamics
expr_phi = vertcat(...
            x + Ts*v*cos(psi),...
            y + Ts*v*sin(psi),...
            psi + Ts*v/L*tan(Ts*delta),...
            v + Ts*a,...
            delta + Ts*d_delta);

%% cost
cost_t   = 'ext_cost';
x0 = [10; 50; 0; 0 ; 0];
x_ref = [140;50;deg2rad(0); 0; 0];
u_ref = zeros(nu, 1);

Qf= diag([10, 10, 60, 0 , 0]);
R = diag([1e-1, 1e-2]);

yr = sym_u - u_ref;
yr_e = sym_x - x_ref;

expr_ext_cost = 0.5 * yr' * (R * yr);
expr_ext_cost_e = 0.5 * yr_e' * (Qf * yr_e);

%% constraints
Jbx_0 = eye(nx) ;
lbx_0 = x0 ;
ubx_0 = x0 ;

Jbx = [ 1 0 0 0 0; 0 1 0 0 0; 0 0 0 1 0; 0 0 0 0 1];  
lbx = [  0;   0; - 15/3.6; -deg2rad(30)] ;             
ubx = [150; 100;   15/3.6;  deg2rad(30)] ; 

Jbx_e = [0 0 0 1 0; 0 0 0 0 1];
lbx_e = [0;0];
ubx_e = [0;0];

Jbu = eye(nu);
lbu = [- 1 ; -deg2rad(30)];
ubu = [  1 ;  deg2rad(30)];

%% Circle Obstacle
h = [...
     (x-70)^2+(y-55)^2-5^2;...
     (x-70)^4+(y-50)^4-5^4;...
     (x-70)^2+(y-45)^2-5^2;
     ];
lh=[...
    0;...
    0;...
    0;...
];
uh=10000*ones(size(h,1),1);

h_e  = h;
lh_e = lh;
uh_e = uh;

%% acados ocp model
ocp_model = acados_ocp_model();
name = 'PathPlanning';
ocp_model.set('name', name);
ocp_model.set('T', T);

% === symbolics ===
ocp_model.set('sym_x', sym_x);
ocp_model.set('sym_u', sym_u);
% ocp_model.set('sym_p', sym_p);
%ocp_model.set('sym_xdot', sym_xdot);

% ===   cost   === 
ocp_model.set('cost_type',cost_t); %'linear_ls', 'ext_cost'
ocp_model.set('cost_type_e',cost_t);
ocp_model.set('cost_expr_ext_cost', expr_ext_cost );
ocp_model.set('cost_expr_ext_cost_e', expr_ext_cost_e );

% === dynamics === 
ocp_model.set('dyn_type', 'discrete');
ocp_model.set('dyn_expr_phi', expr_phi);

% === constraints === 
if exist('Jbx_0','var')
    ocp_model.set('constr_Jbx_0', Jbx_0);
    ocp_model.set('constr_lbx_0', lbx_0);
    ocp_model.set('constr_ubx_0', ubx_0);
end

if exist('Jbx','var')
    ocp_model.set('constr_Jbx', Jbx);
    ocp_model.set('constr_lbx', lbx);
    ocp_model.set('constr_ubx', ubx);
end

if exist('Jbu','var')
    ocp_model.set('constr_Jbu', Jbu);
    ocp_model.set('constr_lbu', lbu);
    ocp_model.set('constr_ubu', ubu);
end

if exist('C_m','var')
    ocp_model.set('constr_C', C_m);
    ocp_model.set('constr_D', D_m);
    ocp_model.set('constr_lg', lg);
    ocp_model.set('constr_ug', ug);
end

if exist('h','var')
    ocp_model.set('constr_expr_h', h);
    ocp_model.set('constr_lh', lh);
    ocp_model.set('constr_uh', uh);
end

if exist('Jbx_e','var')
    ocp_model.set('constr_Jbx_e', Jbx_e);
    ocp_model.set('constr_lbx_e', lbx_e);
    ocp_model.set('constr_ubx_e', ubx_e);
end

if exist('C_m_e','var')
    ocp_model.set('constr_C_e', C_m_e);
    ocp_model.set('constr_lg_e', lg_e);
    ocp_model.set('constr_ug_e', ug_e);
end

if exist('h_e','var')
    ocp_model.set('constr_expr_h_e', h);
    ocp_model.set('constr_lh_e', lh);
    ocp_model.set('constr_uh_e', uh);
end


%% acados ocp set opts
compile_interface = 'true'; %'auto';
codgen_model = 'true';

ocp_opts = acados_ocp_opts();
% ocp_opts.set('compile_interface', compile_interface);
% ocp_opts.set('codgen_model', codgen_model);
ocp_opts.set('param_scheme_N', N);      % uniform grid: number of shooting nodes (int>1)

ocp_opts.set('nlp_solver', 'sqp');         % sqp, sqp_rti
%ocp_opts.set('nlp_solver_exact_hessian', nlp_solver_exact_hessian); %'true', 'false'
ocp_opts.set('regularize_method', 'project_reduc_hess'); %'no_regularize','project','project_reduc_hess','mirror','convexify';
%ocp_opts.set('nlp_solver_ext_qp_res', nlp_solver_ext_qp_res); %0 (int)
ocp_opts.set('nlp_solver_max_iter', 500);   %100
ocp_opts.set('nlp_solver_tol_stat', 1e-2); %1e-6
ocp_opts.set('nlp_solver_tol_eq',   1e-2); %1e-6
ocp_opts.set('nlp_solver_tol_ineq', 1e-2); %1e-6
ocp_opts.set('nlp_solver_tol_comp', 1e-2); %1e-6

ocp_opts.set('qp_solver', 'partial_condensing_hpipm'); % 'full_condensing_hpipm', 'partial_condensing_hpipm', 'full_condensing_qpoases'
ocp_opts.set('qp_solver_cond_N', 5);                   % for partial condensing
%ocp_opts.set('qp_solver_cond_ric_alg', qp_solver_cond_ric_alg); % (int) 0, 1
%ocp_opts.set('qp_solver_warm_start', qp_solver_warm_start); %(int) 0 cold start, 1 warm start primal var, 2 warm start and dual var 
ocp_opts.set('qp_solver_iter_max', 50);    %50

% ocp_opts.set('sim_method', 'erk');  
% ocp_opts.set('sim_method_num_stages', 4);
% ocp_opts.set('sim_method_num_steps', 3);
ocp_opts.set('sim_method', 'discrete');   

ocp = acados_ocp(ocp_model, ocp_opts);

% initialize data structs
ocp.set('constr_lbu', [0 0],0);
ocp.set('constr_ubu', [0 0],0);
ocp.set('constr_lbu', [0 0],N-1);
ocp.set('constr_ubu', [0 0],N-1);

xi=[...
    x0(1) (x0(1):(70-x0(1))/(N/2-1):70) (70:(x_ref(1)-70)/(N/2-1):x_ref(1)) ;...
    x0(2) (x0(2):(60-x0(2))/(N/2-1):60) (60:(x_ref(2)-60)/(N/2-1):x_ref(2)) ;...
    x0(3) (x0(3):(deg2rad(25)-x0(3))/(149):deg2rad(25)) (deg2rad(25):(deg2rad(-25)-deg2rad(25))/(149):deg2rad(-25)) (deg2rad(-25):(x_ref(3)-deg2rad(-25))/(199):x_ref(3));...
    x0(4) zeros(1,N);...
    x0(5) zeros(1,N);...
   ];
ocp.set('init_x', xi);

ocp.solve();
states=ocp.get('x');
input = ocp.get('u');
sts=ocp.get('status');
iter=ocp.get('sqp_iter');

figure(1),
subplot(2,1,1)
plot(states(1,:),states(2,:),'Linewidth',1); legend('Cartesian Coord'), ylabel('m'); xlabel('m');
hold on , grid on
subplot(2,1,2)
plot([1:N+1],rad2deg(states(3,:)),'Linewidth',1);legend('Heading'), ylabel('deg');
hold on , grid on

Hi,

I briefly ran your code and inserted
ocp.print('stat')
and got

iter	res_stat	res_eq		res_ineq	res_comp	qp_stat	qp_iter
0	0.000000e+00	2.811245e-01	2.400938e+07	0.000000e+00	0	0
1	0.000000e+00	0.000000e+00	0.000000e+00	0.000000e+00	3	5

The residual for the inequalities of your initial guess is quite large with ~ 2e+7.
The first QP in the SQP algorithm is reported to be infeasible. I guess you will need an even better initialization for this problem.

Hey @gibor! Did you manage to solve your problem? I’m also trying to add an obstacle avoidance constraint using the circle constraint and I’m getting a QP failure. I first solve the problem without this constraint to get an initial path and then I use it to initialize the obstacle avoidance problem. Even when the initial trajectory doesn’t cross through the obstacle, the nonconvex problem fails. Any tips?