Hi,
I am working on a similar problem as the racecar example with some changes to the model. I have got successful results in the past with the same problem formulation on casadi with qpoases (in python) as the solver. However, I wanted to switch to acados (python interface) since it has more functionalities. I have used different solver options such as ‘FULL_CONDENSING_QPOASES’ and ‘PARTIAL_CONDENSING_HPIPM’ and both SQP and SQP_RTI.
The issue is that the problem doesn’t converge or at best it oscillates over the path to converge after 10 seconds (it’s a car that is supposed to follow a straightline starting from an initial laterally offset point). I start 5 meters lateral to the straight line to see if the mpc can take the car to the straight line and keep it straight.
Sometimes, HPIPM gives me status 3 which seems to be related to NaN solution. This specifically happens when the lateral error is almost near zero and there are slight heading errors.
However, I was not able to see anywhere in my code that results in NaN solution. What can cause this type of error usually? I have tried to get rid of most of my inequalities to check whether that solves the issue but I still end up in status 3 and status 37 (for qpoases) sometimes in the middle of the simulation.
Any help/tips are greatly appreciated as I am new to acados.
Below is a snippet of my mpc problem. acados_settings function defines the OCP and solve_mpc() runs in the loop for each iteration of mpc:
def acados_settings(self, Tf, N):
# create render arguments
ocp = AcadosOcp()
# export model
model, constraint = self.bicycle_model(self.initial_conditions, self.params)
# define acados ODE
model_ac = AcadosModel()
model_ac.f_impl_expr = model.f_impl_expr
model_ac.f_expl_expr = model.f_expl_expr
model_ac.x = model.x
model_ac.xdot = model.xdot
model_ac.u = model.u
model_ac.name = model.name
ocp.model = model_ac
# define constraint
# model_ac.con_h_expr = constraint.expr
# dimensions
nx = model.x.size()[0]
nu = model.u.size()[0]
ny = nx + nu
ny_e = nx
nsbx = 1
# nh = constraint.expr.shape[0]
nh = 0
nsh = nh
ns = nsh + nsbx
# discretization
ocp.dims.N = N
# set cost
Q = np.diag([0, 2000.0, 6000.0, 0, 0, 0])
R = np.eye(nu)
R[0, 0] = 1e-3
R[1, 1] = 5e-3
Qe = np.diag([0, 20000.0, 6000.0, 0, 0, 0])
ocp.cost.cost_type = "LINEAR_LS"
ocp.cost.cost_type_e = "LINEAR_LS"
unscale = N / Tf
ocp.cost.W = unscale * scipy.linalg.block_diag(Q, R)
ocp.cost.W_e = Qe / unscale
Vx = np.zeros((ny, nx))
Vx[1, 1] = 1 # y
Vx[2, 2] = 1 # yaw
Vx[3, 3] = 0 # forward velocity - set to zero for now
ocp.cost.Vx = Vx
Vu = np.zeros((ny, nu))
Vu[6, 0] = 1.0 # tau
Vu[7, 1] = 1.0 # delta
ocp.cost.Vu = Vu
Vx_e = np.zeros((ny_e, nx))
Vx_e[1, 1] = 1 # y
Vx_e[2, 2] = 1 # yaw
Vx_e[3, 3] = 0 # forward velocity
ocp.cost.Vx_e = Vx_e
# set intial references
ocp.cost.yref = np.array([0, 10, 0, 10, 0, 0, 0, 0])
ocp.cost.yref_e = np.array([0, 10, 0, 10, 0, 0])
# input constraints
ocp.constraints.lbu = np.array([model.tau_min, model.delta_min])
ocp.constraints.ubu = np.array([model.tau_max, model.delta_max])
ocp.constraints.Jbu = np.array([[1, 0],
[0, 1]])
# terminal constraints - commented up
# ocp.constraints.Jbx_e = np.array([[0, 1, 0, 0, 0, 0],
# [0, 0, 1, 0, 0, 0]])
# ocp.constraints.lbx_e = np.array([0, -80 * np.pi/180])
# ocp.constraints.ubx_e = np.array([20, +80 * np.pi/180])
# box constraints
# ocp.constraints.lh = np.array(
# [
# constraint.along_min,
# constraint.alat_min,
# ]
# )
# ocp.constraints.uh = np.array(
# [
# constraint.along_max,
# constraint.alat_max,
# ]
# )
# set initial condition
ocp.constraints.x0 = model.x0
# set QP solver and integration
ocp.solver_options.tf = Tf # prediction horizon
# ocp.solver_options.qp_solver = 'FULL_CONDENSING_QPOASES'
ocp.solver_options.qp_solver = "PARTIAL_CONDENSING_HPIPM"
ocp.solver_options.nlp_solver_type = "SQP_RTI"
# ocp.solver_options.levenberg_marquardt = 0.001
ocp.solver_options.hessian_approx = "GAUSS_NEWTON"
ocp.solver_options.integrator_type = "ERK"
ocp.solver_options.sim_method_num_stages = 4
ocp.solver_options.sim_method_num_steps = 3
ocp.solver_options.nlp_solver_max_iter = 1000
ocp.solver_options.tol = 1e-4
# ocp.solver_options.nlp_solver_tol_comp = 1e-1
# create solver
acados_solver = AcadosOcpSolver(ocp, json_file="acados_ocp.json")
acados_solver.options_set('warm_start_first_qp', 0)
return constraint, model, acados_solver
def solve_mpc(self, current_state, uk_prev_step):
# initialize x0 with current measurment
x_r, y_r, yaw_r, vx_r, vy_r, omega_r = current_state
self.acados_solver.set(0, "lbx", current_state)
self.acados_solver.set(0, "ubx", current_state)
# cost parameters
qy = 200 * 1 / 0.001 ** 2 #cost for lateral error
qyaw = 10 * 1 / (0.01 * np.pi / 180) ** 2 #cost for yaw error
Q = np.diag([0, qy, qyaw, 0, 0, 0])
R = np.eye(2)
R[0, 0] = 0 * 1e-3 # R_tau (torque request - set to zero for now as only delta is being commanded)
R[1, 1] = 5000 * 1 / (0.1 * np.pi / 180) ** 2 # R_delta (cost for delta)
W = self.N / self.T * scipy.linalg.block_diag(Q, R)
Qe = np.diag([0, 10 * qy, 10 * qyaw, 0, 0, 0]) / (self.N / self.T)
W_e = Qe
# Update the cost
for i in range(self.N):
self.acados_solver.cost_set(i, 'W', W)
yref = np.array([0, 10, 0, 10.0, 0, 0, uk_prev_step[0], uk_prev_step[1]])
self.acados_solver.set(i, "yref", yref)
self.acados_solver.cost_set(self.N, 'W', W_e)
yref_N = np.array([0, 10, 0, 0.0, 0, 0])
self.acados_solver.set(self.N, "yref", yref_N)
# set options
self.acados_solver.options_set('print_level', 0)
status = self.acados_solver.solve()
# get solution
x0 = self.acados_solver.get(0, "x")
u0 = self.acados_solver.get(0, "u")
xN = self.acados_solver.get(self.N, "x")
if status != 0:
print("acados returned status {}.".format(status))
self.acados_solver.print_statistics()
return [u0, crosstrack, x0, xN, status]