# Reference trajectory definition

Hi, I have recently started using `acados` through the Python interface.
I run the `minimal_example_closed_loop.py` from the `getting_started` folder.
In this example, the initial condition is [0, np.pi, 0, 0] (pendulum downward), and the reference is specified in the `setup` function as follows

``````ocp = AcadosOcp()
...
ocp.cost.yref  = np.zeros((ny, )) # pendulum up
ocp.cost.yref_e = np.zeros((ny_e, ))
...
``````

so it is constant for the entire duration of the simulation and it enters at t=0.

I would like to specify an arbitrary reference trajectory for the state variables, for example I want a step on the angle theta at t=1 s and not at t=0s. A step reference is unfeasible by all continuous time dynamical systems, and thatâ€™s why I would like to try to impose it with an MPC which is able to â€śfilterâ€ť the discontinuous signal and produce a state evolution compatible with the dynamics.
In the `race_cars` example I see that the reference is set at every simulation iteration for all the shooting steps using the functions

``````acados_solver.set(j, "yref", yref)
acados_solver.set(N, "yref", yref_N)
``````

Now, to impose the step after 1s I first define the reference as follows

``````  # Reference definition
ref_signal = np.zeros((Nsim+N_horizon, nx+nu)) # initialization
ref_signal[:round(Nsim/2), 1]  =  np.pi
``````

where `N_sim` is the total number of discrete steps of the simulation and `N_horizon` is the number of shooting steps (which discretizes a time interval of `Tf` seconds).
Then, in the main loop of the simulation, I update `yref` assigning the corresponding slice of the reference signal (note that I use the `cost_set` function which should be equivalent to `set`):

``````for i in range(Nsim):
...
# Reference assignment to the prediction horizon
for kk in range(N_horizon):
yref = ref_signal[i+kk,:]
ocp_solver.cost_set(kk, 'yref', yref)
yref_N = ref_signal[i+kk,0:nx] # consider only 4 elements (no output)
ocp_solver.cost_set(N_horizon, 'yref', yref_N)
...
``````

I think the same procedure could be repeated to track an arbitrary time-varying signal.

The problem is that after some iterations (when the step enters in the prediction horizon) I receive `Exception: acados acados_ocp_solver returned status 2`
which should mean that the solver doesnâ€™t converge.

The cost type is `NONLINEAR_LS` and the cost weights are `Q_mat = 2*np.diag([1e2, 1e2, 1e-2, 1e-2])` and `R_mat = 2*np.diag([1e-2])`.

Is this the correct way to define a time-varying reference trajectory?
If so, how can I solve the error?

Thanks,
Lorenzo

Hi Lorenzo,

it all seems correct what you are doing.
Of course such a jump can make it harder for the solver to converge.
Status 2 mean that the maximum number of SQP iterations was reached.
It makes sense to check the statistics/ residuals.
You could just not raise an exception in that case also.

Best,
Jonathan

Hi Jonathan, thanks!

Here is (part of) the output:

``````ocp_nlp_sqp: maximum iterations reached

iter    res_stat        res_eq          res_ineq   res_comp qp_stat qp_iter alpha
0       6.283185e+02    3.976771e-17    2.976978e-148.000000e-15    0       0       0.000000e+00
1       1.235761e+01    1.500638e+00    2.842171e-141.822583e-09    0       6       1.000000e+00
2       1.497361e+01    1.299151e+00    2.842171e-141.832560e-09    0       11      1.000000e+00
3       2.223578e+01    2.115129e+00    3.008466e-146.055218e-11    0       10      1.000000e+00
4       3.554426e+00    2.045253e+00    2.842171e-142.586131e-09    0       13      1.000000e+00
5       2.341476e+01    8.203553e-01    2.842171e-142.429075e-09    0       6       1.000000e+00
6       6.657227e+00    1.659157e+00    2.842171e-143.133359e-12    0       12      1.000000e+00
7       2.622488e+01    1.325107e+00    2.842171e-142.218879e-08    0       9       1.000000e+00
8       3.465029e+00    1.642193e+00    1.421085e-144.534030e-08    0       14      1.000000e+00
9       1.964541e+01    8.465555e-01    2.842171e-141.638693e-08    0       5       1.000000e+00
10      6.109570e+00    1.649927e+00    2.842171e-142.990791e-08    0       9       1.000000e+00
...
90      5.823709e+00    1.675855e+00    2.842171e-142.688726e-09    0       11      1.000000e+00
91      2.271515e+01    1.252136e+00    2.842171e-143.568049e-10    0       9       1.000000e+00
92      3.725255e+00    1.624788e+00    2.842171e-142.277584e-07    0       8       1.000000e+00
93      1.845568e+01    8.531988e-01    2.842171e-141.600000e-14    0       6       1.000000e+00
94      5.823709e+00    1.675855e+00    2.842171e-142.688726e-09    0       11      1.000000e+00
95      2.271515e+01    1.252136e+00    2.842171e-143.568049e-10    0       9       1.000000e+00
96      3.725255e+00    1.624788e+00    2.842171e-142.277584e-07    0       8       1.000000e+00
97      1.845568e+01    8.531988e-01    2.842171e-141.600000e-14    0       6       1.000000e+00
98      5.823709e+00    1.675855e+00    2.842171e-142.688726e-09    0       11      1.000000e+00
99      2.271515e+01    1.252136e+00    2.842171e-143.568049e-10    0       9       1.000000e+00
100     0.000000e+00    0.000000e+00    0.000000e+000.000000e+00    0       8       1.000000e+00
``````

