Issue: after resetting all relevant variables, calling reset() on the solver and calling load_iterate() with the initial configuration the MPC is still not back to its original state.
Desired behavior:
If the car is too far off the reference, the episode is terminated and restarted, so that the car is at the initial position again. From that point on the MPC start controlling the car again as usual. The behavior should not change after the reset.
The Issue:
Resetting the episode usually works fine. The car then simply restarts at the initial position and the MPC starts solving again from that position. However, sometimes it does not work. Sometimes the MPC seems not to output anything after the reset and the car simply drives forward not following the reference at all. In that case the car diverges too far from the reference and the episode is reset again. This behavior results in an endless loop.
Observations:
- If the car fails quite early along the track, the reset works fine.
- At some iterations the status of the solver is 4 and I get the message
SQP_RTI: QP solver returned error status 3 QP iteration … I believe that means the SQP_RTI status is 4 meaning ACADOS_QP_FAILURE and HPIPM has status 3 meaning NAN_SOL. This does not hinder the car from running along the track. However, when the erroneous behavior described above occurs, SQP_RTI outputs status 4 and HPIPM outputs status 3 at every iteration, meaning the MPC does not solve the problem at all, although the formulation has not changed (at least I think so). - when calling solve_for_x0(self.current_state) the program crashes as soon as HPIPM outputs status 3
- when setting lbx, ubx for stage 0 to be self.current_state and afterwards calling solve() the program does not crash although solve() return status 3 for HPIMP (why and how is this different to solve_for_x0() ?)
What I have tried:
My reset function looks like that
self.mpc_solver.reset()
self.mpc_solver.load_iterate("mpc_initialization.json")
self.mpc_solver.constraints_set(0, "lbx", self.current_state)
self.mpc_solver.constraints_set(0, "ubx", self.current_state)
for j in range(self.N):
self.mpc_solver.set(j, "p", self.nominal_parameters)
self.mpc_solver.set(j, "yref", np.zeros(6))
self.mpc_solver.set(self.N, "yref", np.zeros(4))
first I load the json that I had generated at the very first initialization of the MPC. Than I set the initial constraints to the current_state, which is the exact same state as from the very first initialization. Then I set the nominal parameters again for every shooting node and also I set all references to zero. This is the same setup as with the very first initialization (I think).
During run-time I do following things:
- calculate new reference given the current position of the car
- set new reference trajectory for each shooting node (different for every node)
- set new vehicle parameters for each shooting node (same for every node)
- set reference for final node
- set lbx and ubx constraint for 0 node to be the current simulation state
- call solve()
- retrieve predicted control from node 0
- input that control to simulation in order to obtain next state and set it to the current state
- repeat until the car diverges. Then I reset.
This is the MPC initialization that I run only once at the very beginning. I do not want to re-compile the entire MPC formulation as this would take around 2 seconds each time.
ocp = AcadosOcp()
# define acados ODE
model = AcadosModel()
# pred_model is the prediction model and it is constructed elsewhere
model.x = pred_model.x
model.u = pred_model.u
model.p = pred_model.p
model.name = pred_model.name
ocp.model = model
# Cost type at intermediate shooting nodes (0 to N)
ocp.cost.cost_type = "NONLINEAR_LS"
# Cost type at terminal shooting node (N+1)
ocp.cost.cost_type_e = "NONLINEAR_LS"
ocp.model.cost_y_expr = vertcat(x[:2], y, v, u)
ocp.model.cost_y_expr_e = vertcat(x[:2], y, v)
# W is a block-diagonal matrix with Q and R on it's diagonals
ocp.cost.W = self.W
ocp.cost.W_e = self.Qe
# initial references
ocp.cost.yref = np.array([0, 0, 0, 0, 0, 0])
ocp.cost.yref_e = np.array([0, 0, 0, 0])
# then a bunch of constraints are set
# lh, uh, lh_e, uh_e, lbx, ubx, lbx_e, ubx_e, lbu, ubu
# set initial condition
ocp.constraints.x0 = self.current_mpc_state
# set initial parameter values
ocp.parameter_values = self.nominal_parameters
ocp.solver_options.qp_solver = "FULL_CONDENSING_HPIPM"
ocp.solver_options.qp_solver_iter_max = 50
ocp.solver_options.qp_solver_warm_start = 1
ocp.solver_options.nlp_solver_type = "SQP_RTI"
ocp.solver_options.hessian_approx = "GAUSS_NEWTON"
ocp.solver_options.integrator_type = "ERK"
# Runge-Kutta 4
ocp.solver_options.sim_method_num_stages = 4
ocp.solver_options.sim_method_num_steps = 3
# create solver
self.mpc_solver = AcadosOcpSolver(ocp, json_file="acados_ocp_test.json")
Question:
I would like to understand why sometime the MPC seems not to solve the problem after the reset. The problem must be feasible because normally the MPC is able to control the car along the track. It is only under certain circumstances that the reset fails. Is it possible that I am forgetting to free some cache/memory? Do I also need to reset the simulation somehow and not only the MPC? Is it possible that I forget to set some constraints? I am puzzled right now and would appreciate some guidance. Also general advice on how to debug such kind of problems (which metrics to look at, what properties of the ocp solver to set for more information etc.) would be very helpful.