Having trouble with nonlinear constraints

User Questions
,
1

Hi!

I’m having some trouble with nonlinear constraints.
I want to limit the magnitude of 2D velocity and acceleration ( V_x^2 + V_y^2 < V_max^2), but the SQP solver fails.
When I set constraints on each axis separably, the solver works as expected.
I created a small example in Matlab to demonstrate the problem:

import casadi.*

%% system dimensions
nx = 6;
nu = 2;

%% named symbolic variables
position_error = SX.sym('position_error', 2); % Xt - Xi
Vi = SX.sym('Vi',2);
Vt = SX.sym('Vt',2);

acc = SX.sym('acc',2);

%% (unnamed) symbolic variables
sym_x = vertcat(position_error, Vi, Vt);
sym_xdot = SX.sym('xdot', nx);
sym_u = acc;

%% dynamics
expr_f_expl = vertcat(Vt - Vi, acc, [0,0]');
expr_f_impl = expr_f_expl - sym_xdot;

%% constraints
expr_h = [Vi(1)^2 + Vi(2)^2, acc(1)^2 + acc(2)^2]';

% When this is used, the solver works as expected
% expr_h = [Vi(1), Vi(2), acc(1), acc(2)]';

expr_h_e = [Vi(1), Vi(2)]';

W = eye(2);
W_e = diag([0,0]);

% Map state to y (y consists only of position error)
Vx = zeros(2,6); 
Vx(1,1) = 1;
Vx(2,2) = 1;

Vx_e = zeros(2,6); 
Vx_e(1,1) = 1;
Vx_e(2,2) = 1;

% Map inputs to y
Vu = zeros(2,2);

model = acados_ocp_model();

%% populate structure
model.model_struct.nx = nx;
model.model_struct.nu = nu;
model.model_struct.sym_x = sym_x;
model.model_struct.sym_u = sym_u;
model.model_struct.expr_f_expl = expr_f_expl;
model.model_struct.expr_f_impl = expr_f_impl;
model.model_struct.constr_expr_h = expr_h;
model.model_struct.constr_expr_h_e = expr_h_e;

model.model_struct.cost_W= W;
model.model_struct.cost_W_e= W_e;
model.model_struct.cost_type = 'linear_ls';
model.model_struct.cost_type_e = 'linear_ls';
model.model_struct.cost_Vu = Vu;
model.model_struct.cost_Vx = Vx;
model.model_struct.cost_Vx_e = Vx_e;


%% discretization
N = 50;
T = 5;

Xt_0 = [200, 400];
Vt = [-1,-1];

Xi_0 = [0, 0];
Vi_0 = [0, 0];

x0 = horzcat(Xt_0 - Xi_0, Vi_0, Vt);

nlp_solver = 'sqp'; % sqp, sqp_rti
qp_solver_cond_N = 5; % for partial condensing

% integrator type
sim_method = 'erk'; % erk = explicit Runge Kutta, irk = implicit Runge Kutta, irk_gnsf

%% model dynamics
ocp_model = model;
nx = ocp_model.model_struct.nx;
nu = ocp_model.model_struct.nu;

max_velocity_xy = 30;
max_acc_xy = 4;

ocp_model.model_struct.constr_lh = [0, 0]';
ocp_model.model_struct.constr_uh = [max_velocity_xy^2, max_acc_xy^2]';

% When this is used, the solver works as expected
% ocp_model.model_struct.constr_lh = [-30,-30, -4, -4]';
% ocp_model.model_struct.constr_uh = [30,30, 4, 4]';

ocp_model.model_struct.constr_lh_e = [-30,-30]';
ocp_model.model_struct.constr_uh_e = [30,30]';

% update initial state
ocp_model.set('constr_x0', x0);

% dynamics
if (strcmp(sim_method, 'erk'))
    ocp_model.set('dyn_type', 'explicit');
    ocp_model.set('dyn_expr_f', ocp_model.model_struct.expr_f_expl);
else % irk irk_gnsf
    ocp_model.set('dyn_type', 'implicit');
    ocp_model.set('dyn_expr_f', ocp_model.model_struct.expr_f_impl);
end

ocp_model.model_struct.T = T;

%% acados ocp set opts
ocp_opts = acados_ocp_opts();
ocp_opts.set('param_scheme_N', N);
ocp_opts.set('nlp_solver', nlp_solver);
ocp_opts.set('sim_method', sim_method);

ocp_opts.set('qp_solver_cond_N', qp_solver_cond_N);  

ocp_opts.set('ext_fun_compile_flags', ''); % '-O2'

y_ref = [0,0]; % Bring the position error to 0
y_ref_e = [0,0];

ocp_model.set('cost_y_ref', y_ref);
ocp_model.set('cost_y_ref_e', y_ref_e);

ocp = acados_ocp(ocp_model, ocp_opts);

ocp.set('constr_lbx', x0, 0);
ocp.set('constr_ubx', x0, 0);

x_traj_init = zeros(nx, N+1);
x_traj_init(1,:) = x0(1);
x_traj_init(2,:) = x0(2);
x_traj_init(3,:) = x0(3);
x_traj_init(4,:) = x0(4);
x_traj_init(5,:) = x0(5);
x_traj_init(6,:) = x0(6);

u_traj_init = ones(nu, N);

% set trajectory initialization
ocp.set('init_x', x_traj_init);
ocp.set('init_u', u_traj_init);

ocp.solve();

switch ocp.get('status')
    case 0
        ocp.print('stat')
    case 1
        error('Failed: Detected NAN');
    case 2
        error('Failed: Maximum number of iterations reached');
    case 3
        error('Failed: Minimum step size in QP solver reached');
    case 4
        error('Failed: QP solver failed');
end

% get solution
u_result = ocp.get('u');
x_result = ocp.get('x');

Thanks in advance!

2

I am suffering from the same problem.
If a slack variable is available, changing it might alleviate the problem. But it doesn’t seem to work in matlab!

3

Hi @arisc,

I tried to run your example and also saw the same behavior. However, the two expressions are not actually equivalent.

V_x^2 + V_y^2 < V_max^2 If you visualize this, you see a ball constraint (think about x² + y² < 10)
while
V_x < V_max and V_y < V_max is a box constraint (think about -10<x<10 and -10<y<10).

Having said that, I expect different results when using these two expressions but I don’t see the reason for the ball constraint not converging. Maybe you can try a different initialization?

I can also try a little bit but I am not sure if this is the actual problem you are working on or a small example to showcase this behaviour. If it is the latter, maybe you can provide some more details for the original OCP.

I hope this helps :smile:

4

Thanks for the response!
I understand that the 2 expressions are not equivalent. I was just demonstrating that for linear constraints, it converges and for nonlinear constraints, it doesn’t.
I’m trying to limit the magnitude of a 2D vector, so I need to use the nonlinear constraint.
The example I provided is basically the OCP I’m currently working on (I was planning on expanding it once I got it to work with nonlinear constraints), so if you’re able to help with getting the example to work, that would be very helpful!