 # Inequality Constraint pushing the solution away from the optimal solution

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

%% 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.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', );

% 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.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

% 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

Hi Thomas,
can you quantify what “slightly away” means?
IMO it all boils down to the choice of the QP solver.
An active set solver such as qpOASES returns the “correct” solution, so in this case it should be exactly 0.
An interior point method solver such as HPIPM, as the name IPM suggests, keeps all iterates strictly inside the feasible set, and only attains optimality at the limit, so in practice up to some tolerance, depending on the solver tolerance and the problem conditioning.