Hi.
What are the general recommendations to avoid the solver pushing the solution slightly away from the optimal point due to the inequality constraints?
I have included a simple example with a point-mass with jerk input with state and control constraints. The offending constraint is the asymmetric one put on velocity to limit it between 0 to 10 m/s.
With a position reference equal to the initial state and the system initialized with zero velocity and acceleration the optimal state is to keep the input at 0 for the whole horizon.
This is however not the result of the solver which I suspect is due to the lower velocity constraint of 0 m/s being active? If the constraint is changed to e.g. -0.1 m/s the solution remains at the initial value.
Is there any way to relax the control barrier functions used to implement the inequality constraints?
Or what is the general recommendation in this case?
Example code for MATLAB pasted below:
clear all
import casadi.*
%% Longitudinal model
% States
x = SX.sym('x');
vel = SX.sym('vel');
accel = SX.sym('accel');
x_ = vertcat(x, vel, accel);
nx = length(x_);
% Controls
jerk = SX.sym('jerk');
u_ = vertcat(jerk);
nu = length(u_);
% Dynamics
f = vertcat(...
vel,... % dx
accel,... % dvel
jerk... % daccel
);
%% Problem parameters
ts = 0.1;
N = 100;
T = N * ts; % time horizon length
moved_lower_constraint = true; % try and toggle this
% Initial condition
x0 = [10, 0, 0]';
u0 = 0;
%% Construct problem
ocp_model = acados_ocp_model();
ocp_model.set('name', 'point_mass');
ocp_model.set('T', N * ts); % time horizon length
% Symbolics
ocp_model.set('sym_x', x_);
ocp_model.set('sym_u', u_);
% Dynamics
ocp_model.set('dyn_type', 'explicit');
ocp_model.set('dyn_expr_f', f);
% Constraints
% States
ocp_model.set('constr_Jbx', eye(nx));
if (moved_lower_constraint)
ocp_model.set('constr_lbx', [0, -0.1, -1]);
else
ocp_model.set('constr_lbx', [0, 0, -1]);
end
ocp_model.set('constr_ubx', [100, 10, 1]);
% Input
ocp_model.set('constr_Jbu', eye(nu));
ocp_model.set('constr_lbu', [-10]);
ocp_model.set('constr_ubu', [10]);
% Cost
% Define cost on all states and input
Vx = [eye(nx); zeros(nu,nx)];
Vu = [zeros(nx,nu); eye(nu)];
Vx_e = eye(nx);
% Reference value for state and input
y_ref = [x0; u0];
y_ref_e = [x0];
% Diagonal cost
W = diag([1, 0.01, 0.01, 0.1]);
W_e = diag([1, 0.01, 0.01]);
ocp_model.set('cost_type', 'linear_ls');
ocp_model.set('cost_type_e', 'linear_ls');
ocp_model.set('cost_Vx', Vx);
ocp_model.set('cost_Vu', Vu);
ocp_model.set('cost_Vx_e', Vx_e);
ocp_model.set('cost_y_ref', y_ref);
ocp_model.set('cost_y_ref_e', y_ref_e);
ocp_model.set('cost_W', W);
ocp_model.set('cost_W_e', W_e);
% Initial state
% Fix the initial state
ocp_model.set('constr_x0', x0);
%% Solver parameters
ocp_opts = acados_ocp_opts();
ocp_opts.set('compile_interface', 'auto');
ocp_opts.set('codgen_model', 'true');
ocp_opts.set('param_scheme_N', N);
ocp_opts.set('nlp_solver', 'sqp');
ocp_opts.set('qp_solver', 'partial_condensing_hpipm');
ocp_opts.set('qp_solver_cond_N', 5);
ocp_opts.set('sim_method', 'erk');
%% Construct solver and run
ocp = acados_ocp(ocp_model, ocp_opts);
% Set solver initial guess (if not, set internally using previous solution)
ocp.set('init_x', repmat(x0, [1,N+1]));
ocp.set('init_u', repmat(u0, [1,N]));
% Run the solver
ocp.solve();
x_opt = ocp.get('x')
u_opt = ocp.get('u');
Best regards
Thomas Jespersen