# author: Daniel Kloeser import time, os import numpy as np from acados_settings import * from plotFcn import * from tracks.readDataFcn import getTrack import matplotlib.pyplot as plt """ Example of the frc_racecars in simulation without obstacle avoidance: This example is for the optimal racing of the frc race cars. The model is a simple bycicle model and the lateral acceleration is constraint in order to validate the model assumptions. The simulation starts at s=-2m until one round is completed(s=8.71m). The beginning is cut in the final plots to simulate a 'warm start'. """ track = "LMS_Track.txt" [Sref, _, _, _, _] = getTrack(track) Tf = 1.0 # prediction horizon N = 50 # number of discretization steps T = 10.00 # maximum simulation time[s] sref_N = 3 # reference for final reference progress # load model constraint, model, acados_solver = acados_settings(Tf, N, track) # dimensions nx = model.x.size()[0] nu = model.u.size()[0] ny = nx + nu Nsim = int(T * N / Tf)
def bycicle_model(track="LMS_Track.txt"):
# define structs
constraint = types.SimpleNamespace()
model = types.SimpleNamespace()
model_name = "Spatialbycicle_model"
# load track parameters
[s0, _, _, _, kapparef] = getTrack(track)
length = len(s0)
pathlength = s0[-1]
# copy loop to beginning and end
s0 = np.append(s0, [s0[length - 1] + s0[1:length]])
kapparef = np.append(kapparef, kapparef[1:length])
s0 = np.append([-s0[length - 2] + s0[length - 81:length - 2]], s0)
kapparef = np.append(kapparef[length - 80:length - 1], kapparef)
# compute spline interpolations
kapparef_s = interpolant("kapparef_s", "bspline", [s0], kapparef)
## Race car parameters
m = 0.043
C1 = 0.5
C2 = 15.5
Cm1 = 0.28
Cm2 = 0.05
Cr0 = 0.011
Cr2 = 0.006
## CasADi Model
# set up states & controls
s = MX.sym("s")
n = MX.sym("n")
alpha = MX.sym("alpha")
v = MX.sym("v")
D = MX.sym("D")
delta = MX.sym("delta")
x = vertcat(s, n, alpha, v, D, delta)
# controls
derD = MX.sym("derD")
derDelta = MX.sym("derDelta")
u = vertcat(derD, derDelta)
# xdot
sdot = MX.sym("sdot")
ndot = MX.sym("ndot")
alphadot = MX.sym("alphadot")
vdot = MX.sym("vdot")
Ddot = MX.sym("Ddot")
deltadot = MX.sym("deltadot")
xdot = vertcat(sdot, ndot, alphadot, vdot, Ddot, deltadot)
# algebraic variables
z = vertcat([])
# parameters
p = vertcat([])
# dynamics
Fxd = (Cm1 - Cm2 * v) * D - Cr2 * v * v - Cr0 * tanh(5 * v)
sdota = (v * np.cos(alpha + C1 * delta)) / (1 - kapparef_s(s) * n)
f_expl = vertcat(
sdota,
v * sin(alpha + C1 * delta),
v * C2 * delta - kapparef_s(s) * sdota,
Fxd / m * cos(C1 * delta),
derD,
derDelta,
)
# constraint on forces
a_lat = C2 * v * v * delta + Fxd * sin(C1 * delta) / m
a_long = Fxd / m
# Model bounds
model.n_min = -0.12 # width of the track [m]
model.n_max = 0.12 # width of the track [m]
# state bounds
model.throttle_min = -1.0
model.throttle_max = 1.0
model.delta_min = -0.40 # minimum steering angle [rad]
model.delta_max = 0.40 # maximum steering angle [rad]
# input bounds
model.ddelta_min = -2.0 # minimum change rate of stering angle [rad/s]
model.ddelta_max = 2.0 # maximum change rate of steering angle [rad/s]
model.dthrottle_min = -10 # -10.0 # minimum throttle change rate
model.dthrottle_max = 10 # 10.0 # maximum throttle change rate
# nonlinear constraint
constraint.alat_min = -4 # maximum lateral force [m/s^2]
constraint.alat_max = 4 # maximum lateral force [m/s^1]
constraint.along_min = -4 # maximum lateral force [m/s^2]
constraint.along_max = 4 # maximum lateral force [m/s^2]
# Define initial conditions
model.x0 = np.array([-2, 0, 0, 0, 0, 0])
# define constraints struct
constraint.alat = Function("a_lat", [x, u], [a_lat])
constraint.pathlength = pathlength
constraint.expr = vertcat(a_long, a_lat, n, D, delta)
# Define model struct
params = types.SimpleNamespace()
params.C1 = C1
params.C2 = C2
params.Cm1 = Cm1
params.Cm2 = Cm2
params.Cr0 = Cr0
params.Cr2 = Cr2
model.f_impl_expr = xdot - f_expl
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
model.params = params
return model, constraint
def plotTrackProj(simX, filename='LMS_Track.txt', T_opt=None):
# load track
s = simX[:, 0]
n = simX[:, 1]
alpha = simX[:, 2]
v = simX[:, 3]
distance = 0.12
# transform data
[x, y, _, _] = transformProj2Orig(s, n, alpha, v, filename)
# plot racetrack map
#Setup plot
plt.figure()
plt.ylim(bottom=-1.75, top=0.35)
plt.xlim(left=-1.1, right=1.6)
plt.ylabel('y[m]')
plt.xlabel('x[m]')
# Plot center line
[Sref, Xref, Yref, Psiref, _] = getTrack(filename)
plt.plot(Xref, Yref, '--', color='k')
# Draw Trackboundaries
Xboundleft = Xref - distance * np.sin(Psiref)
Yboundleft = Yref + distance * np.cos(Psiref)
Xboundright = Xref + distance * np.sin(Psiref)
Yboundright = Yref - distance * np.cos(Psiref)
plt.plot(Xboundleft, Yboundleft, color='k', linewidth=1)
plt.plot(Xboundright, Yboundright, color='k', linewidth=1)
plt.plot(x, y, '-b')
# Draw driven trajectory
heatmap = plt.scatter(x,
y,
c=v,
cmap=cm.rainbow,
edgecolor='none',
marker='o')
cbar = plt.colorbar(heatmap, fraction=0.035)
cbar.set_label("velocity in [m/s]")
ax = plt.gca()
ax.set_aspect('equal', 'box')
# Put markers for s values
xi = np.zeros(9)
yi = np.zeros(9)
xi1 = np.zeros(9)
yi1 = np.zeros(9)
xi2 = np.zeros(9)
yi2 = np.zeros(9)
for i in range(int(Sref[-1]) + 1):
try:
k = list(Sref).index(i + min(abs(Sref - i)))
except:
k = list(Sref).index(i - min(abs(Sref - i)))
[_, nrefi, _, _] = transformOrig2Proj(Xref[k], Yref[k], Psiref[k], 0)
[xi[i], yi[i], _, _] = transformProj2Orig(Sref[k], nrefi + 0.24, 0, 0)
# plt.text(xi[i], yi[i], f'{i}m', fontsize=12,horizontalalignment='center',verticalalignment='center')
plt.text(xi[i],
yi[i],
'{}m'.format(i),
fontsize=12,
horizontalalignment='center',
verticalalignment='center')
[xi1[i], yi1[i], _, _] = transformProj2Orig(Sref[k], nrefi + 0.12, 0,
0)
[xi2[i], yi2[i], _, _] = transformProj2Orig(Sref[k], nrefi + 0.15, 0,
0)
plt.plot([xi1[i], xi2[i]], [yi1[i], yi2[i]], color='black')