# Having trouble with simple 2D position, velocity and acceleration test

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()

%% 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);
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;
ocp_opts.set('sim_method', sim_method);
ocp_opts.set('param_scheme_N', N);
ocp_opts.set('nlp_solver', nlp_solver);
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.

Hi

please note that acados uses derivative-based methods for solving optimal control problems. The norm in your constraints, however, is not differentiable at zero. Try to use \sqrt{v_1^2 + v_2^2 + \epsilon} with \epsilon > 0 as your velocity constraint (and similar for your acceleration constraint).

Best, Katrin

Thanks for the response!
Your suggestion seems to help in certain cases, but there is still some very strange behavior.

The algorithms (both SQP and SQP_RTI) seems to be extremely sensitive to the initial guesses (init_x and init_u). For example, in the symmetrical case where the desired position is [100,100], if the initial guesses (in both x and u) aren’t symmetrical (for example initial_velocity = [1,2]) the constraints are violated.
In the case where the desired position is [10,100], the initial guesses must be the same ratio (initial_velocity = [0.1, 1] for example), or else the constraints are violated.

Any idea why this is happening?
The initial guess is just a guess and shouldn’t be causing this behavior.

Sorry, I mixed up the return codes. Your status 2 from the beginning means you ran into the maximum number of SQP iterations. So try to increase these until you get a converged solution. In general, SQP_RTI solves only a single QP and is thus not expected to satisfy nonlinear constraints.

Since your dynamics are linear, you might want to try using an exact Hessian. For your problem you would get additional positive semi-deifinite Hessian contributions from the constraints.

Thanks for the suggestions.
I tried them (increased the max iterations and set ‘nlp_solver_exact_hessian’ to true), and SQP still fails with error 2 (Maximum number of iterations reached), regardless of how high I set the max iterations.
If you have any other suggestions, I’m all ears
Thanks again!

What output do you get from ocp_solver.print('stat')?

This is the output when I set max iterations to 200:

