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 acados_template import AcadosOcp, AcadosOcpSolver, AcadosSimSolver
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
def create_ocp_solver_description() → AcadosOcp:
# create ocp object to formulate the OCP
ocp = AcadosOcp()
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()
acados_ocp_solver = AcadosOcpSolver(
ocp, json_file="acados_ocp_" + ocp.model.name + ".json"
)
acados_integrator = AcadosSimSolver(
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):
acados_ocp_solver.set(stage, "x", 0.0 * np.ones(xcurrent.shape))
for stage in range(N_horizon):
acados_ocp_solver.set(stage, "u", np.zeros((nu,)))
# closed loop
for i in range(Nsim):
print("i="+str(i))
# set initial state constraint
acados_ocp_solver.set(0, "lbx", xcurrent)
acados_ocp_solver.set(0, "ubx", xcurrent)
# 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])
acados_ocp_solver.set(j, "yref", yref)
yref_N = np.array([10, 0, 0, 100, 0, 0, 0, 0])
acados_ocp_solver.set(N_horizon, "yref", yref_N)
# solve ocp
status = acados_ocp_solver.solve()
print("solver status: " + str(status))
if status not in [0, 2]:
acados_ocp_solver.print_statistics()
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}"
)
simU[i, :] = acados_ocp_solver.get(0, "u")
# simulate system
acados_integrator.set("x", xcurrent)
acados_integrator.set("u", simU[i, :])
status = acados_integrator.solve()
if status != 0:
raise Exception(
f"acados integrator returned status {status} in closed loop instance {i}"
)
# update state
xcurrent = acados_integrator.get("x")
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 acados_template import AcadosModel
from casadi import SX, vertcat, sin, cos, atan
Reference for model equations:
http://users.isr.ist.utl.pt/~jag/publications/08-JETC-RCarona-vcontrol.pdf
def export_vehicle_model() → AcadosModel:
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 = AcadosModel()
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
from acados_template import latexify_plot
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)
plt.subplots_adjust(left=None, bottom=None, right=None, top=None, hspace=0.4)
if plt_show:
plt.show()
Thank you for any insights you can offer on how move forward with this.