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