iter	res_stat	res_eq		res_ineq	res_comp	qp_stat	qp_iter	alpha
0	2.000000e+03	1.050000e-02	1.485509e+01	0.000000e+00	0	0	0.000000e+00
1	2.048289e+03	2.842171e-14	1.942258e+02	3.646543e-08	0	51	1.000000e+00
2	1.006158e+03	2.842171e-14	2.209300e+02	3.591404e-08	0	33	1.000000e+00
3	2.971519e+03	2.842171e-14	2.793002e+02	6.337487e-08	0	35	1.000000e+00
4	8.985096e+02	4.263256e-14	2.829576e+02	4.201046e-07	0	36	1.000000e+00
5	1.113135e+03	4.263256e-14	3.294042e+02	5.779737e-09	0	32	1.000000e+00
6	3.446498e+02	2.842171e-14	3.114580e+02	1.039851e-07	0	35	1.000000e+00
7	7.358543e+02	4.263256e-14	3.280540e+02	1.518142e-07	0	32	1.000000e+00
8	4.537492e+02	2.842171e-14	3.174549e+02	1.342003e-08	0	36	1.000000e+00
9	1.070471e+03	2.842171e-14	3.155777e+02	3.104870e-07	0	32	1.000000e+00
10	5.715421e+02	4.263256e-14	3.006763e+02	3.949698e-07	0	31	1.000000e+00
11	8.877325e+02	2.842171e-14	3.207543e+02	3.152498e-08	0	32	1.000000e+00
12	4.514796e+02	2.842171e-14	2.925474e+02	7.096350e-07	0	33	1.000000e+00
13	7.325126e+02	4.263256e-14	3.231030e+02	5.897631e-08	0	31	1.000000e+00
14	3.559498e+02	2.842171e-14	2.880693e+02	9.665454e-11	0	34	1.000000e+00
15	7.160340e+02	4.263256e-14	3.222243e+02	1.189615e-07	0	34	1.000000e+00
16	2.918279e+02	2.842171e-14	2.863440e+02	7.416010e-07	0	30	1.000000e+00
17	7.075935e+02	2.842171e-14	3.209424e+02	5.895636e-07	0	33	1.000000e+00
18	3.149891e+02	4.263256e-14	2.857349e+02	7.180066e-07	0	30	1.000000e+00
19	3.042668e+02	4.263256e-14	3.200222e+02	3.178411e-07	0	28	1.000000e+00
20	4.806699e+02	4.263256e-14	2.850478e+02	7.867879e-08	0	32	1.000000e+00
21	7.418185e+02	2.842171e-14	3.208175e+02	1.633200e-07	0	35	1.000000e+00
22	5.629862e+02	4.263256e-14	2.850460e+02	5.150452e-07	0	32	1.000000e+00
23	7.899229e+02	4.263256e-14	3.213626e+02	7.433751e-09	0	31	1.000000e+00
24	6.177479e+02	4.263256e-14	2.858806e+02	1.913567e-09	0	32	1.000000e+00
25	6.437867e+02	2.842171e-14	3.211801e+02	1.630862e-08	0	34	1.000000e+00
26	6.080139e+02	4.263256e-14	2.865032e+02	1.699892e-10	0	31	1.000000e+00
27	4.727851e+02	2.842171e-14	3.206765e+02	2.078885e-11	0	34	1.000000e+00
28	5.791298e+02	2.842171e-14	2.869192e+02	3.843560e-09	0	32	1.000000e+00
29	4.305230e+02	4.263256e-14	3.200045e+02	3.900165e-07	0	31	1.000000e+00
30	5.603851e+02	4.263256e-14	2.872750e+02	6.152473e-07	0	31	1.000000e+00
31	4.138890e+02	2.842171e-14	3.197957e+02	2.835516e-07	0	30	1.000000e+00
32	5.421098e+02	4.263256e-14	2.877985e+02	2.217380e-07	0	30	1.000000e+00
33	3.982702e+02	2.842171e-14	3.195341e+02	1.083795e-08	0	31	1.000000e+00
34	5.252923e+02	2.842171e-14	2.883084e+02	1.247258e-08	0	31	1.000000e+00
35	3.856838e+02	4.263256e-14	3.190724e+02	5.363372e-08	0	32	1.000000e+00
36	5.230545e+02	4.263256e-14	2.887504e+02	5.608781e-08	0	32	1.000000e+00
37	3.830345e+02	4.263256e-14	3.186705e+02	8.039020e-07	0	32	1.000000e+00
38	5.215542e+02	2.842171e-14	2.892029e+02	5.640593e-07	0	31	1.000000e+00
39	3.829425e+02	2.842171e-14	3.181960e+02	4.452130e-07	0	31	1.000000e+00
40	5.203204e+02	4.263256e-14	2.896477e+02	2.361735e-07	0	32	1.000000e+00
41	3.833135e+02	4.263256e-14	3.176929e+02	3.601160e-07	0	31	1.000000e+00
42	5.187186e+02	2.842171e-14	2.900673e+02	9.569583e-07	0	30	1.000000e+00
43	3.838231e+02	4.263256e-14	3.171728e+02	5.620367e-09	0	30	1.000000e+00
44	5.168305e+02	2.842171e-14	2.904554e+02	3.401210e-08	0	30	1.000000e+00
45	3.843685e+02	4.263256e-14	3.166414e+02	7.350270e-08	0	31	1.000000e+00
46	5.148138e+02	4.263256e-14	2.907290e+02	8.934928e-07	0	29	1.000000e+00
47	3.848921e+02	4.263256e-14	3.161819e+02	4.633123e-08	0	33	1.000000e+00
48	5.130243e+02	2.842171e-14	2.905788e+02	4.215837e-08	0	31	1.000000e+00
49	3.852186e+02	2.842171e-14	3.161193e+02	3.826568e-10	0	33	1.000000e+00
50	5.115673e+02	2.842171e-14	2.903975e+02	5.277366e-08	0	31	1.000000e+00
51	3.851033e+02	2.842171e-14	3.160734e+02	4.633134e-07	0	29	1.000000e+00
52	5.105019e+02	2.842171e-14	2.901851e+02	2.411797e-08	0	31	1.000000e+00
53	3.850814e+02	2.842171e-14	3.160430e+02	1.474860e-09	0	30	1.000000e+00
54	5.115484e+02	2.842171e-14	2.899460e+02	8.919727e-08	0	32	1.000000e+00
55	3.870393e+02	2.842171e-14	3.160219e+02	7.368259e-07	0	30	1.000000e+00
56	5.146198e+02	2.842171e-14	2.896809e+02	7.119545e-07	0	30	1.000000e+00
57	3.931454e+02	2.842171e-14	3.159463e+02	1.979334e-07	0	32	1.000000e+00
58	5.222683e+02	2.842171e-14	2.894011e+02	3.222579e-07	0	32	1.000000e+00
59	4.469772e+02	2.842171e-14	3.158440e+02	5.890440e-07	0	30	1.000000e+00
60	5.420844e+02	2.842171e-14	2.891319e+02	7.403264e-09	0	29	1.000000e+00
61	5.896365e+02	4.263256e-14	3.157105e+02	3.380434e-07	0	31	1.000000e+00
62	5.673011e+02	4.263256e-14	2.888709e+02	1.165194e-08	0	32	1.000000e+00
63	6.461479e+02	4.263256e-14	3.155931e+02	4.409275e-11	0	33	1.000000e+00
64	5.841691e+02	4.263256e-14	2.885592e+02	1.034852e-07	0	32	1.000000e+00
65	6.374723e+02	2.842171e-14	3.154998e+02	1.583562e-08	0	38	1.000000e+00
66	5.903735e+02	4.263256e-14	2.882190e+02	7.743059e-08	0	29	1.000000e+00
67	6.388503e+02	2.842171e-14	3.154315e+02	7.723627e-08	0	37	1.000000e+00
68	5.954021e+02	4.263256e-14	2.878665e+02	4.649633e-07	0	34	1.000000e+00
69	6.397631e+02	4.263256e-14	3.154586e+02	7.329699e-11	0	35	1.000000e+00
70	5.987838e+02	4.263256e-14	2.875195e+02	2.483355e-08	0	35	1.000000e+00
71	6.404425e+02	4.263256e-14	3.155418e+02	1.177713e-10	0	37	1.000000e+00
72	6.040835e+02	2.842171e-14	2.876247e+02	1.107969e-07	0	35	1.000000e+00
73	6.430044e+02	4.263256e-14	3.158977e+02	8.883626e-07	0	37	1.000000e+00
74	6.037633e+02	2.842171e-14	2.874412e+02	1.406957e-07	0	32	1.000000e+00
75	6.452477e+02	4.263256e-14	3.160483e+02	3.622036e-07	0	38	1.000000e+00
76	6.034261e+02	4.263256e-14	2.873224e+02	2.847992e-07	0	32	1.000000e+00
77	6.475174e+02	2.842171e-14	3.161937e+02	6.249397e-12	0	37	1.000000e+00
78	6.028434e+02	2.842171e-14	2.872492e+02	2.329912e-07	0	31	1.000000e+00
79	6.493982e+02	2.842171e-14	3.163186e+02	5.890812e-08	0	34	1.000000e+00
80	6.024397e+02	4.263256e-14	2.871906e+02	8.153045e-07	0	30	1.000000e+00
81	6.511037e+02	4.263256e-14	3.164254e+02	3.595201e-08	0	33	1.000000e+00
82	6.020400e+02	2.842171e-14	2.871513e+02	4.065520e-07	0	35	1.000000e+00
83	6.526202e+02	2.842171e-14	3.165165e+02	6.783544e-07	0	33	1.000000e+00
84	6.017462e+02	4.263256e-14	2.871224e+02	7.495849e-07	0	31	1.000000e+00
85	6.540281e+02	4.263256e-14	3.165948e+02	3.147494e-11	0	34	1.000000e+00
86	6.014522e+02	4.263256e-14	2.871053e+02	1.245679e-07	0	33	1.000000e+00
87	6.553215e+02	2.842171e-14	3.166620e+02	1.684446e-08	0	34	1.000000e+00
88	6.012041e+02	4.263256e-14	2.870955e+02	6.909297e-08	0	32	1.000000e+00
89	6.565401e+02	2.842171e-14	3.167200e+02	2.884723e-08	0	37	1.000000e+00
90	6.009547e+02	4.263256e-14	2.870932e+02	6.109275e-07	0	31	1.000000e+00
91	6.576831e+02	2.842171e-14	3.167698e+02	8.736232e-09	0	36	1.000000e+00
92	6.007289e+02	2.842171e-14	2.870963e+02	2.338873e-07	0	33	1.000000e+00
93	6.587721e+02	4.263256e-14	3.168127e+02	1.454211e-08	0	36	1.000000e+00
94	6.005066e+02	4.263256e-14	2.871051e+02	8.896480e-08	0	32	1.000000e+00
95	6.598070e+02	2.842171e-14	3.168489e+02	2.442043e-07	0	38	1.000000e+00
96	6.002965e+02	2.842171e-14	2.871180e+02	1.907120e-07	0	32	1.000000e+00
97	6.608014e+02	4.263256e-14	3.168795e+02	7.756697e-07	0	38	1.000000e+00
98	6.000853e+02	2.842171e-14	2.871347e+02	2.517738e-07	0	31	1.000000e+00
99	6.617580e+02	2.842171e-14	3.169054e+02	1.342599e-08	0	38	1.000000e+00
100	5.998793e+02	2.842171e-14	2.871541e+02	2.823062e-07	0	31	1.000000e+00
101	6.626841e+02	4.263256e-14	3.169272e+02	4.999721e-07	0	39	1.000000e+00
102	5.996738e+02	4.263256e-14	2.871761e+02	3.077922e-07	0	31	1.000000e+00
103	6.635824e+02	2.842171e-14	3.169452e+02	8.441151e-07	0	37	1.000000e+00
104	5.994726e+02	4.263256e-14	2.872001e+02	3.303679e-07	0	31	1.000000e+00
105	6.644577e+02	4.263256e-14	3.169597e+02	1.663054e-08	0	36	1.000000e+00
106	5.992727e+02	4.263256e-14	2.872259e+02	3.880517e-07	0	29	1.000000e+00
107	6.653120e+02	4.263256e-14	3.169711e+02	8.058318e-07	0	36	1.000000e+00
108	5.990761e+02	4.263256e-14	2.872533e+02	1.205939e-07	0	30	1.000000e+00
109	6.661486e+02	4.263256e-14	3.169796e+02	1.618953e-09	0	38	1.000000e+00
110	5.987159e+02	4.263256e-14	2.872824e+02	9.160470e-07	0	29	1.000000e+00
111	6.669648e+02	4.263256e-14	3.169848e+02	2.554987e-08	0	35	1.000000e+00
112	5.980202e+02	2.842171e-14	2.873136e+02	8.264890e-07	0	29	1.000000e+00
113	9.965387e+02	2.842171e-14	3.169831e+02	7.551030e-10	0	37	1.000000e+00
114	5.973805e+02	2.842171e-14	2.873438e+02	8.201677e-07	0	33	1.000000e+00
115	9.965754e+02	4.263256e-14	3.169767e+02	8.876306e-10	0	36	1.000000e+00
116	5.967680e+02	2.842171e-14	2.873738e+02	2.346047e-07	0	33	1.000000e+00
117	9.964599e+02	2.842171e-14	3.169676e+02	7.218691e-10	0	38	1.000000e+00
118	5.961606e+02	2.842171e-14	2.874032e+02	2.477771e-07	0	33	1.000000e+00
119	9.962451e+02	4.263256e-14	3.169572e+02	5.061587e-10	0	36	1.000000e+00
120	5.955539e+02	2.842171e-14	2.874316e+02	7.043781e-07	0	33	1.000000e+00
121	9.959545e+02	2.842171e-14	3.169457e+02	3.819381e-07	0	41	1.000000e+00
122	5.949485e+02	2.842171e-14	2.874592e+02	6.557920e-07	0	33	1.000000e+00
123	9.956015e+02	4.263256e-14	3.169332e+02	7.882194e-07	0	37	1.000000e+00
124	5.943464e+02	2.842171e-14	2.874865e+02	1.968510e-07	0	33	1.000000e+00
125	9.951929e+02	2.842171e-14	3.169191e+02	2.456018e-07	0	38	1.000000e+00
126	5.937409e+02	2.842171e-14	2.875127e+02	2.751657e-07	0	33	1.000000e+00
127	9.947387e+02	4.263256e-14	3.169042e+02	2.187760e-07	0	38	1.000000e+00
128	5.931343e+02	4.263256e-14	2.875381e+02	2.301336e-07	0	33	1.000000e+00
129	9.942474e+02	4.263256e-14	3.168881e+02	1.882638e-09	0	39	1.000000e+00
130	5.925267e+02	4.263256e-14	2.875627e+02	5.354170e-07	0	33	1.000000e+00
131	9.937247e+02	2.842171e-14	3.168709e+02	2.315274e-09	0	37	1.000000e+00
132	5.919181e+02	2.842171e-14	2.875866e+02	5.840210e-07	0	31	1.000000e+00
133	9.931755e+02	4.263256e-14	3.168526e+02	6.899272e-08	0	40	1.000000e+00
134	5.913088e+02	4.263256e-14	2.876098e+02	4.209139e-07	0	33	1.000000e+00
135	9.926040e+02	4.263256e-14	3.168331e+02	1.655114e-08	0	37	1.000000e+00
136	5.906986e+02	4.263256e-14	2.876325e+02	5.113817e-07	0	33	1.000000e+00
137	9.920141e+02	4.263256e-14	3.168123e+02	3.132264e-08	0	38	1.000000e+00
138	5.900878e+02	4.263256e-14	2.876547e+02	6.786804e-07	0	30	1.000000e+00
139	9.914092e+02	4.263256e-14	3.167903e+02	2.779721e-07	0	38	1.000000e+00
140	5.894737e+02	4.263256e-14	2.876760e+02	2.702660e-07	0	30	1.000000e+00
141	9.907955e+02	2.842171e-14	3.167671e+02	3.145914e-07	0	38	1.000000e+00
142	5.888586e+02	4.263256e-14	2.876968e+02	4.475024e-07	0	32	1.000000e+00
143	9.901738e+02	4.263256e-14	3.167425e+02	6.389483e-08	0	37	1.000000e+00
144	5.882432e+02	4.263256e-14	2.877173e+02	6.806534e-07	0	31	1.000000e+00
145	9.895449e+02	4.263256e-14	3.167165e+02	3.914821e-07	0	37	1.000000e+00
146	5.876276e+02	4.263256e-14	2.877374e+02	1.787952e-07	0	32	1.000000e+00
147	9.889108e+02	2.842171e-14	3.166891e+02	5.848264e-07	0	37	1.000000e+00
148	5.870112e+02	4.263256e-14	2.877572e+02	3.968474e-07	0	33	1.000000e+00
149	9.882734e+02	2.842171e-14	3.166604e+02	1.942788e-07	0	37	1.000000e+00
150	5.863940e+02	4.263256e-14	2.877767e+02	8.795171e-07	0	30	1.000000e+00
151	9.876344e+02	4.263256e-14	3.166302e+02	4.300607e-07	0	37	1.000000e+00
152	5.857760e+02	4.263256e-14	2.877958e+02	6.064130e-07	0	31	1.000000e+00
153	9.869958e+02	2.842171e-14	3.165987e+02	4.209435e-07	0	37	1.000000e+00
154	5.851567e+02	2.842171e-14	2.878146e+02	2.935169e-07	0	32	1.000000e+00
155	9.863593e+02	4.263256e-14	3.165659e+02	4.143290e-07	0	37	1.000000e+00
156	5.845361e+02	2.842171e-14	2.878331e+02	6.494897e-07	0	31	1.000000e+00
157	9.857268e+02	2.842171e-14	3.165317e+02	4.450110e-07	0	37	1.000000e+00
158	5.839120e+02	2.842171e-14	2.878510e+02	7.098959e-07	0	30	1.000000e+00
159	9.850976e+02	4.263256e-14	3.164962e+02	6.132888e-07	0	37	1.000000e+00
160	5.832813e+02	2.842171e-14	2.878680e+02	4.613615e-07	0	31	1.000000e+00
161	9.844696e+02	4.263256e-14	3.164593e+02	1.276241e-08	0	41	1.000000e+00
162	5.826499e+02	4.263256e-14	2.878842e+02	7.393359e-08	0	31	1.000000e+00
163	9.838519e+02	2.842171e-14	3.164207e+02	7.845773e-07	0	38	1.000000e+00
164	5.820157e+02	2.842171e-14	2.878995e+02	8.419477e-09	0	32	1.000000e+00
165	9.832462e+02	4.263256e-14	3.163804e+02	3.792280e-07	0	39	1.000000e+00
166	5.813728e+02	4.263256e-14	2.879133e+02	2.275515e-07	0	31	1.000000e+00
167	9.826649e+02	4.263256e-14	3.163390e+02	3.891467e-07	0	36	1.000000e+00
168	5.807122e+02	2.842171e-14	2.879246e+02	2.736491e-08	0	31	1.000000e+00
169	9.821176e+02	4.263256e-14	3.162969e+02	1.575016e-07	0	44	1.000000e+00
170	5.800344e+02	2.842171e-14	2.879331e+02	2.836574e-08	0	31	1.000000e+00
171	9.815984e+02	4.263256e-14	3.162537e+02	3.150332e-08	0	40	1.000000e+00
172	5.793587e+02	4.263256e-14	2.879414e+02	3.023776e-08	0	31	1.000000e+00
173	9.810975e+02	4.263256e-14	3.162099e+02	3.672419e-08	0	38	1.000000e+00
174	5.786831e+02	4.263256e-14	2.879494e+02	9.423106e-09	0	31	1.000000e+00
175	9.806127e+02	2.842171e-14	3.161654e+02	7.378498e-09	0	41	1.000000e+00
176	5.780094e+02	4.263256e-14	2.879574e+02	2.066960e-09	0	31	1.000000e+00
177	9.801409e+02	4.263256e-14	3.161200e+02	5.389177e-07	0	38	1.000000e+00
178	5.773373e+02	4.263256e-14	2.879655e+02	2.346199e-09	0	31	1.000000e+00
179	9.796803e+02	4.263256e-14	3.160737e+02	2.876525e-07	0	38	1.000000e+00
180	5.766672e+02	2.842171e-14	2.879738e+02	2.019947e-08	0	31	1.000000e+00
181	9.792295e+02	4.263256e-14	3.160267e+02	3.951760e-08	0	40	1.000000e+00
182	5.759991e+02	2.842171e-14	2.879824e+02	2.568000e-07	0	31	1.000000e+00
183	9.787877e+02	4.263256e-14	3.159789e+02	5.880758e-07	0	38	1.000000e+00
184	5.753334e+02	2.842171e-14	2.879914e+02	1.726428e-08	0	31	1.000000e+00
185	9.783555e+02	4.263256e-14	3.159307e+02	5.810126e-08	0	39	1.000000e+00
186	5.746695e+02	2.842171e-14	2.880007e+02	7.230428e-10	0	31	1.000000e+00
187	9.779429e+02	4.263256e-14	3.158824e+02	7.443873e-08	0	41	1.000000e+00
188	5.739710e+02	2.842171e-14	2.880101e+02	2.008899e-10	0	31	1.000000e+00
189	9.775484e+02	2.842171e-14	3.158345e+02	7.877934e-07	0	45	1.000000e+00
190	5.732144e+02	4.263256e-14	2.880194e+02	5.446167e-07	0	31	1.000000e+00
191	9.771594e+02	2.842171e-14	3.157866e+02	6.950771e-08	0	41	1.000000e+00
192	5.724171e+02	2.842171e-14	2.880291e+02	2.041933e-10	0	31	1.000000e+00
193	9.767736e+02	4.263256e-14	3.157382e+02	7.637749e-07	0	36	1.000000e+00
194	5.715861e+02	4.263256e-14	2.880392e+02	5.682024e-08	0	31	1.000000e+00
195	9.763891e+02	4.263256e-14	3.156888e+02	7.142261e-08	0	37	1.000000e+00
196	5.707262e+02	4.263256e-14	2.880495e+02	6.442808e-07	0	30	1.000000e+00
197	9.760038e+02	4.263256e-14	3.156383e+02	1.079536e-07	0	35	1.000000e+00
198	5.698420e+02	4.263256e-14	2.880602e+02	3.198381e-08	0	31	1.000000e+00
199	9.756205e+02	3.552714e-14	3.155872e+02	3.768318e-07	0	34	1.000000e+00
200	0.000000e+00	0.000000e+00	0.000000e+00	0.000000e+00	0	27	1.000000e+00



