# 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)
``````

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