# Solver returning status 1 with 0 iterations how can I diagnose this?

Hello,

I am brand new to acados and nlp problem formulation in general. I am trying to take one of the examples and use it as a starting point for my project. I eventually want to implement this into an MPC formulation. I have taken the Unicycle example and changed the model to be a non-linear bicycle model. After working through all of the compiling bugs, the code runs up to the first iteration of close loop simulation and then the solver exits with status 1 and shows 0 iterations. I have gone through my code and the original example. I am not sure where the issue could be arising. Any thoughts on how to diagnose this or where I am going wrong are greatly appreciated. I am also not sure what is the minimal example needed for this so I will post the 3 files I have.

My apologies this is so much text:

main.py

from vehicle_model import export_vehicle_model
import numpy as np
import scipy.linalg
from utils import plot_robot
import pdb

X0 = np.array([10.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) # Intitalize the states [x,y,v,th,th_d]
N_horizon = 50 # Define the number of discretization steps
T_horizon = 2.0 # Define the prediction horizon
delta_max = 0.001 # Define the max force allowed
delta_min = -0.001
trq_max = 1000.0
trq_min = -1000.0

# create ocp object to formulate the OCP

``````model = export_vehicle_model()
ocp.model = model
nx = model.x.size()[0]
nu = model.u.size()[0]
# nz = model.z.size()[0]
ny = nx + nu

# set dimensions
ocp.dims.N = N_horizon

# set cost
Q_mat = 2 * np.diag([1e3, 0.0, 0.0, 1e2, 0.0, 0.0, 0.0, 0.0])  # [x,y,x_d,y_d,th,th_d]
R_mat = 2 * 5 * np.diag([1e-1, 0.0])

ocp.cost.cost_type = "LINEAR_LS"
ocp.cost.cost_type_e = "LINEAR_LS"

ny = nx + nu
ny_e = nx

ocp.cost.W_e = Q_mat
ocp.cost.W = scipy.linalg.block_diag(Q_mat, R_mat)

ocp.cost.Vx = np.zeros((ny, nx))
ocp.cost.Vx[:nx, :nx] = np.eye(nx)
# ocp.cost.Vz = np.zeros((nz,nz))

Vu = np.zeros((ny, nu))
Vu[nx : nx + nu, 0:nu] = np.eye(nu)
ocp.cost.Vu = Vu

ocp.cost.Vx_e = np.eye(nx)

ocp.cost.yref = np.zeros((ny,))
ocp.cost.yref_e = np.zeros((ny_e,))

# set constraints
ocp.constraints.lbu = np.array([delta_min, trq_min])
ocp.constraints.ubu = np.array([delta_max, trq_max])
ocp.constraints.idxbu = np.array([0,1])

ocp.constraints.x0 = X0

# set options
ocp.solver_options.qp_solver = "PARTIAL_CONDENSING_HPIPM"  # FULL_CONDENSING_QPOASES
ocp.solver_options.hessian_approx = "GAUSS_NEWTON"  # 'GAUSS_NEWTON', 'EXACT'
ocp.solver_options.integrator_type = "IRK"
ocp.solver_options.nlp_solver_type = "SQP"  # SQP_RTI, SQP
ocp.solver_options.nlp_solver_max_iter = 400
# ocp.solver_options.levenberg_marquardt = 1e-2

# set parameters
# Cf = 420
# Cr = 420
# Jz = 1000
# ocp.parameter_values = np.array([Cf, Cr, Jz])

# set prediction horizon
ocp.solver_options.tf = T_horizon
#pdb.set_trace()

return ocp
``````

def closed_loop_simulation():

``````# create solvers
ocp = create_ocp_solver_description()
ocp, json_file="acados_ocp_" + ocp.model.name + ".json"
)
ocp, json_file="acados_ocp_" + ocp.model.name + ".json"
)

# prepare simulation
Nsim = 100
nx = ocp.model.x.size()[0]
nu = ocp.model.u.size()[0]

simX = np.ndarray((Nsim + 1, nx))
simU = np.ndarray((Nsim, nu))

xcurrent = X0
simX[0, :] = xcurrent

# initialize solver
for stage in range(N_horizon + 1):
for stage in range(N_horizon):

# closed loop
for i in range(Nsim):
print("i="+str(i))
# set initial state constraint

# update yref
for j in range(N_horizon):
print("j=" + str(j))
yref = np.array([10, 0, 0, 100, 0, 0, 0, 0, 0, 0])
yref_N = np.array([10, 0, 0, 100, 0, 0, 0, 0])

# solve ocp

print("solver status: " + str(status))
if status not in [0, 2]:
plot_robot(
np.linspace(0, T_horizon / N_horizon * i, i + 1),
trq_max,
simU[:i, :],
simX[: i + 1, :],
)
raise Exception(
f"acados acados_ocp_solver returned status {status} in closed loop instance {i} with {xcurrent}"
)

if status == 2:
print(
f"acados acados_ocp_solver returned status {status} in closed loop instance {i} with {xcurrent}"
)

# simulate system

if status != 0:
raise Exception(
f"acados integrator returned status {status} in closed loop instance {i}"
)

# update state
simX[i + 1, :] = xcurrent

# plot results
plot_robot(
np.linspace(0, T_horizon / N_horizon * Nsim, Nsim + 1), [trq_max, None], simU, simX
)
``````

if name == “main”:
closed_loop_simulation()

vehicle_model.py
from casadi import SX, vertcat, sin, cos, atan