Could you try again with the option ocp_opts.set('globalization', 'merit_backtracking')?

Sure. This is the output with the option ocp_opts.set(‘globalization’, ‘merit_backtracking’):

iter	res_stat	res_eq		res_ineq	res_comp	qp_stat	qp_iter	alpha
0	2.000000e+03	1.050000e-02	1.485509e+01	0.000000e+00	0	0	0.000000e+00
1	1.314000e+03	6.898500e-03	6.662163e+01	4.290121e-09	0	51	3.430000e-01
2	6.701400e+02	3.518235e-03	1.154367e+02	9.273624e+02	0	37	4.900000e-01
3	4.155793e+02	1.794300e-03	1.498543e+02	2.167082e+03	0	32	4.900000e-01
4	3.953189e+02	1.178855e-03	1.467188e+02	1.153979e+03	0	27	3.430000e-01
5	3.968010e+02	3.536565e-04	9.777220e+01	7.737487e+02	0	29	7.000000e-01
6	2.090235e+02	2.323523e-04	1.189244e+02	6.691243e+02	0	31	3.430000e-01
7	2.441063e+02	1.184997e-04	1.152800e+02	9.974109e+02	0	28	4.900000e-01
8	1.498433e+02	7.785429e-05	9.580302e+01	6.507801e+02	0	27	3.430000e-01
9	2.165917e+02	2.842171e-14	1.179383e+02	6.397259e-08	0	26	1.000000e+00
10	2.962429e+02	2.842171e-14	1.140795e+02	5.334363e+01	0	22	4.900000e-01
11	1.321148e+02	2.842171e-14	8.222134e+01	3.108645e+03	0	27	3.430000e-01
12	1.432628e+02	2.842171e-14	8.470481e+01	2.003854e+03	0	23	7.000000e-01
13	2.118914e+02	2.842171e-14	7.018951e+01	1.233586e+03	0	27	4.900000e-01
14	1.309081e+02	2.842171e-14	6.406603e+01	8.058037e+02	0	20	2.401000e-01
15	9.888491e+01	2.842171e-14	5.931464e+01	9.368326e+02	0	17	2.401000e-01
16	8.310042e+01	2.842171e-14	6.062420e+01	9.138821e+02	0	15	1.176490e-01
17	7.518573e+01	2.842171e-14	5.794168e+01	8.071618e+02	0	20	1.176490e-01
18	6.672227e+01	2.842171e-14	5.693898e+01	7.487838e+02	0	10	8.235430e-02
19	6.379436e+01	2.842171e-14	5.523238e+01	7.608452e+02	0	16	8.235430e-02
20	5.734196e+01	2.842171e-14	5.394864e+01	8.896606e+02	0	18	5.764801e-02
21	5.611767e+01	2.842171e-14	5.176213e+01	1.058230e+03	0	22	5.764801e-02
22	5.116509e+01	4.263256e-14	4.953118e+01	1.195167e+03	0	21	5.764801e-02
23	4.781202e+01	4.263256e-14	4.629842e+01	1.344831e+03	0	22	8.235430e-02
24	4.512622e+01	2.842171e-14	4.483011e+01	1.360527e+03	0	21	5.764801e-02
25	4.177365e+01	4.263256e-14	4.367799e+01	1.309553e+03	0	21	5.764801e-02
26	4.423201e+01	4.263256e-14	4.268716e+01	1.211953e+03	0	11	5.764801e-02
27	3.686486e+01	4.263256e-14	4.136980e+01	1.095671e+03	0	18	5.764801e-02
28	4.555025e+01	2.842171e-14	4.007747e+01	1.030839e+03	0	14	5.764801e-02
29	4.392671e+01	4.263256e-14	4.092754e+01	8.942197e+02	0	18	8.235430e-02
30	3.071507e+01	4.263256e-14	4.152423e+01	8.048114e+02	0	13	5.764801e-02
31	5.136280e+01	4.263256e-14	4.075704e+01	8.150206e+02	0	21	5.764801e-02
32	4.616876e+01	5.684342e-14	3.996003e+01	7.878689e+02	0	23	5.764801e-02
33	2.472570e+02	4.263256e-14	3.896323e+01	7.777949e+02	0	23	5.764801e-02
34	5.851157e+01	4.263256e-14	3.888880e+01	7.410304e+02	0	18	5.764801e-02
35	1.914251e+02	4.263256e-14	3.833386e+01	6.912311e+02	0	23	5.764801e-02
36	5.777058e+01	4.263256e-14	3.759530e+01	6.990533e+02	0	23	5.764801e-02
37	4.737549e+02	4.263256e-14	3.760739e+01	7.254997e+02	0	17	5.764801e-02
38	8.006419e+01	4.263256e-14	3.800107e+01	7.366545e+02	0	18	5.764801e-02
39	5.515529e+02	4.263256e-14	3.807788e+01	8.701724e+02	0	27	5.764801e-02
40	7.049738e+01	4.263256e-14	3.826954e+01	8.741224e+02	0	16	5.764801e-02
41	6.257834e+02	4.263256e-14	3.837568e+01	9.557043e+02	0	26	5.764801e-02
42	8.608021e+01	4.263256e-14	3.765475e+01	9.461212e+02	0	15	5.764801e-02
43	7.706437e+02	4.263256e-14	3.736153e+01	1.085349e+03	0	18	5.764801e-02
44	1.504599e+02	2.842171e-14	3.664834e+01	1.076520e+03	0	20	5.764801e-02
45	8.630861e+02	4.263256e-14	3.618079e+01	1.185357e+03	0	18	5.764801e-02
46	7.017808e+01	2.842171e-14	3.605257e+01	1.190827e+03	0	23	5.764801e-02
47	9.696450e+02	4.263256e-14	3.573421e+01	1.435051e+03	0	25	5.764801e-02
48	1.352315e+02	4.263256e-14	3.640533e+01	1.459560e+03	0	18	5.764801e-02
49	1.028525e+03	5.684342e-14	3.654954e+01	1.675745e+03	0	21	5.764801e-02
50	6.057174e+01	5.684342e-14	3.603402e+01	1.664015e+03	0	17	5.764801e-02
51	1.054295e+03	4.263256e-14	3.582943e+01	1.579350e+03	0	20	5.764801e-02
52	8.904657e+01	4.263256e-14	3.641809e+01	1.595810e+03	0	17	5.764801e-02
53	1.043267e+03	4.263256e-14	3.710645e+01	1.663647e+03	0	21	5.764801e-02
54	1.754373e+02	4.263256e-14	3.761952e+01	1.681215e+03	0	13	5.764801e-02
55	9.543031e+02	4.263256e-14	3.712556e+01	1.588766e+03	0	26	5.764801e-02
56	1.844635e+02	4.263256e-14	3.760530e+01	1.601159e+03	0	22	5.764801e-02
57	1.222702e+03	4.263256e-14	3.802405e+01	1.964134e+03	0	24	5.764801e-02
58	1.271709e+02	4.263256e-14	3.736225e+01	1.940720e+03	0	18	5.764801e-02
59	1.415440e+03	4.263256e-14	3.716143e+01	2.042207e+03	0	21	5.764801e-02
60	1.491386e+02	4.263256e-14	3.797796e+01	2.072747e+03	0	12	5.764801e-02
61	1.496515e+03	4.263256e-14	3.918312e+01	1.755590e+03	0	24	8.235430e-02
62	5.653651e+01	5.684342e-14	4.055570e+01	1.802796e+03	0	18	5.764801e-02
63	1.504488e+03	4.263256e-14	4.048057e+01	2.300468e+03	0	24	5.764801e-02
64	2.331718e+02	4.263256e-14	3.974158e+01	2.271983e+03	0	23	5.764801e-02
65	1.674220e+03	4.263256e-14	3.934667e+01	2.403762e+03	0	21	5.764801e-02
66	9.311373e+01	4.263256e-14	3.890717e+01	2.351743e+03	0	25	8.235430e-02
67	1.381149e+03	4.263256e-14	3.897634e+01	2.205313e+03	0	24	5.764801e-02
68	6.657175e+01	4.263256e-14	3.893038e+01	2.203943e+03	0	20	5.764801e-02
69	1.342286e+03	4.263256e-14	3.901349e+01	2.110950e+03	0	23	5.764801e-02
70	6.418927e+01	4.263256e-14	3.943565e+01	2.110758e+03	0	15	5.764801e-02
71	1.669772e+03	4.263256e-14	4.003962e+01	2.596477e+03	0	20	5.764801e-02
72	3.725560e+02	4.263256e-14	4.166159e+01	2.435926e+03	0	24	1.176490e-01
73	1.784541e+03	4.263256e-14	4.374664e+01	1.963292e+03	0	25	1.176490e-01
74	3.654039e+02	5.684342e-14	4.220164e+01	1.987852e+03	0	24	5.764801e-02
75	1.411556e+03	5.684342e-14	3.981360e+01	1.859129e+03	0	21	5.764801e-02
76	2.618602e+02	4.263256e-14	3.801420e+01	1.854504e+03	0	20	5.764801e-02
77	1.234608e+03	4.263256e-14	3.749315e+01	1.676223e+03	0	21	5.764801e-02
78	8.729014e+01	4.263256e-14	3.843072e+01	1.708380e+03	0	15	5.764801e-02
79	1.228069e+03	4.263256e-14	3.853662e+01	1.952112e+03	0	20	5.764801e-02
80	6.944886e+01	4.263256e-14	3.850932e+01	1.951440e+03	0	15	5.764801e-02
81	1.114883e+03	5.684342e-14	3.854240e+01	1.747250e+03	0	24	5.764801e-02
82	1.425871e+02	5.684342e-14	3.906747e+01	1.761595e+03	0	20	5.764801e-02
83	1.506969e+03	4.263256e-14	3.936961e+01	2.375016e+03	0	20	5.764801e-02
84	1.279369e+02	4.263256e-14	3.885003e+01	2.352396e+03	0	22	5.764801e-02
85	1.569509e+03	2.842171e-14	3.869215e+01	2.275580e+03	0	22	5.764801e-02
86	2.056702e+02	2.842171e-14	3.824719e+01	2.260959e+03	0	19	5.764801e-02
87	1.480519e+03	2.842171e-14	3.795697e+01	2.074349e+03	0	24	5.764801e-02
88	6.429660e+01	4.263256e-14	3.859267e+01	2.091405e+03	0	13	5.764801e-02
89	1.264739e+03	4.263256e-14	3.951142e+01	1.915324e+03	0	24	5.764801e-02
90	1.275750e+02	4.263256e-14	4.014079e+01	1.932524e+03	0	16	5.764801e-02
91	1.448640e+03	4.263256e-14	3.987490e+01	2.261719e+03	0	23	5.764801e-02
92	3.397793e+02	4.263256e-14	3.838417e+01	2.216418e+03	0	15	5.764801e-02
93	1.375565e+03	5.684342e-14	3.740471e+01	1.979066e+03	0	21	5.764801e-02
94	1.331500e+02	4.263256e-14	3.823498e+01	2.006396e+03	0	12	5.764801e-02
95	1.486456e+03	4.263256e-14	3.966691e+01	1.698596e+03	0	25	8.235430e-02
96	9.989177e+01	5.684342e-14	4.085403e+01	1.738607e+03	0	22	5.764801e-02
97	1.340155e+03	5.684342e-14	4.069979e+01	1.969403e+03	0	19	5.764801e-02
98	6.929014e+01	5.684342e-14	4.101867e+01	1.978135e+03	0	17	5.764801e-02
99	1.253104e+03	7.105427e-14	4.110369e+01	1.929437e+03	0	21	5.764801e-02
100	9.188873e+01	4.263256e-14	4.151838e+01	1.938179e+03	0	20	5.764801e-02
101	1.083774e+03	5.684342e-14	4.178590e+01	1.722115e+03	0	22	5.764801e-02
102	2.215800e+02	5.684342e-14	4.072874e+01	1.689108e+03	0	21	5.764801e-02
103	1.146557e+03	4.263256e-14	4.028413e+01	1.568012e+03	0	19	5.764801e-02
104	1.057282e+02	4.263256e-14	4.151294e+01	1.598194e+03	0	13	5.764801e-02
105	1.261722e+03	4.263256e-14	4.362084e+01	1.368062e+03	0	26	8.235430e-02
106	2.155409e+02	5.684342e-14	4.436208e+01	1.391665e+03	0	19	5.764801e-02
107	1.077634e+03	5.684342e-14	4.377094e+01	1.507006e+03	0	18	5.764801e-02
108	1.623161e+02	4.263256e-14	4.333025e+01	1.499218e+03	0	18	5.764801e-02
109	8.438770e+02	4.263256e-14	4.234861e+01	1.339740e+03	0	23	5.764801e-02
110	5.135204e+01	5.684342e-14	4.193542e+01	1.330777e+03	0	15	5.764801e-02
111	7.606898e+02	5.684342e-14	4.160000e+01	1.183367e+03	0	19	5.764801e-02
112	7.807118e+01	4.263256e-14	4.197468e+01	1.189962e+03	0	22	5.764801e-02
113	6.335050e+02	5.684342e-14	4.202558e+01	1.057733e+03	0	22	5.764801e-02
114	1.080712e+02	5.684342e-14	4.092750e+01	1.037502e+03	0	18	5.764801e-02
115	7.892952e+02	5.684342e-14	4.046491e+01	1.145141e+03	0	15	5.764801e-02
116	5.923025e+01	5.684342e-14	4.070740e+01	1.152678e+03	0	17	5.764801e-02
117	1.012469e+03	4.263256e-14	4.055801e+01	1.593617e+03	0	22	5.764801e-02
118	4.867247e+01	4.263256e-14	4.014425e+01	1.584799e+03	0	19	5.764801e-02
119	9.870017e+02	4.263256e-14	3.987107e+01	1.504047e+03	0	24	5.764801e-02
120	7.226640e+01	5.684342e-14	3.945910e+01	1.496967e+03	0	14	5.764801e-02
121	7.972430e+02	4.263256e-14	3.870224e+01	1.337284e+03	0	23	5.764801e-02
122	7.514941e+01	5.684342e-14	3.857531e+01	1.335330e+03	0	21	5.764801e-02
123	7.704883e+02	5.684342e-14	3.796749e+01	1.146041e+03	0	23	8.235430e-02
124	6.585426e+01	5.684342e-14	3.844128e+01	1.155029e+03	0	22	5.764801e-02
125	9.013852e+02	4.263256e-14	3.905148e+01	1.427405e+03	0	19	5.764801e-02
126	5.610042e+01	4.263256e-14	3.914081e+01	1.423604e+03	0	22	5.764801e-02
127	8.888729e+02	5.684342e-14	3.797373e+01	1.372861e+03	0	22	5.764801e-02
128	1.825212e+02	4.263256e-14	3.884946e+01	1.395918e+03	0	20	5.764801e-02
129	1.028096e+03	4.263256e-14	4.101528e+01	1.176361e+03	0	22	8.235430e-02
130	5.349391e+01	4.263256e-14	4.263476e+01	1.212703e+03	0	20	5.764801e-02
131	1.044241e+03	4.263256e-14	4.242430e+01	1.640576e+03	0	24	5.764801e-02
132	6.166127e+01	4.263256e-14	4.240680e+01	1.639873e+03	0	19	5.764801e-02
133	1.185687e+03	4.263256e-14	4.229283e+01	1.889552e+03	0	21	5.764801e-02
134	9.288294e+01	5.684342e-14	4.171279e+01	1.872026e+03	0	22	5.764801e-02
135	1.437013e+03	4.263256e-14	4.137517e+01	2.197534e+03	0	23	5.764801e-02
136	1.182071e+02	4.263256e-14	4.091743e+01	2.184513e+03	0	18	5.764801e-02
137	1.484717e+03	5.684342e-14	4.053456e+01	2.252613e+03	0	23	5.764801e-02
138	5.588017e+01	4.263256e-14	4.053436e+01	2.257363e+03	0	23	5.764801e-02
139	1.405211e+03	4.263256e-14	4.032869e+01	2.232918e+03	0	24	5.764801e-02
140	9.124627e+01	4.263256e-14	3.980327e+01	2.216658e+03	0	24	5.764801e-02
141	1.246050e+03	5.684342e-14	4.136556e+01	1.972819e+03	0	23	5.764801e-02
142	6.843962e+01	4.263256e-14	4.262590e+01	1.960864e+03	0	21	5.764801e-02
143	1.179401e+03	7.105427e-14	4.367259e+01	1.684183e+03	0	23	8.235430e-02
144	9.188845e+01	5.684342e-14	4.420590e+01	1.654126e+03	0	24	8.235430e-02
145	9.739749e+02	4.263256e-14	4.366322e+01	1.594957e+03	0	22	5.764801e-02
146	6.654970e+01	7.105427e-14	4.240548e+01	1.595650e+03	0	21	5.764801e-02
147	1.152957e+03	4.263256e-14	4.076465e+01	1.349922e+03	0	21	8.235430e-02
148	7.154047e+01	4.263256e-14	4.067600e+01	1.401467e+03	0	22	5.764801e-02
149	1.061218e+03	4.263256e-14	4.016526e+01	1.720605e+03	0	24	5.764801e-02
150	6.592937e+01	4.263256e-14	3.996353e+01	1.718714e+03	0	21	5.764801e-02
151	9.744888e+02	4.263256e-14	3.993151e+01	1.565069e+03	0	25	5.764801e-02
152	5.505634e+01	2.842171e-14	3.964454e+01	1.558180e+03	0	21	5.764801e-02
153	8.629986e+02	4.263256e-14	3.935388e+01	1.385915e+03	0	22	5.764801e-02
154	9.843502e+01	4.263256e-14	3.877816e+01	1.369233e+03	0	23	5.764801e-02
155	7.295578e+02	5.684342e-14	3.836741e+01	1.224957e+03	0	23	5.764801e-02
156	6.802824e+01	5.684342e-14	3.855197e+01	1.208552e+03	0	20	5.764801e-02
157	8.440467e+02	4.263256e-14	3.926041e+01	1.030147e+03	0	24	8.235430e-02
158	8.601732e+01	5.684342e-14	3.975779e+01	1.044326e+03	0	24	5.764801e-02
159	8.206388e+02	4.263256e-14	3.895740e+01	1.213136e+03	0	21	5.764801e-02
160	5.543424e+01	4.263256e-14	3.808137e+01	1.219814e+03	0	23	5.764801e-02
161	9.548844e+02	4.263256e-14	3.760935e+01	1.505167e+03	0	22	5.764801e-02
162	1.640225e+02	4.263256e-14	3.739212e+01	1.483763e+03	0	22	5.764801e-02
163	9.704149e+02	4.263256e-14	3.769861e+01	1.372463e+03	0	21	5.764801e-02
164	1.465397e+02	5.684342e-14	3.742256e+01	1.366727e+03	0	26	5.764801e-02
165	1.005209e+03	5.684342e-14	3.691735e+01	1.454397e+03	0	22	5.764801e-02
166	6.390736e+01	5.684342e-14	3.825541e+01	1.472884e+03	0	22	5.764801e-02
167	1.121833e+03	5.684342e-14	4.005375e+01	1.813557e+03	0	25	5.764801e-02
168	1.138291e+02	4.263256e-14	4.222449e+01	1.822298e+03	0	20	5.764801e-02
169	1.044626e+03	5.684342e-14	4.216025e+01	1.733889e+03	0	25	5.764801e-02
170	2.855233e+02	4.263256e-14	4.093091e+01	1.760445e+03	0	21	5.764801e-02
171	8.781802e+02	5.684342e-14	3.999022e+01	1.598472e+03	0	25	5.764801e-02
172	6.809291e+01	4.263256e-14	3.882742e+01	1.585834e+03	0	20	5.764801e-02
173	1.027989e+03	4.263256e-14	3.744114e+01	1.667153e+03	0	24	5.764801e-02
174	3.343858e+02	4.263256e-14	3.692965e+01	1.703374e+03	0	22	5.764801e-02
175	9.826477e+02	4.263256e-14	3.768338e+01	1.752588e+03	0	27	5.764801e-02
176	1.550184e+02	5.684342e-14	3.660707e+01	1.713796e+03	0	17	5.764801e-02
177	1.068900e+03	4.263256e-14	3.624883e+01	1.524869e+03	0	21	5.764801e-02
178	1.580717e+02	4.263256e-14	3.742545e+01	1.556296e+03	0	21	5.764801e-02
179	9.203771e+02	5.684342e-14	3.779656e+01	1.536489e+03	0	26	5.764801e-02
180	1.316925e+02	4.263256e-14	3.812629e+01	1.541091e+03	0	23	5.764801e-02
181	1.059604e+03	4.263256e-14	3.847198e+01	1.713822e+03	0	26	5.764801e-02
182	8.538872e+01	4.263256e-14	3.866761e+01	1.715057e+03	0	15	5.764801e-02
183	9.686156e+02	5.684342e-14	3.892310e+01	1.544811e+03	0	28	5.764801e-02
184	8.385603e+01	4.263256e-14	3.839362e+01	1.528331e+03	0	15	5.764801e-02
185	1.048720e+03	4.263256e-14	3.821648e+01	1.543386e+03	0	25	5.764801e-02
186	1.259430e+02	4.263256e-14	3.912920e+01	1.564454e+03	0	17	5.764801e-02
187	1.193117e+03	4.263256e-14	3.951475e+01	1.893147e+03	0	28	5.764801e-02
188	6.508500e+01	4.263256e-14	3.920640e+01	1.879208e+03	0	14	5.764801e-02
189	1.098537e+03	4.263256e-14	3.906486e+01	1.671144e+03	0	25	5.764801e-02
190	1.632268e+02	2.842171e-14	3.842135e+01	1.650940e+03	0	23	5.764801e-02
191	1.143442e+03	4.263256e-14	3.814664e+01	1.604044e+03	0	23	5.764801e-02
192	1.588408e+02	4.263256e-14	3.692633e+01	1.544482e+03	0	24	8.235430e-02
193	9.074663e+02	4.263256e-14	3.752510e+01	1.379117e+03	0	22	5.764801e-02
194	9.607845e+01	4.263256e-14	3.763739e+01	1.391648e+03	0	23	5.764801e-02
195	9.695441e+02	4.263256e-14	3.821296e+01	1.182694e+03	0	25	8.235430e-02
196	6.430119e+01	5.684342e-14	3.894903e+01	1.209614e+03	0	25	5.764801e-02
197	1.184862e+03	5.684342e-14	3.877971e+01	1.797317e+03	0	24	5.764801e-02
198	7.835751e+01	5.684342e-14	3.911845e+01	1.804979e+03	0	16	5.764801e-02
199	9.970206e+02	4.263256e-14	3.919276e+01	1.604985e+03	0	28	5.764801e-02
200	0.000000e+00	0.000000e+00	0.000000e+00	0.000000e+00	0	22	5.764801e-02


