Struggling With Endpoint Constraints

MPC Discussions
,
1

Hello,

I am attempting to enforce some endpoint constraints that resemble a loiter without an additional heading constraint. So, it can finish on a ring of continuous x,y coordinates satisfying some (x-xL)^2 + (y-yL)^2 = r^2. This is also a min-time problem, going from a specific start position to a goal point anywhere on that ring.

I also have ‘box constraints’ enforcing a large x-y map size, utilizing lbx and ubx.

Here is the relevant part of the problem formulation that is resulting in a QP Status of 3 on the second iteration.

Relevant code snippet for model

xPos = SX.sym('xPos');
yPos = SX.sym('yPos');

xL = SX.sym('xL');
yL = SX.sym('yL');


% Ring Constraint

expr_h_e = (xPos-xL)^2 + (yPos-yL)^2;
   
% wrap model
model = AcadosModel();

model.con_h_expr_e = expr_h_e;
model.p = [xL; yL];

Remaining Relevant Problem formulation

ocp.parameter_values = [xf(1); xf(2)];
ocp.constraints.lh_e = ringRadius^2 ;
ocp.constraints.uh_e = ringRadius^2 ;

The above will converge if I make the following modification:

ocp.constraints.lh_e = ringRadius^2 - 1e-1;
ocp.constraints.uh_e = ringRadius^2 + 1e-1;

However, we really need these endpoint constraints to be as tightly satisfied as possible. Any suggestions on how we might achieve that? I am worried slack variables may not meet the ring condition as tightly as needed, but maybe that is something to try?

Here are my solver options:

% define solver options
ocp.solver_options.N_horizon = N;
ocp.solver_options.tf = Tau;
ocp.solver_options.nlp_solver_type                  = 'SQP';
ocp.solver_options.nlp_solver_max_iter              = 10^6;
ocp.solver_options.integrator_type                  = 'ERK';
ocp.solver_options.globalization                    = 'MERIT_BACKTRACKING';
ocp.solver_options.globalization_alpha_reduction    = 0.5;
ocp.solver_options.globalization_alpha_min          = 1e-3;
ocp.solver_options.regularize_method                = 'PROJECT';
ocp.solver_options.reg_epsilon                      = 1e-1;
ocp.solver_options.timeout_max_time                 = 5;
ocp.solver_options.nlp_solver_tol_stat              = 1e-4;
ocp.solver_options.nlp_solver_tol_eq                = 1e-5;
ocp.solver_options.nlp_solver_tol_ineq              = 1e-5;
ocp.solver_options.nlp_solver_tol_comp              = 1e-5;

I do also have this end-point cost, which I am hoping is not causing issues, as it is part of the min-time aspect:

ocp.cost.cost_type_e = 'LINEAR_LS';
ocp.cost.Vx_e = [0 0 0 0; 0 0 0 0; 0 0 0 0; 0 0 0 1];
w_scaled_e = ([0 0 0 1]./[aVar aVar bVar 1]).^2;
ocp.cost.W_e = diag(w_scaled_e);
ocp.cost.yref_e = [0; 0; 0; tmin];

Thanks!

2

As an update, I did try slack variables and it seems to have done the trick. Final solution very, very close to what the hard constraint would have enforced.

Appreciate any other insights and suggestions still!

3

Hi,

using soft constraints is a good idea.
Another option would be to use our new NLP solver SQP_WITH_FEASIBLE_QP which solves slacked QPs in case the nominal ones are not feasible.
Let me know how that works for you :slight_smile:

Best,
Jonathan

1 Like
4

Thank you for the reply FreyJo!

I was able to get a formulation running that seems to be working quite well with slack constraints now, even with an additional heading constraint.

We’ll also try experimenting with the solver you mention to see how that works out for us too and will report back!