Hi 
I’ve created a pretty simple test in Matlab that keeps failing for some reason.
I have a 2D problem (X,Y). I’d like to move to a certain position. There are constraints on the magnitude of the velocity and acceleration.
When I use the “sqp” solver, I get error 2.
When I use “sqp_rti” solver, the constraints are violated.
The model:
function model = test_model()
import casadi.*
%% system dimensions
nx = 4;
nu = 2;
%% named symbolic variables
pos = SX.sym('pos',2);
vel = SX.sym('vel',2);
acc = SX.sym('acc',2);
%% (unnamed) symbolic variables
sym_x = vertcat(pos,vel);
sym_xdot = SX.sym('xdot', nx, 1);
sym_u = acc;
%% dynamics
expr_f_expl = vertcat(vel, acc);
expr_f_impl = expr_f_expl - sym_xdot;
%% constraints
expr_h = [norm(vel), norm(acc)]';
%% cost
W_x = diag([10, 10,0,0]); % Weight is only on position, no weight on velocity
W_u = diag([1, 1]);
P_des = [0, 100]'; % Desired position
V_des = [0, 0]';
X_des = vertcat(P_des, V_des);
expr_ext_cost_e = (X_des - sym_x)'* W_x * (X_des - sym_x);
expr_ext_cost = expr_ext_cost_e + sym_u' * W_u * sym_u;
W = blkdiag(W_x, W_u);
model = acados_ocp_model();
end
The OCP:
check_acados_requirements()
%% discretization
N = 80;
T = 8;
% Initial conditions
Xi_0 = [0,0];
Vi_0 = [0.1,0.1]; % Not [0,0] so as not to begin at the lower constraint
x0 = horzcat(Xi_0, Vi_0);
nlp_solver = 'sqp_rti'; % sqp, sqp_rti
% integrator type
sim_method = 'erk'; % erk = explicit Runge Kutta, irk = implicit Runge Kutta, irk_gnsf
%% model dynamics
ocp_model = test_model;
nx = ocp_model.model_struct.nx;
nu = ocp_model.model_struct.nu;
max_velocity_xy = 15;
max_acc_xy = 3;
ocp_model.model_struct.constr_lh = [0,0]';
ocp_model.model_struct.constr_uh = [max_velocity_xy,max_acc_xy]';
ocp_model.model_struct.constr_lh_e = 0;
ocp_model.model_struct.constr_uh_e = max_velocity_xy;
ocp_model.set('constr_x0', x0);
% dynamics
ocp_model.set('dyn_type', 'explicit');
ocp_model.set('dyn_expr_f', ocp_model.model_struct.expr_f_expl);
ocp_model.model_struct.T = T;
%% acados ocp set opts
ocp_opts = acados_ocp_opts();
ocp_opts.set('sim_method', sim_method);
ocp_opts.set('param_scheme_N', N);
ocp_opts.set('nlp_solver', nlp_solver);
ocp = acados_ocp(ocp_model, ocp_opts);
x_traj_init = repmat(x0', 1,N+1);
u_traj_init = 0.1*ones(nu, N); % Not [0,0] so as not to begin at the lower constraint
% set trajectory initialization
ocp.set('init_x', x_traj_init);
ocp.set('init_u', u_traj_init);
ocp.solve();
Any insights would be very much appreciated.
Thanks in advance!