Setting 'levenberg_marquardt' to extremely high values, seems to help.
When I set it to 10000, I get a valid solution. However, the convergence is very slow (takes over 30,000 iterations).
Any idea why this is happening?

Hi, could you share your current implementation? that would be very helpful for debugging

Sure

The model:



function model = test_model()

%% 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 = [vel(1)*vel(1)+vel(2)*vel(2) , acc(1)*acc(1)+acc(2)*acc(2)]'; % Magnitude of vel and acc squared

%% cost
W_x = diag([100, 100,0,0]); % Weight is only on position, no weight on velocity
W_u = diag([1, 1]);

P_des = [100, -50]'; % Desired Position
V_des = [0, 0]'; % Desired velocity, but no weight is applied to it

X_des = vertcat(P_des, V_des);

expr_ext_cost_e = (X_des - sym_x(1:4))'* W_x * (X_des - sym_x(1:4));
expr_ext_cost = expr_ext_cost_e + 0.5 * sym_u' * W_u * sym_u;

W = blkdiag(W_x, W_u);

%% populate structure
model.model_struct.name = 'Test';
model.model_struct.nx = nx;
model.model_struct.nu = nu;
model.model_struct.sym_x = sym_x;
model.model_struct.sym_xdot = sym_xdot;
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_0 = expr_h;
% model.model_struct.constr_expr_h_e = vel(1)*vel(1)+vel(2)*vel(2);
model.model_struct.cost_expr_ext_cost_0 = expr_ext_cost;

