I figured it out.
The issue are Nan’s in the solution. The reset() function does NOT remove the Nan’s from the solver’s memory. So that after the reset the solver isn’t actually back to the initial state, even if you call load_iterate() with the initial configuration of the solver.
I think something similar was mentioned here: How to reset solver after nan occured in the output of AcadosSim?
My solution was to switch my qp_solver from FULL_CONDENSING_HPIPM to PARTIAL_CONDENSING_HPIPM, because for that solver the reset() function actually deletes the internal memory (also all Nan’s), resulting in the desired reset behavior regardless of how and why the MPC diverged in the last episode. Here the relevant commit explaining that for PARTIAL_CONDENSING_HPIPM the reset() function deletes internal memory: OCP solver reset: Reset QP solver memory in case there are NaNs in HP… · acados/acados@fd2f1d2 · GitHub
My code to reset everything is:
self.mpc_solver.reset()
self.mpc_solver.load_iterate("mpc_initialization.json")
# after that I put all relevant variables (like simulation state, MPC state, reference trajectory) back to the initial values
I would like to request this feature for all qp solver’s if possible. Also I found it very frustrating that their is no proper documentation of the reset() function.