def _add_constraints(self):
# State Bound Constraints
self.opti.subject_to(
self.opti.bounded(self.V_MIN, self.v_dv, self.V_MAX))
# Initial State Constraint
self.opti.subject_to(self.x_dv[0] == self.z_curr[0])
self.opti.subject_to(self.y_dv[0] == self.z_curr[1])
self.opti.subject_to(self.psi_dv[0] == self.z_curr[2])
self.opti.subject_to(self.v_dv[0] == self.z_curr[3])
# State Dynamics Constraints
for i in range(self.N):
beta = casadi.atan(self.L_R / (self.L_F + self.L_R) *
casadi.tan(self.df_dv[i]))
self.opti.subject_to(
self.x_dv[i + 1] == self.x_dv[i] + self.DT *
(self.v_dv[i] * casadi.cos(self.psi_dv[i] + beta)))
self.opti.subject_to(
self.y_dv[i + 1] == self.y_dv[i] + self.DT *
(self.v_dv[i] * casadi.sin(self.psi_dv[i] + beta)))
self.opti.subject_to(self.psi_dv[i +
1] == self.psi_dv[i] + self.DT *
(self.v_dv[i] / self.L_R * casadi.sin(beta)))
self.opti.subject_to(self.v_dv[i + 1] == self.v_dv[i] + self.DT *
(self.acc_dv[i]))
# Input Bound Constraints
self.opti.subject_to(
self.opti.bounded(self.A_MIN, self.acc_dv, self.A_MAX))
self.opti.subject_to(
self.opti.bounded(self.DF_MIN, self.df_dv, self.DF_MAX))
# Input Rate Bound Constraints
self.opti.subject_to(
self.opti.bounded(self.A_DOT_MIN - self.sl_acc_dv[0],
self.acc_dv[0] - self.u_prev[0],
self.A_DOT_MAX + self.sl_acc_dv[0]))
self.opti.subject_to(
self.opti.bounded(self.DF_DOT_MIN - self.sl_df_dv[0],
self.df_dv[0] - self.u_prev[1],
self.DF_DOT_MAX + self.sl_df_dv[0]))
for i in range(self.N - 1):
self.opti.subject_to(
self.opti.bounded(self.A_DOT_MIN - self.sl_acc_dv[i + 1],
self.acc_dv[i + 1] - self.acc_dv[i],
self.A_DOT_MAX + self.sl_acc_dv[i + 1]))
self.opti.subject_to(
self.opti.bounded(self.DF_DOT_MIN - self.sl_df_dv[i + 1],
self.df_dv[i + 1] - self.df_dv[i],
self.DF_DOT_MAX + self.sl_df_dv[i + 1]))
# Other Constraints
self.opti.subject_to(0 <= self.sl_df_dv)
self.opti.subject_to(0 <= self.sl_acc_dv)
def continuous_dynamics(x, u, p):
#extract states and inputs
posx = x[zvars.index('posx')-ninputs]
posy = x[zvars.index('posy')-ninputs]
phi = x[zvars.index('phi')-ninputs]
vx = x[zvars.index('vx')-ninputs]
vy = x[zvars.index('vy')-ninputs]
omega = x[zvars.index('omega')-ninputs]
d = x[zvars.index('d')-ninputs]
delta = x[zvars.index('delta')-ninputs]
theta = x[zvars.index('theta')-ninputs]
ddot = u[zvars.index('ddot')]
deltadot = u[zvars.index('deltadot')]
thetadot = u[zvars.index('thetadot')]
#build CasADi expressions for dynamic model
#front lateral tireforce
alphaf = -casadi.atan2((omega*lf + vy), vx) + delta
Ffy = Df*casadi.sin(Cf*casadi.atan(Bf*alphaf))
#rear lateral tireforce
alphar = casadi.atan2((omega*lr - vy),vx)
Fry = Dr*casadi.sin(Cr*casadi.atan(Br*alphar))
#rear longitudinal forces
Frx = (Cm1-Cm2*vx) * d - Croll -Cd*vx*vx
#let z = [u, x] = [ddot, deltadot, thetadot, posx, posy, phi, vx, vy, omega, d, delta, theta]
statedot = np.array([
vx * casadi.cos(phi) - vy * casadi.sin(phi), #posxdot
vx * casadi.sin(phi) + vy * casadi.cos(phi), #posydot
omega, #phidot
1/m * (Frx - Ffy*casadi.sin(delta) + m*vy*omega), #vxdot
1/m * (Fry + Ffy*casadi.cos(delta) - m*vx*omega), #vydot
1/Iz * (Ffy*lf*casadi.cos(delta) - Fry*lr), #omegadot
ddot,
deltadot,
thetadot
])
return statedot
def smooth_track(track):
N = len(track)
xs = track[:, 0]
ys = track[:, 1]
th1_f = ca.MX.sym('y1_f', N-2)
th2_f = ca.MX.sym('y2_f', N-2)
x_f = ca.MX.sym('x_f', N)
y_f = ca.MX.sym('y_f', N)
real = ca.Function('real', [th1_f, th2_f], [ca.cos(th1_f)*ca.cos(th2_f) + ca.sin(th1_f)*ca.sin(th2_f)])
im = ca.Function('im', [th1_f, th2_f], [-ca.cos(th1_f)*ca.sin(th2_f) + ca.sin(th1_f)*ca.cos(th2_f)])
sub_cmplx = ca.Function('a_cpx', [th1_f, th2_f], [ca.atan(im(th1_f, th2_f)/real(th1_f, th2_f))])
d_th = ca.Function('d_th', [x_f, y_f], [ca.if_else(ca.fabs(x_f[1:] - x_f[:-1]) < 0.01 ,ca.atan((y_f[1:] - y_f[:-1])/(x_f[1:] - x_f[:-1])), 10000)])
x = ca.MX.sym('x', N)
y = ca.MX.sym('y', N)
th = ca.MX.sym('th', N-1)
B = 5
nlp = {\
'x': ca.vertcat(x, y, th),
'f': ca.sumsqr(sub_cmplx(th[1:], th[:-1])) + B* (ca.sumsqr(x-xs) + ca.sumsqr(y-ys)),
# 'f': B* (ca.sumsqr(x-xs) + ca.sumsqr(y-ys)),
'g': ca.vertcat(\
th - d_th(x, y),
x[0] - xs[0], y[0]- ys[0],
x[-1] - xs[-1], y[-1]- ys[-1],
)\
}
S = ca.nlpsol('S', 'ipopt', nlp)
# th0 = [lib.get_bearing(track[i, 0:2], track[i+1, 0:2]) for i in range(N-1)]
th0 = d_th(xs, ys)
x0 = ca.vertcat(xs, ys, th0)
lbx = [0] *2* N + [-np.pi]*(N -1)
ubx = [100] *2 * N + [np.pi]*(N-1)
r = S(x0=x0, lbg=0, ubg=0, lbx=lbx, ubx=ubx)
print(f"Solution found")
x_opt = r['x']
xs_new = np.array(x_opt[:N])
ys_new = np.array(x_opt[N:2*N])
track[:, 0] = xs_new[:, 0]
track[:, 1] = ys_new[:, 0]
return track
def _add_constraints(self):
## State Bound Constraints
self.opti.subject_to( self.opti.bounded(self.EY_MIN, self.ey_dv, self.EY_MAX) )
self.opti.subject_to( self.opti.bounded(self.EPSI_MIN, self.epsi_dv, self.EPSI_MAX) )
## Initial State Constraint
self.opti.subject_to( self.s_dv[0] == self.z_curr[0] )
self.opti.subject_to( self.ey_dv[0] == self.z_curr[1] )
self.opti.subject_to( self.epsi_dv[0] == self.z_curr[2] )
self.opti.subject_to( self.v_dv[0] == self.z_curr[3] )
## State Dynamics Constraints
for i in range(self.N):
beta = casadi.atan( self.L_R / (self.L_F + self.L_R) * casadi.tan(self.df_dv[i]) )
dyawdt = self.v_dv[i] / self.L_R * casadi.sin(beta)
dsdt = self.v_dv[i] * casadi.cos(self.epsi_dv[i]+beta) / (1 - self.ey_dv[i] * self.curv_ref[i] )
ay = self.v_dv[i] * dyawdt
self.opti.subject_to( self.s_dv[i+1] == self.s_dv[i] + self.DT * (dsdt) )
self.opti.subject_to( self.ey_dv[i+1] == self.ey_dv[i] + self.DT * (self.v_dv[i] * casadi.sin(self.epsi_dv[i] + beta)) )
self.opti.subject_to( self.epsi_dv[i+1] == self.epsi_dv[i] + self.DT * (dyawdt - dsdt * self.curv_ref[i]) )
self.opti.subject_to( self.v_dv[i+1] == self.v_dv[i] + self.DT * (self.acc_dv[i]) )
self.opti.subject_to( self.opti.bounded(self.AY_MIN - self.sl_ay_dv[i],
ay,
self.AY_MAX + self.sl_ay_dv[i]) )
## Input Bound Constraints
self.opti.subject_to( self.opti.bounded(self.AX_MIN, self.acc_dv, self.AX_MAX) )
self.opti.subject_to( self.opti.bounded(self.DF_MIN, self.df_dv, self.DF_MAX) )
# Input Rate Bound Constraints
self.opti.subject_to( self.opti.bounded( self.AX_DOT_MIN*self.DT,
self.acc_dv[0] - self.u_prev[0],
self.AX_DOT_MAX*self.DT) )
self.opti.subject_to( self.opti.bounded( self.DF_DOT_MIN*self.DT,
self.df_dv[0] - self.u_prev[1],
self.DF_DOT_MAX*self.DT) )
for i in range(self.N - 1):
self.opti.subject_to( self.opti.bounded( self.AX_DOT_MIN*self.DT,
self.acc_dv[i+1] - self.acc_dv[i],
self.AX_DOT_MAX*self.DT) )
self.opti.subject_to( self.opti.bounded( self.DF_DOT_MIN*self.DT,
self.df_dv[i+1] - self.df_dv[i],
self.DF_DOT_MAX*self.DT) )
# Other Constraints
self.opti.subject_to( 0 <= self.sl_ay_dv ) # nonnegative lateral acceleration slack variable
def symbolicalErrors(T_L_I_of_t_G0, t_G0_Ps, p_L_Ps, sym_p_I_P, sym_t_corr_G0,
sym_T_corr_L_twist):
errvec = casadi.MX.sym('', 0, 0)
T_corr_L = casadi_pose.fromTwist(sym_T_corr_L_twist)
for t_G0_P, p_L_P in zip(t_G0_Ps, p_L_Ps):
T_L_I = T_L_I_of_t_G0.timeDerivativeSample(t_G0_P + sym_t_corr_G0,
t_G0_P)
if T_L_I is None:
continue
p_corr_P_guess = (T_corr_L * T_L_I * sym_p_I_P)
sqn = casadi_pose.squareNorm(p_L_P - p_corr_P_guess)
# Robust loss: Flatten the loss of errors > 5cm, most likely outliers.
# TODO completely discard a portion of worst measurements instead?
rloss = casadi.atan(sqn / 0.0025)
errvec = casadi.vertcat(errvec, rloss)
return errvec
def rocket_equations(jit=True):
x = ca.SX.sym('x', 14)
u = ca.SX.sym('u', 4)
p = ca.SX.sym('p', 16)
t = ca.SX.sym('t')
dt = ca.SX.sym('dt')
# State: x
# body frame: Forward, Right, Down
omega_b = x[0:3] # inertial angular velocity expressed in body frame
r_nb = x[3:7] # modified rodrigues parameters
v_b = x[7:10] # inertial velocity expressed in body components
p_n = x[10:13] # positon in nav frame
m_fuel = x[13] # mass
# Input: u
m_dot = ca.if_else(m_fuel > 0, u[0], 0)
fin = u_to_fin(u)
# Parameters: p
g = p[0] # gravity
Jx = p[1] # moment of inertia
Jy = p[2]
Jz = p[3]
Jxz = p[4]
ve = p[5]
l_fin = p[6]
w_fin = p[7]
CL_alpha = p[8]
CL0 = p[9]
CD0 = p[10]
K = p[11]
s_fin = p[12]
rho = p[13]
m_empty = p[14]
l_motor = p[15]
# Calculations
m = m_empty + m_fuel
J_b = ca.SX.zeros(3, 3)
J_b[0, 0] = Jx + m_fuel * l_motor**2
J_b[1, 1] = Jy + m_fuel * l_motor**2
J_b[2, 2] = Jz
J_b[0, 2] = J_b[2, 0] = Jxz
C_nb = so3.Dcm.from_mrp(r_nb)
g_n = ca.vertcat(0, 0, g)
v_n = ca.mtimes(C_nb, v_b)
# aerodynamics
VT = ca.norm_2(v_b)
q = 0.5 * rho * ca.dot(v_b, v_b)
fins = {
'top': {
'fwd': [1, 0, 0],
'up': [0, 1, 0],
'angle': fin[0]
},
'left': {
'fwd': [1, 0, 0],
'up': [0, 0, -1],
'angle': fin[1]
},
'down': {
'fwd': [1, 0, 0],
'up': [0, -1, 0],
'angle': fin[2]
},
'right': {
'fwd': [1, 0, 0],
'up': [0, 0, 1],
'angle': fin[3]
},
}
rel_wind_dir = v_b / VT
# build fin lift/drag forces
vel_tol = 1e-3
FA_b = ca.vertcat(0, 0, 0)
MA_b = ca.vertcat(0, 0, 0)
for key, data in fins.items():
fwd = data['fwd']
up = data['up']
angle = data['angle']
U = ca.dot(fwd, v_b)
W = ca.dot(up, v_b)
side = ca.cross(fwd, up)
alpha = ca.if_else(ca.fabs(U) > vel_tol, -ca.atan(W / U), 0)
perp_wind_dir = ca.cross(side, rel_wind_dir)
norm_perp = ca.norm_2(perp_wind_dir)
perp_wind_dir = ca.if_else(
ca.fabs(norm_perp) > vel_tol, perp_wind_dir / norm_perp, up)
CL = CL0 + CL_alpha * (alpha + angle)
CD = CD0 + K * (CL - CL0)**2
# model stall as no lift if above 23 deg.
L = ca.if_else(ca.fabs(alpha) < 0.4, CL * q * s_fin, 0)
D = CD * q * s_fin
FAi_b = L * perp_wind_dir - D * rel_wind_dir
FA_b += FAi_b
MA_b += ca.cross(-l_fin * fwd - w_fin * side, FAi_b)
FA_b = ca.if_else(ca.fabs(VT) > vel_tol, FA_b, ca.SX.zeros(3))
MA_b = ca.if_else(ca.fabs(VT) > vel_tol, MA_b, ca.SX.zeros(3))
# propulsion
FP_b = ca.vertcat(m_dot * ve, 0, 0)
# force and momental total
F_b = FA_b + FP_b + ca.mtimes(C_nb.T, m * g_n)
M_b = MA_b
force_moment = ca.Function('force_moment', [x, u, p], [F_b, M_b],
['x', 'u', 'p'], ['F_b', 'M_b'])
# right hand side
rhs = ca.Function('rhs', [x, u, p], [
ca.vertcat(
ca.mtimes(ca.inv(J_b),
M_b - ca.cross(omega_b, ca.mtimes(J_b, omega_b))),
so3.Mrp.kinematics(r_nb, omega_b),
F_b / m - ca.cross(omega_b, v_b), ca.mtimes(C_nb, v_b), -m_dot)
], ['x', 'u', 'p'], ['rhs'], {'jit': jit})
# prediction
t0 = ca.SX.sym('t0')
h = ca.SX.sym('h')
x0 = ca.SX.sym('x', 14)
x1 = rk4(lambda t, x: rhs(x, u, p), t0, x0, h)
x1[3:7] = so3.Mrp.shadow_if_necessary(x1[3:7])
predict = ca.Function('predict', [x0, u, p, t0, h], [x1], {'jit': jit})
def schedule(t, start, ty_pairs):
val = start
for ti, yi in ty_pairs:
val = ca.if_else(t > ti, yi, val)
return val
# reference trajectory
pitch_d = 1.0
euler = so3.Euler.from_mrp(r_nb) # roll, pitch, yaw
pitch = euler[1]
# control
u_control = ca.SX.zeros(4)
# these controls are just test controls to make sure the fins are working
u_control[0] = 0.1 # mass flow rate
import control
s = control.tf([1, 0], [0, 1])
H = 140 * (s + 50) * (s + 50) / (s * (2138 * s + 208.8))
Hd = control.tf2ss(control.c2d(H, 0.01))
theta_c = (100 - p_n[2]) * (0.01) / (v_b[2] * ca.cos(p_n[2]))
x_elev = ca.SX.sym('x_elev', 2)
u_elev = ca.SX.sym('u_elev', 1)
x_1 = ca.mtimes(Hd.A, x_elev) + ca.mtimes(Hd.B, u_elev)
y_elev = ca.mtimes(Hd.C, x_elev) + ca.mtimes(Hd.D, u_elev)
elev_c = (theta_c - p_n[2]) * y_elev / (0.5 * rho * v_b[2]**2)
u_control[0] = 0.1 # mass flow rate
u_control[1] = 0
u_control[2] = elev_c
u_control[3] = 0
control = ca.Function('control', [x, p, t, dt], [u_control],
['x', 'p', 't', 'dt'], ['u'])
# initialize
pitch_deg = ca.SX.sym('pitch_deg')
omega0_b = ca.vertcat(0, 0, 0)
r0_nb = so3.Mrp.from_euler(ca.vertcat(0, pitch_deg * ca.pi / 180, 0))
v0_b = ca.vertcat(0, 0, 0)
p0_n = ca.vertcat(0, 0, 0)
m0_fuel = 0.8
# x: omega_b, r_nb, v_b, p_n, m_fuel
x0 = ca.vertcat(omega0_b, r0_nb, v0_b, p0_n, m0_fuel)
# g, Jx, Jy, Jz, Jxz, ve, l_fin, w_fin, CL_alpha, CL0, CD0, K, s, rho, m_emptpy, l_motor
p0 = [
9.8, 0.05, 1.0, 1.0, 0.0, 350, 1.0, 0.05, 2 * np.pi, 0, 0.01, 0.01,
0.05, 1.225, 0.2, 1.0
]
initialize = ca.Function('initialize', [pitch_deg], [x0, p0])
return {
'rhs': rhs,
'predict': predict,
'control': control,
'initialize': initialize,
'force_moment': force_moment,
'x': x,
'u': u,
'p': p
}
return rhs, x, u, p
def opt_mintime(reftrack: np.ndarray,
coeffs_x: np.ndarray,
coeffs_y: np.ndarray,
normvectors: np.ndarray,
pars: dict,
tpamap_path: str,
tpadata_path: str,
export_path: str,
print_debug: bool = False,
plot_debug: bool = False) -> tuple:
"""
Created by:
Fabian Christ
Documentation:
The minimum lap time problem is described as an optimal control problem, converted to a nonlinear program using
direct orthogonal Gauss-Legendre collocation and then solved by the interior-point method IPOPT. Reduced computing
times are achieved using a curvilinear abscissa approach for track description, algorithmic differentiation using
the software framework CasADi, and a smoothing of the track input data by approximate spline regression. The
vehicles behavior is approximated as a double track model with quasi-steady state tire load simplification and
nonlinear tire model.
Please refer to our paper for further information:
Christ, Wischnewski, Heilmeier, Lohmann
Time-Optimal Trajectory Planning for a Race Car Considering Variable Tire-Road Friction Coefficients
Inputs:
reftrack: track [x_m, y_m, w_tr_right_m, w_tr_left_m]
coeffs_x: coefficient matrix of the x splines with size (no_splines x 4)
coeffs_y: coefficient matrix of the y splines with size (no_splines x 4)
normvectors: array containing normalized normal vectors for every traj. point [x_component, y_component]
pars: parameters dictionary
tpamap_path: file path to tpa map (required for friction map loading)
tpadata_path: file path to tpa data (required for friction map loading)
export_path: path to output folder for warm start files and solution files
print_debug: determines if debug messages are printed
plot_debug: determines if debug plots are shown
Outputs:
alpha_opt: solution vector of the optimization problem containing the lateral shift in m for every point
v_opt: velocity profile for the raceline
"""
# ------------------------------------------------------------------------------------------------------------------
# PREPARE TRACK INFORMATION ----------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# spline lengths
spline_lengths_refline = tph.calc_spline_lengths.calc_spline_lengths(
coeffs_x=coeffs_x, coeffs_y=coeffs_y)
# calculate heading and curvature (numerically)
kappa_refline = tph.calc_head_curv_num. \
calc_head_curv_num(path=reftrack[:, :2],
el_lengths=spline_lengths_refline,
is_closed=True,
stepsize_curv_preview=pars["curv_calc_opts"]["d_preview_curv"],
stepsize_curv_review=pars["curv_calc_opts"]["d_review_curv"],
stepsize_psi_preview=pars["curv_calc_opts"]["d_preview_head"],
stepsize_psi_review=pars["curv_calc_opts"]["d_review_head"])[1]
# close track
kappa_refline_cl = np.append(kappa_refline, kappa_refline[0])
w_tr_left_cl = np.append(reftrack[:, 3], reftrack[0, 3])
w_tr_right_cl = np.append(reftrack[:, 2], reftrack[0, 2])
# step size along the reference line
h = pars["stepsize_opts"]["stepsize_reg"]
# optimization steps (0, 1, 2 ... end point/start point)
steps = [i for i in range(kappa_refline_cl.size)]
# number of control intervals
N = steps[-1]
# station along the reference line
s_opt = np.linspace(0.0, N * h, N + 1)
# interpolate curvature of reference line in terms of steps
kappa_interp = ca.interpolant('kappa_interp', 'linear', [steps],
kappa_refline_cl)
# interpolate track width (left and right to reference line) in terms of steps
w_tr_left_interp = ca.interpolant('w_tr_left_interp', 'linear', [steps],
w_tr_left_cl)
w_tr_right_interp = ca.interpolant('w_tr_right_interp', 'linear', [steps],
w_tr_right_cl)
# describe friction coefficients from friction map with linear equations or gaussian basis functions
if pars["optim_opts"]["var_friction"] is not None:
w_mue_fl, w_mue_fr, w_mue_rl, w_mue_rr, center_dist = opt_mintime_traj.src. \
approx_friction_map.approx_friction_map(reftrack=reftrack,
normvectors=normvectors,
tpamap_path=tpamap_path,
tpadata_path=tpadata_path,
pars=pars,
dn=pars["optim_opts"]["dn"],
n_gauss=pars["optim_opts"]["n_gauss"],
print_debug=print_debug,
plot_debug=plot_debug)
# ------------------------------------------------------------------------------------------------------------------
# DIRECT GAUSS-LEGENDRE COLLOCATION --------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# degree of interpolating polynomial
d = 3
# legendre collocation points
tau = np.append(0, ca.collocation_points(d, 'legendre'))
# coefficient matrix for formulating the collocation equation
C = np.zeros((d + 1, d + 1))
# coefficient matrix for formulating the collocation equation
D = np.zeros(d + 1)
# coefficient matrix for formulating the collocation equation
B = np.zeros(d + 1)
# construct polynomial basis
for j in range(d + 1):
# construct Lagrange polynomials to get the polynomial basis at the collocation point
p = np.poly1d([1])
for r in range(d + 1):
if r != j:
p *= np.poly1d([1, -tau[r]]) / (tau[j] - tau[r])
# evaluate polynomial at the final time to get the coefficients of the continuity equation
D[j] = p(1.0)
# evaluate time derivative of polynomial at collocation points to get the coefficients of continuity equation
p_der = np.polyder(p)
for r in range(d + 1):
C[j, r] = p_der(tau[r])
# evaluate integral of the polynomial to get the coefficients of the quadrature function
pint = np.polyint(p)
B[j] = pint(1.0)
# ------------------------------------------------------------------------------------------------------------------
# STATE VARIABLES --------------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# number of state variables
nx = 5
# velocity [m/s]
v_n = ca.SX.sym('v_n')
v_s = 50
v = v_s * v_n
# side slip angle [rad]
beta_n = ca.SX.sym('beta_n')
beta_s = 0.5
beta = beta_s * beta_n
# yaw rate [rad/s]
omega_z_n = ca.SX.sym('omega_z_n')
omega_z_s = 1
omega_z = omega_z_s * omega_z_n
# lateral distance to reference line (positive = left) [m]
n_n = ca.SX.sym('n_n')
n_s = 5.0
n = n_s * n_n
# relative angle to tangent on reference line [rad]
xi_n = ca.SX.sym('xi_n')
xi_s = 1.0
xi = xi_s * xi_n
# scaling factors for state variables
x_s = np.array([v_s, beta_s, omega_z_s, n_s, xi_s])
# put all states together
x = ca.vertcat(v_n, beta_n, omega_z_n, n_n, xi_n)
# ------------------------------------------------------------------------------------------------------------------
# CONTROL VARIABLES ------------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# number of control variables
nu = 4
# steer angle [rad]
delta_n = ca.SX.sym('delta_n')
delta_s = 0.5
delta = delta_s * delta_n
# positive longitudinal force (drive) [N]
f_drive_n = ca.SX.sym('f_drive_n')
f_drive_s = 7500.0
f_drive = f_drive_s * f_drive_n
# negative longitudinal force (brake) [N]
f_brake_n = ca.SX.sym('f_brake_n')
f_brake_s = 20000.0
f_brake = f_brake_s * f_brake_n
# lateral wheel load transfer [N]
gamma_y_n = ca.SX.sym('gamma_y_n')
gamma_y_s = 5000.0
gamma_y = gamma_y_s * gamma_y_n
# scaling factors for control variables
u_s = np.array([delta_s, f_drive_s, f_brake_s, gamma_y_s])
# put all controls together
u = ca.vertcat(delta_n, f_drive_n, f_brake_n, gamma_y_n)
# ------------------------------------------------------------------------------------------------------------------
# MODEL EQUATIONS --------------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# extract vehicle and tire parameters
veh = pars["vehicle_params_mintime"]
tire = pars["tire_params_mintime"]
# general constants
g = pars["veh_params"]["g"]
rho = pars["veh_params"]["rho_air"]
mass = pars["veh_params"]["mass"]
# curvature of reference line [rad/m]
kappa = ca.SX.sym('kappa')
# drag force [N]
f_xdrag = pars["veh_params"]["dragcoeff"] * v**2
# rolling resistance forces [N]
f_xroll_fl = 0.5 * tire["c_roll"] * mass * g * veh["wheelbase_rear"] / veh[
"wheelbase"]
f_xroll_fr = 0.5 * tire["c_roll"] * mass * g * veh["wheelbase_rear"] / veh[
"wheelbase"]
f_xroll_rl = 0.5 * tire["c_roll"] * mass * g * veh[
"wheelbase_front"] / veh["wheelbase"]
f_xroll_rr = 0.5 * tire["c_roll"] * mass * g * veh[
"wheelbase_front"] / veh["wheelbase"]
f_xroll = tire["c_roll"] * mass * g
# static normal tire forces [N]
f_zstat_fl = 0.5 * mass * g * veh["wheelbase_rear"] / veh["wheelbase"]
f_zstat_fr = 0.5 * mass * g * veh["wheelbase_rear"] / veh["wheelbase"]
f_zstat_rl = 0.5 * mass * g * veh["wheelbase_front"] / veh["wheelbase"]
f_zstat_rr = 0.5 * mass * g * veh["wheelbase_front"] / veh["wheelbase"]
# dynamic normal tire forces (aerodynamic downforces) [N]
f_zlift_fl = 0.5 * 0.5 * veh["clA_front"] * rho * v**2
f_zlift_fr = 0.5 * 0.5 * veh["clA_front"] * rho * v**2
f_zlift_rl = 0.5 * 0.5 * veh["clA_rear"] * rho * v**2
f_zlift_rr = 0.5 * 0.5 * veh["clA_rear"] * rho * v**2
# dynamic normal tire forces (load transfers) [N]
f_zdyn_fl = (-0.5 * veh["cog_z"] / veh["wheelbase"] *
(f_drive + f_brake - f_xdrag - f_xroll) -
veh["k_roll"] * gamma_y)
f_zdyn_fr = (-0.5 * veh["cog_z"] / veh["wheelbase"] *
(f_drive + f_brake - f_xdrag - f_xroll) +
veh["k_roll"] * gamma_y)
f_zdyn_rl = (0.5 * veh["cog_z"] / veh["wheelbase"] *
(f_drive + f_brake - f_xdrag - f_xroll) -
(1.0 - veh["k_roll"]) * gamma_y)
f_zdyn_rr = (0.5 * veh["cog_z"] / veh["wheelbase"] *
(f_drive + f_brake - f_xdrag - f_xroll) +
(1.0 - veh["k_roll"]) * gamma_y)
# sum of all normal tire forces [N]
f_z_fl = f_zstat_fl + f_zlift_fl + f_zdyn_fl
f_z_fr = f_zstat_fr + f_zlift_fr + f_zdyn_fr
f_z_rl = f_zstat_rl + f_zlift_rl + f_zdyn_rl
f_z_rr = f_zstat_rr + f_zlift_rr + f_zdyn_rr
# slip angles [rad]
alpha_fl = delta - ca.atan(
(v * ca.sin(beta) + veh["wheelbase_front"] * omega_z) /
(v * ca.cos(beta) - 0.5 * veh["track_width_front"] * omega_z))
alpha_fr = delta - ca.atan(
(v * ca.sin(beta) + veh["wheelbase_front"] * omega_z) /
(v * ca.cos(beta) + 0.5 * veh["track_width_front"] * omega_z))
alpha_rl = ca.atan(
(-v * ca.sin(beta) + veh["wheelbase_rear"] * omega_z) /
(v * ca.cos(beta) - 0.5 * veh["track_width_rear"] * omega_z))
alpha_rr = ca.atan(
(-v * ca.sin(beta) + veh["wheelbase_rear"] * omega_z) /
(v * ca.cos(beta) + 0.5 * veh["track_width_rear"] * omega_z))
# lateral tire forces [N]
f_y_fl = (pars["optim_opts"]["mue"] * f_z_fl *
(1 + tire["eps_front"] * f_z_fl / tire["f_z0"]) *
ca.sin(tire["C_front"] *
ca.atan(tire["B_front"] * alpha_fl - tire["E_front"] *
(tire["B_front"] * alpha_fl -
ca.atan(tire["B_front"] * alpha_fl)))))
f_y_fr = (pars["optim_opts"]["mue"] * f_z_fr *
(1 + tire["eps_front"] * f_z_fr / tire["f_z0"]) *
ca.sin(tire["C_front"] *
ca.atan(tire["B_front"] * alpha_fr - tire["E_front"] *
(tire["B_front"] * alpha_fr -
ca.atan(tire["B_front"] * alpha_fr)))))
f_y_rl = (pars["optim_opts"]["mue"] * f_z_rl *
(1 + tire["eps_rear"] * f_z_rl / tire["f_z0"]) *
ca.sin(tire["C_rear"] *
ca.atan(tire["B_rear"] * alpha_rl - tire["E_rear"] *
(tire["B_rear"] * alpha_rl -
ca.atan(tire["B_rear"] * alpha_rl)))))
f_y_rr = (pars["optim_opts"]["mue"] * f_z_rr *
(1 + tire["eps_rear"] * f_z_rr / tire["f_z0"]) *
ca.sin(tire["C_rear"] *
ca.atan(tire["B_rear"] * alpha_rr - tire["E_rear"] *
(tire["B_rear"] * alpha_rr -
ca.atan(tire["B_rear"] * alpha_rr)))))
# longitudinal tire forces [N]
f_x_fl = 0.5 * f_drive * veh["k_drive_front"] + 0.5 * f_brake * veh[
"k_brake_front"] - f_xroll_fl
f_x_fr = 0.5 * f_drive * veh["k_drive_front"] + 0.5 * f_brake * veh[
"k_brake_front"] - f_xroll_fr
f_x_rl = 0.5 * f_drive * (1 - veh["k_drive_front"]) + 0.5 * f_brake * (
1 - veh["k_brake_front"]) - f_xroll_rl
f_x_rr = 0.5 * f_drive * (1 - veh["k_drive_front"]) + 0.5 * f_brake * (
1 - veh["k_brake_front"]) - f_xroll_rr
# longitudinal acceleration [m/s²]
ax = (f_x_rl + f_x_rr + (f_x_fl + f_x_fr) * ca.cos(delta) -
(f_y_fl + f_y_fr) * ca.sin(delta) -
pars["veh_params"]["dragcoeff"] * v**2) / mass
# lateral acceleration [m/s²]
ay = ((f_x_fl + f_x_fr) * ca.sin(delta) + f_y_rl + f_y_rr +
(f_y_fl + f_y_fr) * ca.cos(delta)) / mass
# time-distance scaling factor (dt/ds)
sf = (1.0 - n * kappa) / (v * (ca.cos(xi + beta)))
# model equations for two track model (ordinary differential equations)
dv = (sf / mass) * (
(f_x_rl + f_x_rr) * ca.cos(beta) +
(f_x_fl + f_x_fr) * ca.cos(delta - beta) +
(f_y_rl + f_y_rr) * ca.sin(beta) -
(f_y_fl + f_y_fr) * ca.sin(delta - beta) - f_xdrag * ca.cos(beta))
dbeta = sf * (
-omega_z +
(-(f_x_rl + f_x_rr) * ca.sin(beta) +
(f_x_fl + f_x_fr) * ca.sin(delta - beta) +
(f_y_rl + f_y_rr) * ca.cos(beta) +
(f_y_fl + f_y_fr) * ca.cos(delta - beta) + f_xdrag * ca.sin(beta)) /
(mass * v))
domega_z = (sf / veh["I_z"]) * (
(f_x_rr - f_x_rl) * veh["track_width_rear"] / 2 -
(f_y_rl + f_y_rr) * veh["wheelbase_rear"] +
((f_x_fr - f_x_fl) * ca.cos(delta) +
(f_y_fl - f_y_fr) * ca.sin(delta)) * veh["track_width_front"] / 2 +
((f_y_fl + f_y_fr) * ca.cos(delta) +
(f_x_fl + f_x_fr) * ca.sin(delta)) * veh["track_width_front"])
dn = sf * v * ca.sin(xi + beta)
dxi = sf * omega_z - kappa
# put all ODEs together
dx = ca.vertcat(dv, dbeta, domega_z, dn, dxi) / x_s
# ------------------------------------------------------------------------------------------------------------------
# CONTROL BOUNDARIES -----------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
delta_min = -veh["delta_max"] / delta_s # min. steer angle [rad]
delta_max = veh["delta_max"] / delta_s # max. steer angle [rad]
f_drive_min = 0.0 # min. longitudinal drive force [N]
f_drive_max = veh[
"f_drive_max"] / f_drive_s # max. longitudinal drive force [N]
f_brake_min = -veh[
"f_brake_max"] / f_brake_s # min. longitudinal brake force [N]
f_brake_max = 0.0 # max. longitudinal brake force [N]
gamma_y_min = -np.inf # min. lateral wheel load transfer [N]
gamma_y_max = np.inf # max. lateral wheel load transfer [N]
# ------------------------------------------------------------------------------------------------------------------
# STATE BOUNDARIES -------------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
v_min = 1.0 / v_s # min. velocity [m/s]
v_max = pars["optim_opts"]["v_max"] / v_s # max. velocity [m/s]
beta_min = -0.5 * np.pi / beta_s # min. side slip angle [rad]
beta_max = 0.5 * np.pi / beta_s # max. side slip angle [rad]
omega_z_min = -0.5 * np.pi / omega_z_s # min. yaw rate [rad/s]
omega_z_max = 0.5 * np.pi / omega_z_s # max. yaw rate [rad/s]
xi_min = -0.5 * np.pi / xi_s # min. relative angle to tangent on reference line [rad]
xi_max = 0.5 * np.pi / xi_s # max. relative angle to tangent on reference line [rad]
# ------------------------------------------------------------------------------------------------------------------
# INITIAL GUESS FOR DECISION VARIABLES -----------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
v_guess = 20.0 / v_s
# ------------------------------------------------------------------------------------------------------------------
# HELPER FUNCTIONS -------------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# continuous time dynamics
f_dyn = ca.Function('f_dyn', [x, u, kappa], [dx, sf], ['x', 'u', 'kappa'],
['dx', 'sf'])
# longitudinal tire forces [N]
f_fx = ca.Function('f_fx', [x, u], [f_x_fl, f_x_fr, f_x_rl, f_x_rr],
['x', 'u'], ['f_x_fl', 'f_x_fr', 'f_x_rl', 'f_x_rr'])
# lateral tire forces [N]
f_fy = ca.Function('f_fy', [x, u], [f_y_fl, f_y_fr, f_y_rl, f_y_rr],
['x', 'u'], ['f_y_fl', 'f_y_fr', 'f_y_rl', 'f_y_rr'])
# vertical tire forces [N]
f_fz = ca.Function('f_fz', [x, u], [f_z_fl, f_z_fr, f_z_rl, f_z_rr],
['x', 'u'], ['f_z_fl', 'f_z_fr', 'f_z_rl', 'f_z_rr'])
# longitudinal and lateral acceleration [m/s²]
f_a = ca.Function('f_a', [x, u], [ax, ay], ['x', 'u'], ['ax', 'ay'])
# ------------------------------------------------------------------------------------------------------------------
# FORMULATE NONLINEAR PROGRAM --------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# initialize NLP vectors
w = []
w0 = []
lbw = []
ubw = []
J = 0
g = []
lbg = []
ubg = []
# initialize ouput vectors
x_opt = []
u_opt = []
dt_opt = []
tf_opt = []
ax_opt = []
ay_opt = []
ec_opt = []
# initialize control vectors (for regularization)
delta_p = []
F_p = []
# boundary constraint: lift initial conditions
Xk = ca.MX.sym('X0', nx)
w.append(Xk)
n_min = (-w_tr_right_interp(0) + pars["optim_opts"]["width_opt"] / 2) / n_s
n_max = (w_tr_left_interp(0) - pars["optim_opts"]["width_opt"] / 2) / n_s
lbw.append([v_min, beta_min, omega_z_min, n_min, xi_min])
ubw.append([v_max, beta_max, omega_z_max, n_max, xi_max])
w0.append([v_guess, 0.0, 0.0, 0.0, 0.0])
x_opt.append(Xk * x_s)
# loop along the racetrack and formulate path constraints & system dynamic
for k in range(N):
# add decision variables for the control
Uk = ca.MX.sym('U_' + str(k), nu)
w.append(Uk)
lbw.append([delta_min, f_drive_min, f_brake_min, gamma_y_min])
ubw.append([delta_max, f_drive_max, f_brake_max, gamma_y_max])
w0.append([0.0] * nu)
# add decision variables for the state at collocation points
Xc = []
for j in range(d):
Xkj = ca.MX.sym('X_' + str(k) + '_' + str(j), nx)
Xc.append(Xkj)
w.append(Xkj)
lbw.append([-np.inf] * nx)
ubw.append([np.inf] * nx)
w0.append([v_guess, 0.0, 0.0, 0.0, 0.0])
# loop over all collocation points
Xk_end = D[0] * Xk
sf_opt = []
for j in range(1, d + 1):
# calculate the state derivative at the collocation point
xp = C[0, j] * Xk
for r in range(d):
xp = xp + C[r + 1, j] * Xc[r]
# interpolate kappa at the collocation point
kappa_col = kappa_interp(k + tau[j])
# append collocation equations (system dynamic)
fj, qj = f_dyn(Xc[j - 1], Uk, kappa_col)
g.append(h * fj - xp)
lbg.append([0.0] * nx)
ubg.append([0.0] * nx)
# add contribution to the end state
Xk_end = Xk_end + D[j] * Xc[j - 1]
# add contribution to quadrature function
J = J + B[j] * qj * h
# add contribution to scaling factor (for calculating lap time)
sf_opt.append(B[j] * qj * h)
# calculate used energy
dt_opt.append(sf_opt[0] + sf_opt[1] + sf_opt[2])
ec_opt.append(Xk[0] * v_s * Uk[1] * f_drive_s * dt_opt[-1])
# add new decision variables for state at end of the collocation interval
Xk = ca.MX.sym('X_' + str(k + 1), nx)
w.append(Xk)
n_min = (-w_tr_right_interp(k + 1) +
pars["optim_opts"]["width_opt"] / 2.0) / n_s
n_max = (w_tr_left_interp(k + 1) -
pars["optim_opts"]["width_opt"] / 2.0) / n_s
lbw.append([v_min, beta_min, omega_z_min, n_min, xi_min])
ubw.append([v_max, beta_max, omega_z_max, n_max, xi_max])
w0.append([v_guess, 0.0, 0.0, 0.0, 0.0])
# add equality constraint
g.append(Xk_end - Xk)
lbg.append([0.0] * nx)
ubg.append([0.0] * nx)
# get tire forces
f_x_flk, f_x_frk, f_x_rlk, f_x_rrk = f_fx(Xk, Uk)
f_y_flk, f_y_frk, f_y_rlk, f_y_rrk = f_fy(Xk, Uk)
f_z_flk, f_z_frk, f_z_rlk, f_z_rrk = f_fz(Xk, Uk)
# get accelerations (longitudinal + lateral)
axk, ayk = f_a(Xk, Uk)
# path constraint: limitied engine power
g.append(Xk[0] * Uk[1])
lbg.append([-np.inf])
ubg.append([veh["power_max"] / (f_drive_s * v_s)])
# get constant friction coefficient
if pars["optim_opts"]["var_friction"] is None:
mue_fl = pars["optim_opts"]["mue"]
mue_fr = pars["optim_opts"]["mue"]
mue_rl = pars["optim_opts"]["mue"]
mue_rr = pars["optim_opts"]["mue"]
# calculate variable friction coefficients along the reference line (regression with linear equations)
elif pars["optim_opts"]["var_friction"] == "linear":
# friction coefficient for each tire
mue_fl = w_mue_fl[k + 1, 0] * Xk[3] * n_s + w_mue_fl[k + 1, 1]
mue_fr = w_mue_fr[k + 1, 0] * Xk[3] * n_s + w_mue_fr[k + 1, 1]
mue_rl = w_mue_rl[k + 1, 0] * Xk[3] * n_s + w_mue_rl[k + 1, 1]
mue_rr = w_mue_rr[k + 1, 0] * Xk[3] * n_s + w_mue_rr[k + 1, 1]
# calculate variable friction coefficients along the reference line (regression with gaussian basis functions)
elif pars["optim_opts"]["var_friction"] == "gauss":
# gaussian basis functions
sigma = 2.0 * center_dist[k + 1, 0]
n_gauss = pars["optim_opts"]["n_gauss"]
n_q = np.linspace(-n_gauss, n_gauss,
2 * n_gauss + 1) * center_dist[k + 1, 0]
gauss_basis = []
for i in range(2 * n_gauss + 1):
gauss_basis.append(
ca.exp(-(Xk[3] * n_s - n_q[i])**2 / (2 * (sigma**2))))
gauss_basis = ca.vertcat(*gauss_basis)
mue_fl = ca.dot(w_mue_fl[k + 1, :-1],
gauss_basis) + w_mue_fl[k + 1, -1]
mue_fr = ca.dot(w_mue_fr[k + 1, :-1],
gauss_basis) + w_mue_fr[k + 1, -1]
mue_rl = ca.dot(w_mue_rl[k + 1, :-1],
gauss_basis) + w_mue_rl[k + 1, -1]
mue_rr = ca.dot(w_mue_rr[k + 1, :-1],
gauss_basis) + w_mue_rr[k + 1, -1]
else:
raise ValueError("No friction coefficients are available!")
# path constraint: Kamm's Circle for each wheel
g.append(((f_x_flk / (mue_fl * f_z_flk))**2 + (f_y_flk /
(mue_fl * f_z_flk))**2))
g.append(((f_x_frk / (mue_fr * f_z_frk))**2 + (f_y_frk /
(mue_fr * f_z_frk))**2))
g.append(((f_x_rlk / (mue_rl * f_z_rlk))**2 + (f_y_rlk /
(mue_rl * f_z_rlk))**2))
g.append(((f_x_rrk / (mue_rr * f_z_rrk))**2 + (f_y_rrk /
(mue_rr * f_z_rrk))**2))
lbg.append([0.0] * 4)
ubg.append([1.0] * 4)
# path constraint: lateral wheel load transfer
g.append((
(f_y_flk + f_y_frk) * ca.cos(Uk[0] * delta_s) + f_y_rlk + f_y_rrk +
(f_x_flk + f_x_frk) * ca.sin(Uk[0] * delta_s)) * veh["cog_z"] /
((veh["track_width_front"] + veh["track_width_rear"]) / 2) -
Uk[3] * gamma_y_s)
lbg.append([0.0])
ubg.append([0.0])
# path constraint: f_drive * f_brake == 0 (no simultaneous operation of brake and accelerator pedal)
g.append(Uk[1] * Uk[2])
lbg.append([-20000.0 / (f_drive_s * f_brake_s)])
ubg.append([0.0])
# path constraint: actor dynamic
if k > 0:
sigma = (1 - kappa_interp(k) * Xk[3] * n_s) / (Xk[0] * v_s)
g.append((Uk - w[1 + (k - 1) * nx]) /
(pars["stepsize_opts"]["stepsize_reg"] * sigma))
lbg.append([
delta_min / (veh["t_delta"]), -np.inf,
f_brake_min / (veh["t_brake"]), -np.inf
])
ubg.append([
delta_max / (veh["t_delta"]), f_drive_max / (veh["t_drive"]),
np.inf, np.inf
])
# path constraint: safe trajectories with acceleration ellipse
if pars["optim_opts"]["safe_traj"]:
g.append((ca.fmax(axk, 0) / pars["optim_opts"]["ax_pos_safe"])**2 +
(ayk / pars["optim_opts"]["ay_safe"])**2)
g.append((ca.fmin(axk, 0) / pars["optim_opts"]["ax_neg_safe"])**2 +
(ayk / pars["optim_opts"]["ay_safe"])**2)
lbg.append([0.0] * 2)
ubg.append([1.0] * 2)
# append controls (for regularization)
delta_p.append(Uk[0] * delta_s)
F_p.append(Uk[1] * f_drive_s / 10000.0 + Uk[2] * f_brake_s / 10000.0)
# append outputs
x_opt.append(Xk * x_s)
u_opt.append(Uk * u_s)
tf_opt.extend([f_x_flk, f_y_flk, f_z_flk, f_x_frk, f_y_frk, f_z_frk])
tf_opt.extend([f_x_rlk, f_y_rlk, f_z_rlk, f_x_rrk, f_y_rrk, f_z_rrk])
ax_opt.append(axk)
ay_opt.append(ayk)
# boundary constraint: start states = final states
g.append(w[0] - Xk)
lbg.append([0.0] * nx)
ubg.append([0.0] * nx)
# path constraint: limited energy consumption
if pars["optim_opts"]["limit_energy"]:
g.append(ca.sum1(ca.vertcat(*ec_opt)) / 3600000.0)
lbg.append([0])
ubg.append([pars["optim_opts"]["energy_limit"]])
# formulate differentiation matrix (for regularization)
diff_matrix = np.eye(N)
for i in range(N - 1):
diff_matrix[i, i + 1] = -1.0
diff_matrix[N - 1, 0] = -1.0
# regularization (delta)
delta_p = ca.vertcat(*delta_p)
Jp_delta = ca.mtimes(ca.MX(diff_matrix), delta_p)
Jp_delta = ca.dot(Jp_delta, Jp_delta)
# regularization (f_drive + f_brake)
F_p = ca.vertcat(*F_p)
Jp_f = ca.mtimes(ca.MX(diff_matrix), F_p)
Jp_f = ca.dot(Jp_f, Jp_f)
# formulate objective
J = J + pars["optim_opts"]["penalty_F"] * Jp_f + pars["optim_opts"][
"penalty_delta"] * Jp_delta
# concatenate NLP vectors
w = ca.vertcat(*w)
g = ca.vertcat(*g)
w0 = np.concatenate(w0)
lbw = np.concatenate(lbw)
ubw = np.concatenate(ubw)
lbg = np.concatenate(lbg)
ubg = np.concatenate(ubg)
# concatenate output vectors
x_opt = ca.vertcat(*x_opt)
u_opt = ca.vertcat(*u_opt)
tf_opt = ca.vertcat(*tf_opt)
dt_opt = ca.vertcat(*dt_opt)
ax_opt = ca.vertcat(*ax_opt)
ay_opt = ca.vertcat(*ay_opt)
ec_opt = ca.vertcat(*ec_opt)
# ------------------------------------------------------------------------------------------------------------------
# CREATE NLP SOLVER ------------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# fill nlp structure
nlp = {'f': J, 'x': w, 'g': g}
# solver options
opts = {
"expand": True,
"verbose": print_debug,
"ipopt.max_iter": 2000,
"ipopt.tol": 1e-7
}
# solver options for warm start
if pars["optim_opts"]["warm_start"]:
opts_warm_start = {
"ipopt.warm_start_init_point": "yes",
"ipopt.warm_start_bound_push": 1e-9,
"ipopt.warm_start_mult_bound_push": 1e-9,
"ipopt.warm_start_slack_bound_push": 1e-9,
"ipopt.mu_init": 1e-9
}
opts.update(opts_warm_start)
# load warm start files
if pars["optim_opts"]["warm_start"]:
try:
w0 = np.loadtxt(os.path.join(export_path, 'w0.csv'))
lam_x0 = np.loadtxt(os.path.join(export_path, 'lam_x0.csv'))
lam_g0 = np.loadtxt(os.path.join(export_path, 'lam_g0.csv'))
except IOError:
print('\033[91m' + 'WARNING: Failed to load warm start files!' +
'\033[0m')
sys.exit(1)
# check warm start files
if pars["optim_opts"]["warm_start"] and not len(w0) == len(lbw):
print(
'\033[91m' +
'WARNING: Warm start files do not fit to the dimension of the NLP!'
+ '\033[0m')
sys.exit(1)
# create solver instance
solver = ca.nlpsol("solver", "ipopt", nlp, opts)
# ------------------------------------------------------------------------------------------------------------------
# SOLVE NLP --------------------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# start time measure
t0 = time.perf_counter()
# solve NLP
if pars["optim_opts"]["warm_start"]:
sol = solver(x0=w0,
lbx=lbw,
ubx=ubw,
lbg=lbg,
ubg=ubg,
lam_x0=lam_x0,
lam_g0=lam_g0)
else:
sol = solver(x0=w0, lbx=lbw, ubx=ubw, lbg=lbg, ubg=ubg)
# end time measure
tend = time.perf_counter()
# ------------------------------------------------------------------------------------------------------------------
# EXTRACT SOLUTION -------------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# helper function to extract solution for state variables, control variables, tire forces, time
f_sol = ca.Function(
'f_sol', [w], [x_opt, u_opt, tf_opt, dt_opt, ax_opt, ay_opt, ec_opt],
['w'],
['x_opt', 'u_opt', 'tf_opt', 'dt_opt', 'ax_opt', 'ay_opt', 'ec_opt'])
# extract solution
x_opt, u_opt, tf_opt, dt_opt, ax_opt, ay_opt, ec_opt = f_sol(sol['x'])
# solution for state variables
x_opt = np.reshape(x_opt, (N + 1, nx))
# solution for control variables
u_opt = np.reshape(u_opt, (N, nu))
# solution for tire forces
tf_opt = np.append(tf_opt[-12:], tf_opt[:])
tf_opt = np.reshape(tf_opt, (N + 1, 12))
# solution for time
t_opt = np.hstack((0.0, np.cumsum(dt_opt)))
# solution for acceleration
ax_opt = np.append(ax_opt[-1], ax_opt)
ay_opt = np.append(ay_opt[-1], ay_opt)
atot_opt = np.sqrt(np.power(ax_opt, 2) + np.power(ay_opt, 2))
# solution for energy consumption
ec_opt_cum = np.hstack((0.0, np.cumsum(ec_opt))) / 3600.0
# ------------------------------------------------------------------------------------------------------------------
# EXPORT SOLUTION --------------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# export data to CSVs
opt_mintime_traj.src.export_mintime_solution.export_mintime_solution(
file_path=export_path,
s=s_opt,
t=t_opt,
x=x_opt,
u=u_opt,
tf=tf_opt,
ax=ax_opt,
ay=ay_opt,
atot=atot_opt,
w0=sol["x"],
lam_x0=sol["lam_x"],
lam_g0=sol["lam_g"])
# ------------------------------------------------------------------------------------------------------------------
# PLOT & PRINT RESULTS ---------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
if plot_debug:
opt_mintime_traj.src.result_plots_mintime.result_plots_mintime(
pars=pars,
reftrack=reftrack,
s=s_opt,
t=t_opt,
x=x_opt,
u=u_opt,
ax=ax_opt,
ay=ay_opt,
atot=atot_opt,
tf=tf_opt,
ec=ec_opt_cum)
if print_debug:
print("INFO: Laptime: %.3fs" % t_opt[-1])
print("INFO: NLP solving time: %.3fs" % (tend - t0))
print("INFO: Maximum abs(ay): %.2fm/s2" % np.amax(ay_opt))
print("INFO: Maximum ax: %.2fm/s2" % np.amax(ax_opt))
print("INFO: Minimum ax: %.2fm/s2" % np.amin(ax_opt))
print("INFO: Maximum total acc: %.2fm/s2" % np.amax(atot_opt))
print('INFO: Energy consumption: %.3fWh' % ec_opt_cum[-1])
return -x_opt[:-1, 3], x_opt[:-1, 0]
S = ca.nlpsol('S', 'ipopt', nlp)
n0 = ones * 0
# n0 = np.random.uniform(-1, 1, N)
th0 = []
for i in range(N - 1):
th_00 = lib.get_bearing(track[i, 0:2], track[i + 1, 0:2])
th0.append(th_00)
th0.append(0)
th0 = np.array(th0)
# calculate this afterwards
d0 = ca.atan(l * (th0[1:] - th0[:-1]) / (ones * 2)[:-1])
d0 = np.insert(np.array(d0), -1, 0)
v0, a0 = [1], []
for i in range(N - 1):
if v0[-1] < 3:
a0.append(0.2)
else:
a0.append(0)
new_v = np.sqrt(v0[-1]**2 + 2 * a0[-1] * sls[i])
v0.append(new_v)
a0.append(0) # last a
t0 = [0]
for i in range(N - 1):
new_t = sls[i] / v0[i] # t0[-1]
def MinCurvature():
track = run_map_gen()
txs = track[:, 0]
tys = track[:, 1]
nvecs = track[:, 2:4]
th_ns = [lib.get_bearing([0, 0], nvecs[i, 0:2]) for i in range(len(nvecs))]
# sls = np.sqrt(np.sum(np.power(np.diff(track[:, :2], axis=0), 2), axis=1))
n_max = 3
N = len(track)
n_f_a = ca.MX.sym('n_f', N)
n_f = ca.MX.sym('n_f', N - 1)
th_f_a = ca.MX.sym('n_f', N)
th_f = ca.MX.sym('n_f', N - 1)
x0_f = ca.MX.sym('x0_f', N - 1)
x1_f = ca.MX.sym('x1_f', N - 1)
y0_f = ca.MX.sym('y0_f', N - 1)
y1_f = ca.MX.sym('y1_f', N - 1)
th1_f = ca.MX.sym('y1_f', N - 1)
th2_f = ca.MX.sym('y1_f', N - 1)
th1_f1 = ca.MX.sym('y1_f', N - 2)
th2_f1 = ca.MX.sym('y1_f', N - 2)
o_x_s = ca.Function('o_x', [n_f], [track[:-1, 0] + nvecs[:-1, 0] * n_f])
o_y_s = ca.Function('o_y', [n_f], [track[:-1, 1] + nvecs[:-1, 1] * n_f])
o_x_e = ca.Function('o_x', [n_f], [track[1:, 0] + nvecs[1:, 0] * n_f])
o_y_e = ca.Function('o_y', [n_f], [track[1:, 1] + nvecs[1:, 1] * n_f])
dis = ca.Function('dis', [x0_f, x1_f, y0_f, y1_f],
[ca.sqrt((x1_f - x0_f)**2 + (y1_f - y0_f)**2)])
track_length = ca.Function('length', [n_f_a], [
dis(o_x_s(n_f_a[:-1]), o_x_e(n_f_a[1:]), o_y_s(n_f_a[:-1]),
o_y_e(n_f_a[1:]))
])
real = ca.Function(
'real', [th1_f, th2_f],
[ca.cos(th1_f) * ca.cos(th2_f) + ca.sin(th1_f) * ca.sin(th2_f)])
im = ca.Function(
'im', [th1_f, th2_f],
[-ca.cos(th1_f) * ca.sin(th2_f) + ca.sin(th1_f) * ca.cos(th2_f)])
sub_cmplx = ca.Function('a_cpx', [th1_f, th2_f],
[ca.atan(im(th1_f, th2_f) / real(th1_f, th2_f))])
get_th_n = ca.Function(
'gth', [th_f],
[sub_cmplx(ca.pi * np.ones(N - 1), sub_cmplx(th_f, th_ns[:-1]))])
real1 = ca.Function(
'real', [th1_f1, th2_f1],
[ca.cos(th1_f1) * ca.cos(th2_f1) + ca.sin(th1_f1) * ca.sin(th2_f1)])
im1 = ca.Function(
'im', [th1_f1, th2_f1],
[-ca.cos(th1_f1) * ca.sin(th2_f1) + ca.sin(th1_f1) * ca.cos(th2_f1)])
sub_cmplx1 = ca.Function(
'a_cpx', [th1_f1, th2_f1],
[ca.atan(im1(th1_f1, th2_f1) / real1(th1_f1, th2_f1))])
d_n = ca.Function('d_n', [n_f_a, th_f],
[track_length(n_f_a) / ca.tan(get_th_n(th_f))])
# curvature = ca.Function('curv', [th_f_a], [th_f_a[1:] - th_f_a[:-1]])
grad = ca.Function(
'grad', [n_f_a],
[(o_y_e(n_f_a[1:]) - o_y_s(n_f_a[:-1])) /
ca.fmax(o_x_e(n_f_a[1:]) - o_x_s(n_f_a[:-1]), 0.1 * np.ones(N - 1))])
curvature = ca.Function(
'curv', [n_f_a],
[grad(n_f_a)[1:] - grad(n_f_a)[:-1]]) # changes in grad
# define symbols
n = ca.MX.sym('n', N)
th = ca.MX.sym('th', N)
nlp = {\
'x': ca.vertcat(n, th),
# 'f': ca.sumsqr(curvature(n)),
# 'f': ca.sumsqr(sub_cmplx(th[1:], th[:-1])),
'f': ca.sumsqr(sub_cmplx1(sub_cmplx1(th[2:], th[1:-1]), sub_cmplx1(th[1:-1], th[:-2]))),
# 'f': ca.sumsqr(track_length(n)),
'g': ca.vertcat(
# dynamic constraints
n[1:] - (n[:-1] + d_n(n, th[:-1])),
# boundary constraints
n[0], th[0],
n[-1], #th[-1],
) \
}
S = ca.nlpsol('S', 'ipopt', nlp)
ones = np.ones(N)
n0 = ones * 0
th0 = []
for i in range(N - 1):
th_00 = lib.get_bearing(track[i, 0:2], track[i + 1, 0:2])
th0.append(th_00)
th0.append(0)
th0 = np.array(th0)
x0 = ca.vertcat(n0, th0)
lbx = [-n_max] * N + [-np.pi] * N
ubx = [n_max] * N + [np.pi] * N
print(curvature(n0))
r = S(x0=x0, lbg=0, ubg=0, lbx=lbx, ubx=ubx)
print(f"Solution found")
x_opt = r['x']
n_set = np.array(x_opt[:N])
thetas = np.array(x_opt[1 * N:2 * N])
# print(sub_cmplx(thetas[1:], thetas[:-1]))
plt.figure(2)
th_data = sub_cmplx(thetas[1:], thetas[:-1])
plt.plot(th_data)
plt.plot(ca.fabs(th_data), '--')
plt.pause(0.001)
plt.figure(3)
plt.plot(abs(thetas), '--')
plt.plot(thetas)
plt.pause(0.001)
plt.figure(4)
plt.plot(sub_cmplx(th0[1:], th0[:-1]))
plt.pause(0.001)
plot_race_line(np.array(track), n_set, width=3, wait=True)
def MinCurvatureSteer():
track = run_map_gen()
txs = track[:, 0]
tys = track[:, 1]
nvecs = track[:, 2:4]
th_ns = [lib.get_bearing([0, 0], nvecs[i, 0:2]) for i in range(len(nvecs))]
sls = np.sqrt(np.sum(np.power(np.diff(track[:, :2], axis=0), 2), axis=1))
l = 0.33
a_max = 7.5
d_max = 0.2
v_max = 6
n_max = 3
N = len(track)
n_f_a = ca.MX.sym('n_f', N)
n_f = ca.MX.sym('n_f', N - 1)
d_f = ca.MX.sym('d_f', N - 1)
th_f = ca.MX.sym('th_f', N - 1)
v_f = ca.MX.sym('v_f', N - 1)
x0_f = ca.MX.sym('x0_f', N - 1)
x1_f = ca.MX.sym('x1_f', N - 1)
y0_f = ca.MX.sym('y0_f', N - 1)
y1_f = ca.MX.sym('y1_f', N - 1)
th1_f = ca.MX.sym('th_f', N - 1)
th2_f = ca.MX.sym('th_f', N - 1)
o_x_s = ca.Function('o_x', [n_f], [track[:-1, 0] + nvecs[:-1, 0] * n_f])
o_y_s = ca.Function('o_y', [n_f], [track[:-1, 1] + nvecs[:-1, 1] * n_f])
o_x_e = ca.Function('o_x', [n_f], [track[1:, 0] + nvecs[1:, 0] * n_f])
o_y_e = ca.Function('o_y', [n_f], [track[1:, 1] + nvecs[1:, 1] * n_f])
dis = ca.Function('dis', [x0_f, x1_f, y0_f, y1_f],
[ca.sqrt((x1_f - x0_f)**2 + (y1_f - y0_f)**2)])
track_length = ca.Function('length', [n_f_a], [
dis(o_x_s(n_f_a[:-1]), o_x_e(n_f_a[1:]), o_y_s(n_f_a[:-1]),
o_y_e(n_f_a[1:]))
])
# d_n = ca.Function('d_n', [n_f, th_f], [sls/ca.tan(th_ns[:-1] - th_f)])
# sls_f = ca.Function('sls_f', [n_f_a], )
# get_th = ca.Function('gth', [th_f], [th_ns[:-1] - th_f])
# get_th_n = ca.Function('gth', [th_f], [th_f - th_ns[:-1] + (np.pi/2)*np.ones(N-1)])
real = ca.Function(
'real', [th1_f, th2_f],
[ca.cos(th1_f) * ca.cos(th2_f) + ca.sin(th1_f) * ca.sin(th2_f)])
im = ca.Function(
'im', [th1_f, th2_f],
[-ca.cos(th1_f) * ca.sin(th2_f) + ca.sin(th1_f) * ca.cos(th2_f)])
sub_cmplx = ca.Function('a_cpx', [th1_f, th2_f],
[ca.atan(im(th1_f, th2_f) / real(th1_f, th2_f))])
get_th_n = ca.Function('gth', [th_f], [sub_cmplx(th_f, th_ns[:-1])])
d_n = ca.Function('d_n', [n_f_a, th_f],
[track_length(n_f_a) / ca.tan(get_th_n(th_f))])
d_t = ca.Function('d_t', [n_f_a, v_f], [
dis(o_x_e(n_f_a[1:]), o_x_s(n_f_a[:-1]), o_y_e(n_f_a[1:]),
o_y_s(n_f_a[:-1])) / v_f
])
d_th = ca.Function('d_th', [v_f, d_f], [v_f / l * ca.tan(d_f)])
# define symbols
n = ca.MX.sym('n', N)
dt = ca.MX.sym('t', N)
v = ca.MX.sym('v', N)
th = ca.MX.sym('th', N)
a = ca.MX.sym('a', N)
d = ca.MX.sym('d', N)
nlp = {\
'x': ca.vertcat(n, dt, v, th, a, d),
# 'f': ca.sumsqr(curvature(n)),
# 'f': ca.sumsqr(track_length(n)),
# 'f': ca.sumsqr(d) - ca.sumsqr(v),
# 'f': ca.sumsqr(sub_cmplx(th[1:], th[:-1])),
'f': ca.sum1(dt),
# 'f': - ca.sumsqr(v),
'g': ca.vertcat(
# dynamic constraints
n[1:] - (n[:-1] + d_n(n, th[:-1])),
th[1:] - (th[:-1] + d_th(v[:-1], d[:-1]) * (dt[:-1])),
v[1:] - (v[:-1] + a[:-1] * dt[:-1]),
dt[:-1] - (d_t(n, v[:-1])),
# boundary constraints
n[0], v[0] - 1,
n[-1], th[-1], d[-1], dt[-1]
) \
}
S = ca.nlpsol('S', 'ipopt', nlp)
ones = np.ones(N)
n0 = ones * 0
th0 = []
for i in range(N - 1):
th_00 = lib.get_bearing(track[i, 0:2], track[i + 1, 0:2])
th0.append(th_00)
th0.append(0)
th0 = np.array(th0)
d0 = []
for i in range(N - 1):
angle = lib.sub_angles_complex(th0[i + 1], th0[i])
d00 = ca.atan(l * (angle) / 3)
d0.append(d00)
d0.append(0)
d0 = np.array(d0)
v0, a0 = [1], []
for i in range(N - 1):
if v0[-1] < 3:
a0.append(0.2)
else:
a0.append(0)
new_v = np.sqrt(v0[-1]**2 + 2 * a0[-1] * sls[i])
v0.append(new_v)
a0.append(0) # last a
t0 = [0]
for i in range(N - 1):
new_t = sls[i] / v0[i] # t0[-1]
t0.append(new_t)
x0 = ca.vertcat(n0, t0, v0, th0, a0, d0)
lbx = [-n_max] * N + [0] * N + [0] * N + [-np.pi] * N + [-a_max] * N + [
-d_max
] * N
ubx = [n_max] * N + [np.inf] * N + [v_max] * N + [np.pi] * N + [
a_max
] * N + [d_max] * N
# plot_x_opt(x0, track)
r = S(x0=x0, lbg=0, ubg=0, lbx=lbx, ubx=ubx)
print(f"Solution found")
x_opt = r['x']
plt.figure(7)
plt.plot(track_length(x_opt[:N]))
plot_x_opt(x_opt, track)
def log(cls, r):
assert r.shape == (4, 1) or r.shape == (4, )
n = ca.norm_2(r[:3])
return ca.if_else(n > eps, 4 * ca.atan(n) * r[:3] / n, ca.SX([0, 0,
0]))
def __init__(self, modelparams, Ts, x0, nodes):
with open(modelparams) as file:
params = yaml.load(file, Loader=yaml.FullLoader)
self.xvars = [
'posx', 'posy', 'phi', 'vx', 'vy', 'omega', 'd', 'delta', 'theta'
]
self.uvars = ['ddot', 'deltadot', 'thetadot']
m = params['m'] #[kg]
lf = params['lf'] #[m]
lr = params['lr'] #[m]
Iz = params['Iz'] #[kg*m^3]
lencar = lf + lr
widthcar = lencar / 2
#pajecka and motor coefficients
Bf = params['Bf']
Br = params['Br']
Cf = params['Cf']
Cr = params['Cr']
Cm1 = params['Cm1']
Cm2 = params['Cm2']
Croll = params['Croll']
Cd = params['Cd']
Df = params['Df']
Dr = params['Dr']
x = casadi.SX.sym("x", len(self.xvars))
u = casadi.SX.sym("u", len(self.uvars))
#extract states and inputs
posx = x[self.xvars.index('posx')]
posy = x[self.xvars.index('posy')]
phi = x[self.xvars.index('phi')]
vx = x[self.xvars.index('vx')]
vy = x[self.xvars.index('vy')]
omega = x[self.xvars.index('omega')]
d = x[self.xvars.index('d')]
delta = x[self.xvars.index('delta')]
theta = x[self.xvars.index('theta')]
ddot = u[self.uvars.index('ddot')]
deltadot = u[self.uvars.index('deltadot')]
thetadot = u[self.uvars.index('thetadot')]
#build CasADi expressions for dynamic model
#front lateral tireforce
alphaf = -casadi.atan2((omega * lf + vy), vx) + delta
Ffy = Df * casadi.sin(Cf * casadi.atan(Bf * alphaf))
#rear lateral tireforce
alphar = casadi.atan2((omega * lr - vy), vx)
Fry = Dr * casadi.sin(Cr * casadi.atan(Br * alphar))
#rear longitudinal forces
Frx = (Cm1 - Cm2 * vx) * d - Croll - Cd * vx * vx
#let z = [u, x] = [ddot, deltadot, thetadot, posx, posy, phi, vx, vy, omega, d, delta, theta]
statedot = np.array([
vx * casadi.cos(phi) - vy * casadi.sin(phi), #posxdot
vx * casadi.sin(phi) + vy * casadi.cos(phi), #posydot
omega, #phidot
1 / m * (Frx - Ffy * casadi.sin(delta) + m * vy * omega), #vxdot
1 / m * (Fry + Ffy * casadi.cos(delta) - m * vx * omega), #vydot
1 / Iz * (Ffy * lf * casadi.cos(delta) - Fry * lr), #omegadot
ddot,
deltadot,
thetadot
])
#xdot = f(x,u)
self.f = casadi.Function('f', [x, u], [statedot])
#state of system
self.x = x0
#sampling timestep
self.Ts = Ts
#integration nodes
self.nodes = nodes
def dynamic_model(modelparams):
# define casadi struct
model = casadi.types.SimpleNamespace()
constraints = casadi.types.SimpleNamespace()
model_name = "f110_dynamic_model"
model.name = model_name
#loadparameters
with open(modelparams) as file:
params = yaml.load(file, Loader=yaml.FullLoader)
m = params['m'] #[kg]
lf = params['lf'] #[m]
lr = params['lr'] #[m]
Iz = params['Iz'] #[kg*m^3]
#pajecka and motor coefficients
Bf = params['Bf']
Br = params['Br']
Cf = params['Cf']
Cr = params['Cr']
Cm1 = params['Cm1']
Cm2 = params['Cm2']
Croll = params['Croll']
Cd = params['Cd']
Df = params['Df']
Dr = params['Dr']
print("CasADi model created with the following parameters: filename: ",
modelparams, "\n values:", params)
xvars = ['posx', 'posy', 'phi', 'vx', 'vy', 'omega', 'd', 'delta', 'theta']
uvars = ['ddot', 'deltadot', 'thetadot']
pvars = [
'xt', 'yt', 'phit', 'sin_phit', 'cos_phit', 'theta_hat', 'Qc', 'Ql',
'Q_theta', 'R_d', 'R_delta', 'r'
]
#parameter vector, contains linearization poitns
xt = casadi.SX.sym("xt")
yt = casadi.SX.sym("yt")
phit = casadi.SX.sym("phit")
sin_phit = casadi.SX.sym("sin_phit")
cos_phit = casadi.SX.sym("cos_phit")
#stores linearization point
theta_hat = casadi.SX.sym("theta_hat")
Qc = casadi.SX.sym("Qc")
Ql = casadi.SX.sym("Ql")
Q_theta = casadi.SX.sym("Q_theta")
#cost on smoothnes of motorinput
R_d = casadi.SX.sym("R_d")
#cost on smoothness of steering_angle
R_delta = casadi.SX.sym("R_delta")
#trackwidth
r = casadi.SX.sym("r")
#pvars = ['xt', 'yt', 'phit', 'sin_phit', 'cos_phit', 'theta_hat', 'Qc', 'Ql', 'Q_theta', 'R_d', 'R_delta', 'r']
p = casadi.vertcat(xt, yt, phit, sin_phit, cos_phit, theta_hat, Qc, Ql,
Q_theta, R_d, R_delta, r)
#single track model with pajecka tireforces as in Optimization-Based Autonomous Racing of 1:43 Scale RC Cars Alexander Liniger, Alexander Domahidi and Manfred Morari
#pose
posx = casadi.SX.sym("posx")
posy = casadi.SX.sym("posy")
#vel (long and lateral)
vx = casadi.SX.sym("vx")
vy = casadi.SX.sym("vy")
#body angular Rate
omega = casadi.SX.sym("omega")
#heading
phi = casadi.SX.sym("phi")
#steering_angle
delta = casadi.SX.sym("delta")
#motorinput
d = casadi.SX.sym("d")
#dynamic forces
Frx = casadi.SX.sym("Frx")
Fry = casadi.SX.sym("Fry")
Ffx = casadi.SX.sym("Ffx")
Ffy = casadi.SX.sym("Ffy")
#arclength progress
theta = casadi.SX.sym("theta")
#temporal derivatives
posxdot = casadi.SX.sym("xdot")
posydot = casadi.SX.sym("ydot")
vxdot = casadi.SX.sym("vxdot")
vydot = casadi.SX.sym("vydot")
phidot = casadi.SX.sym("phidot")
omegadot = casadi.SX.sym("omegadot")
deltadot = casadi.SX.sym("deltadot")
thetadot = casadi.SX.sym("thetadot")
ddot = casadi.SX.sym("ddot")
#inputvector
#uvars = ['ddot', 'deltadot', 'thetadot']
u = casadi.vertcat(ddot, deltadot, thetadot)
#car state Dynamics
#xvars = ['posx', 'posy', 'phi', 'vx', 'vy', 'omega', 'd', 'delta', 'theta']
x = casadi.vertcat(posx, posy, phi, vx, vy, omega, d, delta, theta)
xdot = casadi.vertcat(posxdot, posydot, phidot, vxdot, vydot, omegadot,
ddot, deltadot, thetadot)
#build CasADi expressions for dynamic model
#front lateral tireforce
alphaf = -casadi.atan2((omega * lf + vy), vx) + delta
Ffy = Df * casadi.sin(Cf * casadi.atan(Bf * alphaf))
#rear lateral tireforce
alphar = casadi.atan2((omega * lr - vy), vx)
Fry = Dr * casadi.sin(Cr * casadi.atan(Br * alphar))
#rear longitudinal forces
Frx = (Cm1 - Cm2 * vx) * d - Croll - Cd * vx * vx
f_expl = casadi.vertcat(
vx * casadi.cos(phi) - vy * casadi.sin(phi), #posxdot
vx * casadi.sin(phi) + vy * casadi.cos(phi), #posydot
omega, #phidot
1 / m * (Frx - Ffy * casadi.sin(delta) + m * vy * omega), #vxdot
1 / m * (Fry + Ffy * casadi.cos(delta) - m * vx * omega), #vydot
1 / Iz * (Ffy * lf * casadi.cos(delta) - Fry * lr), #omegadot
ddot,
deltadot,
thetadot)
# algebraic variables
z = casadi.vertcat([])
model.f_expl_expr = f_expl
model.f_impl_expr = xdot - f_expl
model.x = x
model.xdot = xdot
model.u = u
model.p = p
model.z = z
#boxconstraints
model.ddot_min = -10.0 #min change in d [-]
model.ddot_max = 10.0 #max change in d [-]
model.d_min = -0.1 #min d [-]
model.d_max = 1 #max d [-]
model.delta_min = -0.40 # minimum steering angle [rad]
model.delta_max = 0.40 # maximum steering angle [rad]
model.deltadot_min = -2 # minimum steering angle cahgne[rad/s]
model.deltadot_max = 2 # maximum steering angle cahgne[rad/s]
model.omega_min = -100 # minimum yawrate [rad/sec]
model.omega_max = 100 # maximum yawrate [rad/sec]
model.thetadot_min = 0.05 # minimum adv param speed [m/s]
model.thetadot_max = 5 # maximum adv param speed [m/s]
model.theta_min = 0.00 # minimum adv param [m]
model.theta_max = 1000 # maximum adv param [m]
model.vx_max = 3.5 # max long vel [m/s]
model.vx_min = -0.5 #0.05 # min long vel [m/s]
model.vy_max = 3 # max lat vel [m/s]
model.vy_min = -3 # min lat vel [m/s]
model.x0 = np.array([0, 0, 0, 1, 0.01, 0, 0, 0, 0])
#compute approximate linearized contouring and lag error
xt_hat = xt + cos_phit * (theta - theta_hat)
yt_hat = yt + sin_phit * (theta - theta_hat)
e_cont = sin_phit * (xt_hat - posx) - cos_phit * (yt_hat - posy)
e_lag = cos_phit * (xt_hat - posx) + sin_phit * (yt_hat - posy)
cost = e_cont * Qc * e_cont + e_lag * Qc * e_lag - Q_theta * thetadot + ddot * R_d * ddot + deltadot * R_delta * deltadot
#error = casadi.vertcat(e_cont, e_lag)
#set up stage cost
#Q = diag(vertcat(Qc, Ql))
model.con_h_expr = (xt_hat - posx)**2 + (yt_hat - posy)**2 - r**2
model.stage_cost = e_cont * Qc * e_cont + e_lag * Qc * e_lag - Q_theta * thetadot + ddot * R_d * ddot + deltadot * R_delta * deltadot
return model, constraints