model.model_struct.constr_type = 'auto';

model.model_struct.cost_expr_ext_cost = expr_ext_cost;
model.model_struct.cost_expr_ext_cost_e = expr_ext_cost_e;
model.model_struct.cost_W= W;
model.model_struct.cost_W_e= W_x;
end


The solver:

clc;
clear;
close all;

%% discretization
N = 40;
T = 4;

% Initial conditions
Xi_0 = [-10,0];
Vi_0 = [3,4];
x0 = horzcat(Xi_0, Vi_0);
u0 = [0,0];

nlp_solver = 'sqp'; % 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_squared_xy = 100;
max_acc_squared_xy = 9;

ocp_model.model_struct.constr_lh = [-1,-1]'; % Not 0 because then 0 initial conditions are problematic

ocp_model.model_struct.constr_uh = [max_velocity_squared_xy,max_acc_squared_xy]';

ocp_model.model_struct.constr_lh_0 = [-1,-1]'; % Not 0 because then 0 initial conditions are problematic

ocp_model.model_struct.constr_uh_0 = [max_velocity_squared_xy,max_acc_squared_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;

ocp_opts.set('sim_method', sim_method);
ocp_opts.set('param_scheme_N', N);
ocp_opts.set('nlp_solver', nlp_solver);
ocp_opts.set('nlp_solver_max_iter', 30000);
ocp_opts.set('globalization', 'merit_backtracking')

ocp_opts.set('levenberg_marquardt', 10000);

x_traj_init = repmat(x0', 1,N+1);
u_traj_init = repmat(u0', 1,N);

%% call ocp solver

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

Xi(:,1) = Xi_0;
x(:,1) = x0';

ocp.solve();
ocp.print('stat')

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');

acc_norm= zeros(1,size(u_result,2));

position(:,1) = Xi_0;
Vi(:,1) = Vi_0;
Vi_norm(:,1) = norm(Vi_0);

for j = 1:1:size(u_result,2)

acc_norm(j) = norm(u_result(:,j));
position(1:2,j) = x_result(1:2,j);
Vi(1:2,j) = x_result(3:4,j);
Vi_norm(j) = norm(Vi(:,j));

end

%% Plots
figure;
plot(position(1,:),position(2,:));
title('Position XY');
legend('Inteceptor Position');
grid on;
xlabel('X [m]');
ylabel('Y [m]');

figure;
dt = T/N;
time = dt:dt:T;
plot(time, acc_norm);
title('Acceleration XY Magnitude');
ylabel('Acceleration XY [m/sec^2]')
xlabel('Time [sec]')
grid on;

figure;
plot(time, Vi_norm);
title('Velocity XY Magnitude');
ylabel('Velocity XY [m/sec]')
xlabel('Time [sec]')
grid on;