# http://users.isr.ist.utl.pt/~jag/publications/08-JETC-RCarona-vcontrol.pdf

model_name = “bicycle_model_ode”

``````# set up states & controls
V = SX.sym("V")
v = SX.sym("v")
r = SX.sym("r")
X = SX.sym("X")
Y = SX.sym("Y")
Psi = SX.sym("Psi")
delta = SX.sym("delta")
trq = SX.sym("trq")

x = vertcat(V,v,r,X,Y,Psi,delta,trq)

d_delta = SX.sym("d_delta")
d_trq = SX.sym("d_trq")
u = vertcat(d_delta,d_trq)

# xdot
V_dot = SX.sym("V_dot")
v_dot = SX.sym("v_dot")
r_dot = SX.sym("r_dot")
X_dot = SX.sym("X_dot")
Y_dot = SX.sym("Y_dot")
Psi_dot = SX.sym("Psi_dot")
delta_dot = SX.sym("delta_dot")
trq_dot = SX.sym("trq_dot")

xdot = vertcat(V_dot,v_dot,r_dot,X_dot,Y_dot,Psi_dot,delta_dot,trq_dot)

# parameters
mass =1350
r_tire = 0.5
lf = 1.8
lr = 1.8
tr = 2
Cf = 420;
Cr = 420;
Jz = 1000;
# p = vertcat([Cf, Cr, Jz])
p = []

# algebraic variables
# alpha_f = atan((v+lf*r)/V) -delta
# alpha_r = atan((v-lr*r)/V)
# z = vertcat(alpha_f, alpha_r)

# dynamics

# f_expl = vertcat(T/r_tire/mass + r*v, (-z[0]*p[0]-z[1]*p[1])/mass - r*V, (-lf*z[0]*p[0]+lr*z[1]*p[1])/Jz,
#                  V*cos(Psi) - v*sin(Psi), V*sin(Psi) + v*cos(Psi), r, delta_dot)

f_expl = vertcat(trq/r_tire/mass + r*v,
(-Cf*(atan((v+lf*r)/V)-delta) - Cr*(atan(v-lr*r)/V))/mass - r*V,
(-lf*Cf*(atan((v+lf*r)/V)-delta) + lr*Cr*(atan(v-lr*r)/V))/Jz,
V*cos(Psi) - v*sin(Psi),
V*sin(Psi) + v*cos(Psi),
r,
d_delta,
d_trq)

f_impl = xdot - f_expl

model.f_impl_expr = f_impl
model.f_expl_expr = f_expl
model.x = x
model.xdot = xdot
model.u = u
# model.z = z
model.p = p
model.name = model_name

return model
``````

utils.py

import os
import matplotlib.pyplot as plt
import numpy as np

def plot_robot(
shooting_nodes,
u_max,
U,
X_true,
X_est=None,
Y_measured=None,
latexify=False,
plt_show=True,
X_true_label=None,
):
“”"
Params:
shooting_nodes: time values of the discretization
u_max: maximum absolute value of u
U: arrray with shape (N_sim-1, nu) or (N_sim, nu)
X_true: arrray with shape (N_sim, nx)
X_est: arrray with shape (N_sim-N_mhe, nx)
Y_measured: array with shape (N_sim, ny)
latexify: latex style plots
“”"

``````if latexify:
latexify_plot()

WITH_ESTIMATION = X_est is not None and Y_measured is not None

N_sim = X_true.shape[0]
nx = X_true.shape[1]
nu = U.shape[1]

Tf = shooting_nodes[N_sim - 1]
t = shooting_nodes

print(X_true)
print(N_sim)
print(Tf)
print(t)

Ts = t[1] - t[0]
if WITH_ESTIMATION:
N_mhe = N_sim - X_est.shape[0]
t_mhe = np.linspace(N_mhe * Ts, Tf, N_sim - N_mhe)

control_lables = ["\$F\$", "\$T\$"]
for i in range(nu):
plt.subplot(nx + nu, 1, i+1)
# line, = plt.step(t, np.append([U[0]], U))
# line, = plt.plot(t, U[:, 0], label='U')
(line,) = plt.step(t, np.append([U[0, i]], U[:, i]))
# (line,) = plt.step(t, np.append([U[0, 0]], U[:, 0]))
if X_true_label is not None:
line.set_label(X_true_label)
else:
line.set_color("r")
# plt.title('closed-loop simulation')
plt.ylabel(control_lables[i])
plt.xlabel("\$t\$")
if u_max[i] is not None:
plt.hlines(u_max[i], t[0], t[-1], linestyles="dashed", alpha=0.7)
plt.hlines(-u_max[i], t[0], t[-1], linestyles="dashed", alpha=0.7)
plt.ylim([-1.2 * u_max[i], 1.2 * u_max[i]])
plt.grid()

states_lables = ["\$x\$", "\$y\$", "\$v\$", "\$theta\$", "\$thetad\$"]
for i in range(nx):
plt.subplot(nx + nu, 1, i + nu+1)
(line,) = plt.plot(t, X_true[:, i], label="true")
if X_true_label is not None:
line.set_label(X_true_label)

if WITH_ESTIMATION:
plt.plot(t_mhe, X_est[:, i], "--", label="estimated")
plt.plot(t, Y_measured[:, i], "x", label="measured")

plt.ylabel(states_lables[i])
plt.xlabel("\$t\$")
plt.grid()
plt.legend(loc=1)