Hi
This is my first time using acados, I modified minimal_example_closed_loop.py example and replaced export_pendulum_ode_model function with my desired dynamics (bicycle model). I have gone through the examples several times but I haven’t found the issue yet. xcurrent never converges to y_ref. I tried increasing max iteration to 1000 but it didn’t solve the issue. When I increase the prediction horizon to 1s, it returns immediately with error status 3.
I would be very grateful if someone could take a look at my code to see if I have done something wrong.
pendulum_model.py
#
# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
#
# This file is part of acados.
#
# The 2-Clause BSD License
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# 1. Redistributions of source code must retain the above copyright notice,
# this list of conditions and the following disclaimer.
#
# 2. Redistributions in binary form must reproduce the above copyright notice,
# this list of conditions and the following disclaimer in the documentation
# and/or other materials provided with the distribution.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.;
#
from acados_template import AcadosModel
from casadi import SX, MX, vertcat, sin, cos, atan, Function
def export_pendulum_ode_model():
model_name = 'pendulum_ode'
# constants
m = 0.041
C1 = 0.5
C2 = 15.5
Cm1 = 0.287
Cm2 = 0.05
Cr0 = 0.0518
Cr2 = 0.00035
Dr = 0.1737
Cr = 1.2691
Br = 3.3852
Df = 0.192
Cf = 1.2
Bf = 2.579
lr = 0.033
lf = 0.033
Ptv = 0.1
Iz = 27.8E-6
# set up states & controls
px = MX.sym("px")
py = MX.sym("py")
phi = MX.sym("phi")
vx = MX.sym("vx")
vy = MX.sym("vy")
r = MX.sym("r")
delta = MX.sym("delta")
T = MX.sym("T")
x = vertcat(px, py, phi, vx, vy, r, delta, T)
# controls
ddelta = MX.sym("ddelta")
dT = MX.sym("dT")
u = vertcat(ddelta, dT)
# xdot
px_dot = MX.sym("px_dot")
py_dot = MX.sym("py_dot")
phi_dot = MX.sym("phi_dot")
vx_dot = MX.sym("vx_dot")
vy_dot = MX.sym("vy_dot")
r_dot = MX.sym("r_dot")
delta_dot = MX.sym("delta_dot")
T_dot = MX.sym("_Tdot")
xdot = vertcat(px_dot, py_dot, phi_dot, vx_dot, vy_dot, r_dot, delta_dot, T_dot)
# algebraic variables
# z = None
# parameters
p = []
# dynamics
Fx = Cm1 * T - Cr0 - Cr2 * vx * vx
alphar = atan((vy - lr * r) / vx)
alphaf = atan((vy + lf * r) / vx) - delta
Fry = Dr * sin(Cr * atan(Br * alphar))
Ffy = Df * sin(Cf * atan(Bf * alphaf))
rtarget = delta * vx / (lf + lr)
Tou = (rtarget - r) * Ptv
f_expl = vertcat(
vx * cos(phi) - vy * sin(phi),
vx * sin(phi) + vy * cos(phi),
r,
(Fx - Ffy * sin(delta) + m * vy * r) / m,
(Fry + Ffy * cos(delta) - m * vx * r) / m,
(Ffy * lf * cos(delta) - Fry * lr + Tou) / Iz,
ddelta,
dT
)
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
# def export_pendulum_ode_model():
# model_name = 'pendulum_ode'
# # constants
# M = 1. # mass of the cart [kg] -> now estimated
# m = 0.1 # mass of the ball [kg]
# g = 9.81 # length of the rod [m]
# l = 0.8 # gravity constant [m/s^2]
# # set up states & controls
# x1 = SX.sym('x1')
# theta = SX.sym('theta')
# v1 = SX.sym('v1')
# dtheta = SX.sym('dtheta')
# x = vertcat(x1, theta, v1, dtheta)
# # controls
# F = SX.sym('F')
# u = vertcat(F)
# # xdot
# x1_dot = SX.sym('x1_dot')
# theta_dot = SX.sym('theta_dot')
# v1_dot = SX.sym('v1_dot')
# dtheta_dot = SX.sym('dtheta_dot')
# xdot = vertcat(x1_dot, theta_dot, v1_dot, dtheta_dot)
# # algebraic variables
# # z = None
# # parameters
# p = []
# # dynamics
# denominator = M + m - m*cos(theta)*cos(theta)
# f_expl = vertcat(v1,
# dtheta,
# (-m*l*sin(theta)*dtheta*dtheta + m*g*cos(theta)*sin(theta)+F)/denominator,
# (-m*l*cos(theta)*sin(theta)*dtheta*dtheta + F*cos(theta)+(M+m)*g*sin(theta))/(l*denominator)
# )
# 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
def export_pendulum_ode_model_with_discrete_rk4(dT):
model = export_pendulum_ode_model()
x = model.x
u = model.u
nx = x.size()[0]
ode = Function('ode', [x, u], [model.f_expl_expr])
# set up RK4
k1 = ode(x, u)
k2 = ode(x+dT/2*k1,u)
k3 = ode(x+dT/2*k2,u)
k4 = ode(x+dT*k3, u)
xf = x + dT/6 * (k1 + 2*k2 + 2*k3 + k4)
model.disc_dyn_expr = xf
print("built RK4 for pendulum model with dT = ", dT)
print(xf)
return model
def export_augmented_pendulum_model():
# pendulum model augmented with algebraic variable just for testing
model = export_pendulum_ode_model()
model_name = 'augmented_pendulum'
z = SX.sym('z')
f_impl = vertcat( model.xdot - model.f_expl_expr, \
z - model.u*model.u)
model.f_impl_expr = f_impl
model.z = z
model.name = model_name
return model
minimal_example_closed_loop.py
#
# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
#
# This file is part of acados.
#
# The 2-Clause BSD License
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# 1. Redistributions of source code must retain the above copyright notice,
# this list of conditions and the following disclaimer.
#
# 2. Redistributions in binary form must reproduce the above copyright notice,
# this list of conditions and the following disclaimer in the documentation
# and/or other materials provided with the distribution.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.;
#
import sys
sys.path.insert(0, 'common')
from acados_template import AcadosOcp, AcadosOcpSolver, AcadosSimSolver
from pendulum_model import export_pendulum_ode_model
from utils import plot_pendulum
import numpy as np
import scipy.linalg
# create ocp object to formulate the OCP
ocp = AcadosOcp()
# set model
model = export_pendulum_ode_model()
ocp.model = model
Tf = 0.001
nx = model.x.size()[0]
nu = model.u.size()[0]
ny = nx + nu
ny_e = nx
N = 20
# set dimensions
ocp.dims.N = N
# NOTE: all dimensions but N are now detected automatically in the Python
# interface, all other dimensions will be overwritten by the detection.
# set cost module
ocp.cost.cost_type = 'LINEAR_LS'
ocp.cost.cost_type_e = 'LINEAR_LS'
# Q = 2*np.diag([1e3, 1e3, 1e-2, 1e-2])
# R = 2*np.diag([1e-2])
Q = 2*np.diag([1e4, 1e4, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-1])
R = 2*np.diag([1e-2, 1e-2])
ocp.cost.W = scipy.linalg.block_diag(Q, R)
ocp.cost.W_e = Q
ocp.cost.Vx = np.zeros((ny, nx))
ocp.cost.Vx[:nx,:nx] = np.eye(nx)
Vu = np.zeros((ny, nu))
# Vu[4,0] = 1.0
Vu[8, 0] = 1.0
Vu[9, 1] = 1.0
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
# Fmax = 80
x0 = np.array([10.0, 5.0, 1.0, 1.0, 1.0, 1.0, 0.0, 0.0])
ocp.constraints.constr_type = 'BGH'
# ocp.constraints.lbu = np.array([-Fmax])
# ocp.constraints.ubu = np.array([+Fmax])
ocp.constraints.lbu = np.array([-2.0, -1.0])
ocp.constraints.ubu = np.array([2.0, 1.0])
ocp.constraints.x0 = x0
ocp.constraints.idxbu = np.array([0, 1])
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
ocp.solver_options.integrator_type = 'ERK'
ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI
ocp.solver_options.nlp_solver_max_iter = 1000
ocp.solver_options.qp_solver_cond_N = N
# set prediction horizon
ocp.solver_options.tf = Tf
acados_ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp_' + model.name + '.json')
acados_integrator = AcadosSimSolver(ocp, json_file = 'acados_ocp_' + model.name + '.json')
iter = 10000
simX = np.ndarray((iter+1, nx))
simU = np.ndarray((iter, nu))
xcurrent = x0
simX[0,:] = xcurrent
# closed loop
yref = np.array([20.0, 10.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.6, 0.0, 0.0])
for i in range(iter):
for k in range(N):
acados_ocp_solver.set(k, "yref", yref)
# solve ocp
acados_ocp_solver.set(0, "lbx", xcurrent)
acados_ocp_solver.set(0, "ubx", xcurrent)
acados_ocp_solver.print_statistics()
status = acados_ocp_solver.solve()
if status != 0:
raise Exception('acados acados_ocp_solver returned status {}. Exiting.'.format(status))
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('acados integrator returned status {}. Exiting.'.format(status))
# update state
xcurrent = acados_integrator.get("x")
simX[i+1,:] = xcurrent
print(xcurrent)
# plot results
# plot_pendulum(np.linspace(0, Tf, N+1), Fmax, simU, simX)
Cheers,
Babak