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