Iâ€™ve noticed that the iteration in which solver doesnâ€™t converge is exactly the first time the step enters in the prediction horizon.
Just in case, how can I disable the exception?

Thanks,
Lorenzo

Hello, I have made other trials and I have some updates.

Setting a prediction horizon of 0.4s with 40 steps and a step reference signal after 1s works with the approach above, with and without RTI.

Setting a prediction horizon of 0.8s with 80 time steps produces error 2 without RTI and works with RTI (because only one SQP iteration in performed). I tried to disable the exception in case of error and the control works without RTI, even if the plots are not good when the reference signal enters in the prediction horizon.
I also tried to substitute the step reference with a smoother approximation (cosine interpolation) but the situation is the same.
Any ideas to improve? Does the prediction horizon influence so much the convergence of the problem?

Lorenzo

I have a similar issue while running my ocp but there are some weird results I do not understand. These are the stats for the first iteration of the OCP.
I am using the python interface of ACADOS.

``````iter	res_stat	res_eq		res_ineq	res_comp	qp_stat	qp_iter	alpha
0	1.750749e+00	0.000000e+00	1.000000e+15	0.000000e+00	0	0	0.000000e+00
1	1.302601e+00	3.299512e-01	1.250000e-01	6.326065e-01	2	50	1.000000e+00
2	4.623300e-02	2.531534e-01	1.250000e-01	6.273171e-01	2	50	1.000000e+00
3	2.504175e-02	5.066867e-01	1.250000e-01	6.706056e-01	2	50	1.000000e+00
4	3.185088e-02	9.797397e-02	1.250000e-01	6.563744e-01	2	50	1.000000e+00
5	1.657294e-02	3.565863e-02	1.250000e-01	2.895376e-01	2	50	1.000000e+00
6	1.150030e-01	3.565327e-01	1.250000e-01	6.616907e-01	2	50	1.000000e+00
7	9.590410e-03	1.483987e-01	1.250000e-01	1.000000e-01	2	50	1.000000e+00
8	6.599057e-03	8.020798e-02	1.250000e-01	1.000000e-01	2	50	1.000000e+00
9	2.487954e-03	1.665559e-02	8.881784e-16	1.000000e-01	2	50	1.000000e+00
10	1.339956e-03	3.011325e-03	1.250000e-01	1.000000e-01	2	50	1.000000e+00
11	6.438184e-04	7.121784e-04	1.250000e-01	1.000000e-01	2	50	1.000000e+00
12	3.129557e-04	1.671614e-04	4.440892e-16	1.000000e-01	2	50	1.000000e+00
13	1.519880e-04	3.956805e-05	8.881784e-16	1.000000e-01	2	50	1.000000e+00
14	7.489485e-05	9.580210e-06	8.881784e-16	1.000000e-01	2	50	1.000000e+00
15	3.688512e-05	2.324764e-06	8.881784e-16	1.000000e-01	2	50	1.000000e+00
16	1.828668e-05	5.707087e-07	4.440892e-16	1.000000e-01	2	50	1.000000e+00
17	9.083085e-06	1.407632e-07	4.440892e-16	1.000000e-01	2	50	1.000000e+00
18	4.528954e-06	3.497066e-08	8.881784e-16	1.000000e-01	2	50	1.000000e+00
19	2.263894e-06	8.734220e-09	8.881784e-16	1.000000e-01	2	50	1.000000e+00
20	1.135016e-06	2.194196e-09	4.440892e-16	1.000000e-01	2	50	1.000000e+00
21	5.704979e-07	5.540883e-10	8.881784e-16	1.000000e-01	2	50	1.000000e+00
22	2.874972e-07	1.406474e-10	8.881784e-16	1.000000e-01	2	50	1.000000e+00
23	1.452327e-07	3.587608e-11	8.881784e-16	1.000000e-01	2	50	1.000000e+00
24	7.354040e-08	9.194756e-12	4.440892e-16	1.000000e-01	2	50	1.000000e+00
25	3.732277e-08	2.367329e-12	8.881784e-16	1.000000e-01	2	50	1.000000e+00
26	1.898368e-08	6.123990e-13	4.440892e-16	1.000000e-01	2	50	1.000000e+00
27	9.676397e-09	1.588729e-13	4.440892e-16	1.000000e-01	2	50	1.000000e+00
28	4.942513e-09	4.152234e-14	4.440892e-16	1.000000e-01	2	50	1.000000e+00
29	2.529634e-09	1.088019e-14	4.440892e-16	1.000000e-01	2	50	1.000000e+00
30	1.297243e-09	2.886580e-15	8.881784e-16	1.000000e-01	2	50	1.000000e+00
31	6.687387e-10	8.881784e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
32	3.654757e-10	8.881784e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
33	1.999367e-10	6.661338e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
34	1.094865e-10	6.661338e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
35	6.001536e-11	4.440892e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
36	3.293003e-11	6.661338e-16	8.881784e-16	1.000000e-01	2	50	1.000000e+00
37	1.808603e-11	6.661338e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
38	9.950304e-12	6.661338e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
39	5.562280e-12	4.440892e-16	8.881784e-16	1.000000e-01	2	50	1.000000e+00
40	3.114045e-12	4.440892e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
41	1.745956e-12	4.440892e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
42	9.802523e-13	6.661338e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
43	5.511164e-13	4.440892e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
44	3.102189e-13	6.661338e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
45	1.748480e-13	6.661338e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
46	9.863985e-14	6.661338e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
47	5.572973e-14	6.661338e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
48	3.150084e-14	4.440892e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
49	1.784163e-14	6.661338e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
50	1.009262e-14	8.881784e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
51	5.734996e-15	6.661338e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
52	3.238729e-15	6.661338e-16	8.881784e-16	1.000000e-01	2	50	1.000000e+00
53	1.850950e-15	6.661338e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
54	1.032160e-15	4.440892e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
55	5.984796e-16	2.220446e-16	8.881784e-16	1.000000e-01	2	50	1.000000e+00
56	3.278627e-16	4.440892e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
57	1.994932e-16	2.220446e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
58	1.110223e-16	4.440892e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
59	1.110223e-16	3.330669e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
60	1.110223e-16	3.330669e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
61	1.110223e-16	4.440892e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
62	1.110223e-16	4.440892e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
63	1.110223e-16	4.440892e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
64	1.110223e-16	4.440892e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
65	1.110223e-16	4.440892e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
66	1.110223e-16	4.440892e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
67	1.110223e-16	4.440892e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
68	1.110223e-16	4.440892e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
69	1.110223e-16	4.440892e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
70	1.110223e-16	2.220446e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
71	1.110223e-16	2.220446e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
72	1.110223e-16	2.220446e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
73	1.110223e-16	4.440892e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
74	1.110223e-16	6.661338e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
75	1.110223e-16	8.881784e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
76	1.110223e-16	4.440892e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
77	1.110223e-16	4.440892e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
78	1.110223e-16	2.220446e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
79	1.110223e-16	2.220446e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
80	1.110223e-16	2.220446e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
81	1.110223e-16	2.220446e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
82	1.110223e-16	4.440892e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
83	1.110223e-16	2.220446e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
84	1.110223e-16	2.220446e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
85	1.110223e-16	4.440892e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
86	1.110223e-16	4.440892e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
87	1.387779e-16	4.440892e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
88	1.110223e-16	4.440892e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
89	1.110223e-16	2.220446e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
90	1.110223e-16	2.220446e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
91	1.110223e-16	2.220446e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
92	1.110223e-16	2.220446e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
93	1.110223e-16	2.220446e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
94	1.110223e-16	2.220446e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
95	1.110223e-16	4.440892e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
96	1.110223e-16	4.440892e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
97	1.110223e-16	4.440892e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
98	1.110223e-16	2.220446e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
99	1.387779e-16	2.220446e-16	4.440892e-16	1.000000e-01	2	50	1.000000e+00
100	0.000000e+00	0.000000e+00	0.000000e+00	0.000000e+00	2	50	1.000000e+00
``````

My current tolerance is 1e-2 but it seems like my residuals are below that at the beginning but it keeps going. Is there a reason this might be happening?

Any help would be appreciated. I am happy to provide more information if necessary.

Okay, since the QP solver is also returning the status 2, Iâ€™m guessing something is off with my initial guess. In case anyone is interested.

Hi, sorry for my late reply.
Honestly I cannot help regarding the tolerance question.
As regards error 2 (maximum number of iterations reached), I found out that I made a mistake in the definition of the reference inside the loop: I was not setting the correct value in the last sample of the time horizon.
Later, I also had error 1, and that was due to a wrong initial condition (I was using quaternions to represent attitude and I didnâ€™t use a unit quaternion) which was not compatible with the dynamics since the first iteration.

Hope this can help at least a bit,
Lorenzo

1 Like

I can check this out. Thank you for the reply though.