def solve_nlp(prop_name, thrust):
rho0 = 1.225
i0_motor0 = 0.04
V_inf0 = 1
R_motor0 = 0.05
T_desired = thrust #### Thrust constraint
prop_data = prop_db[prop_name]
key0 = list(prop_data['dynamic'].keys())[0]
my_prop = prop_data['dynamic'][key0]['data']
CT_lut = ca.interpolant('CT', 'bspline', [my_prop.index], my_prop.CT)
CP_lut = ca.interpolant('CP', 'bspline', [my_prop.index], my_prop.CP)
eta_lut = ca.interpolant('CP', 'bspline', [my_prop.index], my_prop.eta)
eta_prop_lut = ca.interpolant('eta', 'bspline', [my_prop.index],
my_prop.eta)
Q_prop_lut = ca.substitute(Q_prop, CP, CP_lut(J))
T_prop_lut = ca.substitute(T_prop, CT, CT_lut(J))
states = ca.vertcat(rpm_prop, v_batt, Kv_motor)
params = ca.vertcat(rho, r_prop_in, i0_motor, V_inf, R_motor)
p0 = [rho0, prop_data['D'] / 2, i0_motor0, V_inf0, R_motor0]
f_Q_prop = ca.Function('Q_prop', [states, params], [Q_prop_lut])
f_Q_motor = ca.Function('Q_motor', [states, params], [Q_motor])
f_eta_motor = ca.Function('eta_motor', [states, params], [eta_motor])
f_eta_prop = ca.Function('eta_prop', [states, params], [eta_prop_lut(J)])
f_eta = ca.Function('eta', [states, params], [eta_prop_lut(J) * eta_motor])
f_T_prop = ca.Function('T_prop', [states, params], [T_prop_lut])
#%%
nlp = {
'f':
-f_eta(states, p0),
'x':
states,
'g':
ca.substitute(ca.vertcat(Q_prop_lut - Q_motor, T_prop_lut - T_desired),
params, p0)
}
S = ca.nlpsol('eff_max', 'ipopt', nlp, {
'print_time': 0,
'ipopt': {
'sb': 'yes',
'print_level': 0,
}
})
res = S(x0=[1000, 5, 1000], lbg=[0, 0], ubg=[0, 0])
stats = S.stats()
if stats['success']:
print('\n')
print('propeller -> ' + prop_name, res['x'], res['g'], -res['f'])
print('\n')
v0 = float(res['x'][1])
kv0 = float(res['x'][2])
if -res['f'] > 0.3:
rpm_sweep(prop_name, v0, kv0, i0_motor0, V_inf0, R_motor0)
return res, stats
def rpm_sweep(prop_name, v0, kv0, i0_motor0, V_inf0, R_motor0):
prop_data = prop_db[prop_name]
key0 = list(prop_data['dynamic'].keys())[0]
my_prop = prop_data['dynamic'][key0]['data']
CT_lut = ca.interpolant('CT','bspline',[my_prop.index],my_prop.CT)
CP_lut = ca.interpolant('CP','bspline',[my_prop.index],my_prop.CP)
eta_lut = ca.interpolant('CP','bspline',[my_prop.index],my_prop.eta)
eta_prop_lut = ca.interpolant('eta','bspline',[my_prop.index],my_prop.eta)
Q_prop_lut = ca.substitute(Q_prop, CP, CP_lut(J))
T_prop_lut = ca.substitute(T_prop, CT, CT_lut(J))
states = ca.vertcat(rpm_prop)
params = ca.vertcat(rho, r_prop_in, v_batt, Kv_motor, i0_motor, V_inf, R_motor)
p0 = [1.225, prop_data['D']/2, v0, kv0, i0_motor0, V_inf0, R_motor0]
f_Q_prop = ca.Function('Q_prop', [states, params], [Q_prop_lut])
f_Q_motor = ca.Function('Q_motor', [states, params], [Q_motor])
f_T_prop = ca.Function('T_prop', [states, params], [T_prop_lut])
f_eta_motor = ca.Function('eta_motor', [states, params], [eta_motor])
f_eta_prop = ca.Function('eta_prop', [states, params], [eta_prop_lut(J)])
f_eta = ca.Function('eta', [states, params], [eta_prop_lut(J)*eta_motor])
rpm = np.array([np.linspace(0, 4000, 1000)])
plt.figure()
plt.subplot(311)
plt.plot(rpm.T, f_Q_motor(rpm, p0).T, label='motor')
plt.plot(rpm.T, f_Q_prop(rpm, p0).T, label='prop')
plt.title('prop motor matching')
plt.legend()
plt.ylabel('torque, N-m')
plt.grid(True)
plt.subplot(312)
plt.plot(rpm.T, f_T_prop(rpm, p0).T, label='prop')
plt.legend()
plt.ylabel('thrust, N')
plt.grid(True)
plt.subplot(313)
plt.plot(rpm.T, f_eta_motor(rpm, p0).T, label='motor')
plt.plot(rpm.T, f_eta_prop(rpm, p0).T, label='prop')
plt.plot(rpm.T, f_eta(rpm, p0).T, label='total')
plt.xlabel('prop angular velocity, rpm')
plt.ylabel('efficiency')
plt.grid(True)
plt.legend()
plt.gca().set_ylim(0, 1)
plt.show()
def solve_grid(self):
"""
Implements the gridding algorithm by solving the optimization problem
for each point in the grid.
"""
print("Solving grid...")
for n in range(int(self.sim_time / self.dt)):
print("Iteration: ", n, "/", int(self.sim_time / self.dt))
# Get cost at grid point
# Hint: take inspiration from Gridder class
# for ...
# for ...
# Update cost to go
J_flat = self.J[:, :, :, :, n]
J_flat = J_flat.ravel(order='F')
if self.DEBUG:
print("U[:,:,", n, "]:\n ", self.U[:, :, n])
print("J[:,:,:,:,", n, "]:\n ", J_flat)
# Update cost-to-go policy
self.Jtogo = ca.interpolant('new_cost', 'bspline',
[self.x1, self.x2, self.x3, self.x4],
J_flat, {"algorithm": "smooth_linear"})
self.Jtogo_updated = True
print("Finished!")
pass
def create_table2D(name, row_label, col_label, data, abs_row=False, abs_col=False, interp_method=INTERP_DEFAULT):
"""
Creates a table interpolation function with x as rows and y as columns
"""
assert data[0, 0] == 0
row_grid = data[1:, 0]
col_grid = data[0, 1:]
table_data = data[1:, 1:]
interp = ca.interpolant(name + '_interp', interp_method, [row_grid, col_grid],
table_data.ravel(order='F'))
x = ca.MX.sym('x')
y = ca.MX.sym('y')
if abs_row:
xs = ca.fabs(x)
else:
xs = x
if abs_col:
ys = ca.fabs(y)
else:
ys = y
func = ca.Function('Cx', [x, y], [interp(ca.vertcat(xs, ys))], [row_label, col_label], [name])
# check
for i, x in enumerate(row_grid):
for j, y in enumerate(col_grid):
assert ca.fabs(func(x, y) - table_data[i, j]) < TABLE_CHECK_TOL
return func
def cubic_spline(self, step=40):
# Loading waypoints from csv file
# input_file = rospy.get_param("~wp_file")
input_file = "/home/lva/data_file/wp-for-mpcc.csv"
data = genfromtxt(input_file, delimiter=',')
x_list = data[:, 0]
y_list = data[:, 1]
self.wp_len = len(x_list)
l_list = np.arange(0, self.wp_len, 1)
self.L = int(self.wp_len / step) * step
self.cs_x = ca.interpolant('cs_x', 'bspline', [l_list[::step]],
x_list[::step])
self.cs_y = ca.interpolant('cs_y', 'bspline', [l_list[::step]],
y_list[::step])
th = ca.MX.sym('th')
# Tangent angle
self.Phi = ca.Function('Phi', [th], [
ca.arctan(
ca.jacobian(self.cs_y(th), th) /
ca.jacobian(self.cs_x(th), th))
])
print(self.L)
def create_Cz():
alpha_deg = ca.MX.sym('alpha_deg')
beta_deg = ca.MX.sym('beta_deg')
elev_deg = ca.MX.sym('elev_deg')
data = np.array([
[-10, -5, 0, 5, 10, 15, 20, 25, 30, 35, 40, 45],
[0.770, 0.241, -0.100, -0.416, -0.731, -1.053, -1.366, -1.646, -1.917, -2.120, -2.248, -2.229]
])
interp = ca.interpolant('Cz_interp', INTERP_DEFAULT, [data[0, :]],
data[1, :])
return ca.Function('Cz',
[alpha_deg, beta_deg, elev_deg],
[interp(alpha_deg)*(1 - (beta_deg/57.3)**2) - 0.19*elev_deg/25.0],
['alpha_deg', 'beta_deg', 'elev_deg'], ['Cz'])
def create_damping():
data = np.array([
[-10, -5, 0, 5, 10, 15, 20, 25, 30, 35, 40, 45], # alpha, deg
[
-0.267, -0.110, 0.308, 1.34, 2.08, 2.91, 2.76, 2.05, 1.50,
1.49, 1.83, 1.21
], # CXq
[
0.882, 0.852, 0.876, 0.958, 0.962, 0.974, 0.819, 0.483, 0.590,
1.21, -0.493, -1.04
], # CYr
[
-0.108, -0.108, -0.188, 0.110, 0.258, 0.226, 0.344, 0.362,
0.611, 0.529, 0.298, -2.27
], # CYp
[
-8.80, -25.8, -28.9, -31.4, -31.2, -30.7, -27.7, -28.2, -29.0,
-29.8, -38.3, -35.3
], # CZq
[
-0.126, -0.026, 0.063, 0.113, 0.208, 0.230, 0.319, 0.437,
0.680, 0.100, 0.447, -0.330
], # Clr
[
-0.360, -0.359, -0.443, -0.420, -0.383, -0.375, -0.329, -0.294,
-0.230, -0.210, -0.120, -0.100
], # Clp
[
-7.21, -0.540, -5.23, -5.26, -6.11, -6.64, -5.69, -6.00, -6.20,
-6.40, -6.60, -6.00
], # Cmq
[
-0.380, -0.363, -0.378, -0.386, -0.370, -0.453, -0.550, -0.582,
-0.595, -0.637, -1.02, -0.840
], # Cnr
[
0.061, 0.052, 0.052, -0.012, -0.013, -0.024, 0.050, 0.150,
0.130, 0.158, 0.240, 0.150
]
]) # Cnp
names = ['CXq', 'CYr', 'CYp', 'CZq', 'Clr', 'Clp', 'Cmq', 'Cnr', 'Cnp']
for i, name in enumerate(names):
tables[name] = ca.interpolant('{:s}_interp'.format(name),
INTERP_DEFAULT, [data[0, :]],
data[i + 1, :])
# check
for j, x in enumerate(data[0, :]):
assert ca.fabs(tables[name](x) -
data[i + 1, j]) < TABLE_CHECK_TOL
def __init__(self, deltaT=0):
"""
:param deltaT: Temperature differential w.r.t. ISA conditions
:type deltaT: float, optional
:returns: None
"""
self.deltaT = deltaT
z = np.arange(0, 20.5e3, 500)
T = np.maximum(T0_MSL + lapse_rate_troposphere * z,
T_tropopause * np.ones_like(z))
P_tropo = P0_MSL * (T / T0_MSL)**(-g / Rg / lapse_rate_troposphere)
P_pause = P_tropo[22] * np.exp(-g / Rg / T * (z - h_tropopause))
P = np.hstack([P_tropo[:23], P_pause[23:]])
z_I = np.arange(0, 20.5e3, 341)
T_I = np.maximum(T0_MSL + lapse_rate_troposphere * z_I,
T_tropopause * np.ones_like(z_I))
self.IP = interpolant('P', 'bspline', [z], P, {})
self.IT = interpolant('T', 'bspline', [z_I], T_I, {})
def solve_grid(self):
"""
Implements the gridding algorithm by solving the optimization problem
for each point in the grid.
"""
print("Solving grid...")
for n in range(int(self.sim_time / self.dt)):
print("Time-step: ", "{:.2f}".format(n * self.dt), "/",
self.sim_time)
# Get cost at grid point
for i in range(len(self.x1)):
for j in range(len(self.x2)):
x = np.array([self.x1[i], self.x2[j]])
u, cost = self.solve_dp(x)
if self.use_nonlinear_controller is False:
self.U[i, j, n] = u
else:
# Modify grid to account for virtual input v
a0 = self.model.a0
b0 = self.model.b0
self.U[i, j,
n] = (1 / ca.cos(x[0]) *
(a0 / b0 * (ca.sin(x[0]) - x[0]) + u))
self.J[i, j, n] = cost
# Update cost to go
if self.DEBUG:
print("U[:,:,", n, "]:\n ", self.U[:, :, n])
print("J[:,:,", n, "]:\n ", self.J[:, :, n])
J_flat = self.J[:, :, n]
J_flat = J_flat.ravel(order='F')
# Update cost-to-go policy
self.Jtogo = ca.interpolant('new_cost', 'bspline',
[self.x1, self.x2], J_flat,
{"algorithm": "smooth_linear"})
self.Jtogo_updated = True
print("Finished!")
pass
def solve_grid(self):
"""
Implements the gridding algorithm by solving the optimization problem
for each point in the grid.
"""
print("Solving grid...")
for n in range(int(self.sim_time / self.dt)):
print("Iteration: ", n, "/", int(self.sim_time / self.dt))
# Get cost at grid point
for i in range(len(self.x1)):
for j in range(len(self.x2)):
for k in range(len(self.x3)):
for l in range(len(self.x4)):
x = np.array([
self.x1[i], self.x2[j], self.x3[k], self.x4[l]
])
u, cost = self.solve_dp(x)
self.U[i, j, k, l, n] = u
self.J[i, j, k, l, n] = cost
# Update cost to go
J_flat = self.J[:, :, :, :, n]
J_flat = J_flat.ravel(order='F')
if self.DEBUG:
print("U[:,:,", n, "]:\n ", self.U[:, :, n])
print("J[:,:,:,:,", n, "]:\n ", J_flat)
# Update cost-to-go policy
self.Jtogo = ca.interpolant('new_cost', 'bspline',
[self.x1, self.x2, self.x3, self.x4],
J_flat, {"algorithm": "smooth_linear"})
self.Jtogo_updated = True
print("Finished!")
pass
def finite_time_dp(self, x, t):
"""
Retrieves the optimal control for a given state and time-step.
:param x: current state
:type x: numpy array
:param t: time step
:type t: float
:return: optimal control input
:rtype: float
"""
n = int(self.sim_time / self.dt) - int(t / self.dt) - 1
U_flat = self.U[:, :, :, :, n].ravel(order="F")
self.sample_u = ca.interpolant('new_cost', 'bspline',
[self.x1, self.x2, self.x3, self.x4],
U_flat, {"algorithm": "smooth_linear"})
u = self.sample_u(x.T)
# Log cost and energy
self.log_cost(x, u)
return u
def _setup_model(self):
# States
self.x = ca.MX.sym("x", self.nx)
T_hts = self.x[self.x_index["T_hts"]]
T_lts = self.x[self.x_index["T_lts"]]
T_fpsc = self.x[self.x_index["T_fpsc"]]
T_fpsc_s = self.x[self.x_index["T_fpsc_s"]]
T_vtsc = self.x[self.x_index["T_vtsc"]]
T_vtsc_s = self.x[self.x_index["T_vtsc_s"]]
T_pscf = self.x[self.x_index["T_pscf"]]
T_pscr = self.x[self.x_index["T_pscr"]]
T_shx_psc = self.x[self.x_index["T_shx_psc"]]
T_shx_ssc = self.x[self.x_index["T_shx_ssc"]]
T_fcu_a = self.x[self.x_index["T_fcu_a"]]
T_fcu_w = self.x[self.x_index["T_fcu_w"]]
T_r_c = self.x[self.x_index["T_r_c"]]
T_r_a = self.x[self.x_index["T_r_a"]]
dT_amb = self.x[self.x_aux_index["dT_amb"]]
dI_vtsc = self.x[self.x_aux_index["dI_vtsc"]]
dI_fpsc = self.x[self.x_aux_index["dI_fpsc"]]
Qdot_n_c = self.x[self.x_aux_index["Qdot_n_c"]]
Qdot_n_a = self.x[self.x_aux_index["Qdot_n_a"]]
dalpha_vtsc = self.x[self.x_aux_index["dalpha_vtsc"]]
dalpha_fpsc = self.x[self.x_aux_index["dalpha_fpsc"]]
# Discrete controls
self.b = ca.MX.sym("b", self.nb)
b_ac = self.b[self.b_index["b_ac"]]
b_fc = self.b[self.b_index["b_fc"]]
# Continuous controls
self.u = ca.MX.sym("u", self.nu)
v_ppsc = self.u[self.u_index["v_ppsc"]]
p_mpsc = self.u[self.u_index["p_mpsc"]]
v_plc = self.u[self.u_index["v_plc"]]
v_pssc = self.u[self.u_index["v_pssc"]]
mdot_o_hts_b = self.u[self.u_index["mdot_o_hts_b"]]
mdot_i_hts_b = self.u[self.u_index["mdot_i_hts_b"]]
# Time-varying parameters
self.c = ca.MX.sym("c", self.nc)
T_amb = self.c[self.c_index["T_amb"]] + dT_amb
I_vtsc = self.c[self.c_index["I_vtsc"]] + dI_vtsc
I_fpsc = self.c[self.c_index["I_fpsc"]] + dI_fpsc
I_r_dir = self.c[self.c_index["I_r_dir"]]
I_r_diff = self.c[self.c_index["I_r_diff"]]
# Process noise
self.w = ca.MX.sym("w", self.nw)
w_dT_amb = self.w[self.w_index["dT_amb"]]
w_dI_vtsc = self.w[self.w_index["dI_vtsc"]]
w_dI_fpsc = self.w[self.w_index["dI_fpsc"]]
w_Qdot_n_c = self.w[self.w_index["Qdot_n_c"]]
w_Qdot_n_a = self.w[self.w_index["Qdot_n_a"]]
w_dalpha_vtsc = self.w[self.w_index["dalpha_vtsc"]]
w_dalpha_fpsc = self.w[self.w_index["dalpha_fpsc"]]
# Modeling
f = []
# Grey box ACM model
char_curve_acm = ca.veccat(1.0, \
T_lts[0], T_hts[0], (T_amb + self.p["dT_rc"]), \
T_lts[0]**2, T_hts[0]**2, (T_amb + self.p["dT_rc"])**2, \
T_lts[0] * T_hts[0], T_lts[0] * (T_amb + self.p["dT_rc"]), \
T_hts[0] * (T_amb + self.p["dT_rc"]), \
T_lts[0] * T_hts[0] * (T_amb + self.p["dT_rc"]))
params_COP_ac = np.asarray([2.03268, -0.116526, -0.0165648, \
-0.043367, -0.00074309, -0.000105659, -0.00172085, \
0.00113422, 0.00540221, 0.00116735, -3.87996e-05])
params_Qdot_ac_lt = np.asarray([-5.6924, 1.12102, 0.291654, \
-0.484546, -0.00585722, -0.00140483, 0.00795341, \
0.00399118, -0.0287113, -0.00221606, 5.42825e-05])
COP_ac = ca.mtimes(np.atleast_2d(params_COP_ac), char_curve_acm)
Qdot_ac_lt = ca.mtimes(np.atleast_2d(params_Qdot_ac_lt),
char_curve_acm) * 1e3
T_ac_ht = T_hts[0] - ((Qdot_ac_lt / COP_ac) /
(self.p["mdot_ac_ht"] * self.p["c_w"]))
T_ac_lt = T_lts[0] - (Qdot_ac_lt /
(self.p["mdot_ac_lt"] * self.p["c_w"]))
# Free cooling
T_fc_lt = T_amb + self.p["dT_rc"]
# HT storage model
mdot_ssc = self.p["mdot_ssc_max"] * v_pssc
m_hts = (self.p["V_hts"] * self.p["rho_w"]) / T_hts.numel()
mdot_hts_t_s = mdot_ssc - b_ac * self.p["mdot_ac_ht"]
mdot_hts_t_sbar = ca.sqrt(mdot_hts_t_s**2 + self.p["eps_hts"])
mdot_hts_b_s = mdot_i_hts_b - mdot_o_hts_b
mdot_hts_b_sbar = ca.sqrt(mdot_hts_b_s**2 + self.p["eps_hts"])
f.append( \
(1.0 / m_hts) * ( \
mdot_ssc * T_shx_ssc[-1] \
- b_ac * self.p["mdot_ac_ht"] * T_hts[0] \
- (mdot_hts_t_s * ((T_hts[0] + T_hts[1]) / 2.0) \
+ mdot_hts_t_sbar * ((T_hts[0] - T_hts[1]) / 2.0)) \
- (self.p["lambda_hts"][0] / self.p["c_w"] * (T_hts[0] - T_amb))) \
)
f.append( \
(1.0 / m_hts) * ( \
(b_ac * self.p["mdot_ac_ht"] - mdot_i_hts_b) * T_ac_ht
- (mdot_ssc - mdot_o_hts_b) * T_hts[1]
+ (mdot_hts_t_s * ((T_hts[0] + T_hts[1]) / 2.0) + mdot_hts_t_sbar * ((T_hts[0] - T_hts[1]) / 2.0))
+ (mdot_hts_b_s * ((T_hts[2] + T_hts[1]) / 2.0) + mdot_hts_b_sbar * ((T_hts[2] - T_hts[1]) / 2.0))
- (self.p["lambda_hts"][3] / self.p["c_w"] * (T_hts[1] - T_amb)))
)
f.append( \
(1.0 / m_hts) * ( \
mdot_hts_b_s * (((T_hts[-1] + T_hts[-2]) / 2.0) - ((T_hts[-2] + T_hts[-3]) / 2.0)) \
+ mdot_hts_b_sbar * (((T_hts[-1] - T_hts[-2]) / 2.0) - ((T_hts[-2] - T_hts[-3]) / 2.0))
- (self.p["lambda_hts"][-2] / self.p["c_w"] * (T_hts[-2] - T_amb)))
)
f.append( \
(1.0 / m_hts) * ( \
mdot_i_hts_b * T_ac_ht
- mdot_o_hts_b * T_hts[-1]
- (mdot_hts_b_s * ((T_hts[-1] + T_hts[-2]) / 2.0) + mdot_hts_b_sbar * ((T_hts[-1] - T_hts[-2]) / 2.0))
- (self.p["lambda_hts"][-1] / self.p["c_w"] * (T_hts[-1] - T_amb)))
)
# LT storage model
mdot_lc = self.p["mdot_lc_max"] * v_plc
m_lts = (self.p["V_lts"] * self.p["rho_w"]) / T_lts.numel()
f.append( \
(1.0 / m_lts) * ( \
mdot_lc * T_fcu_w \
- (1.0 - b_ac - b_fc) * mdot_lc * T_lts[0] \
+ b_ac * (self.p["mdot_ac_lt"] - mdot_lc) * T_lts[1] \
- b_ac * self.p["mdot_ac_lt"] * T_lts[0]
+ b_fc * (self.p["mdot_fc_lt"] - mdot_lc) * T_lts[1] \
- b_fc * self.p["mdot_fc_lt"] * T_lts[0])
)
f.append( \
(1.0 / m_lts) * ( \
(1.0 - b_ac - b_fc) * mdot_lc * T_lts[-2] \
- mdot_lc * T_lts[-1] \
+ b_ac * self.p["mdot_ac_lt"] * T_ac_lt \
- b_ac * (self.p["mdot_ac_lt"] - mdot_lc) * T_lts[-1]
+ b_fc * self.p["mdot_fc_lt"] * T_fc_lt
- b_fc * (self.p["mdot_fc_lt"] - mdot_lc) * T_lts[-1])
)
# Flat plate collectors
data_v_ppsc = [-0.1, 0.0, 0.4, 0.6, 0.8, 1.0, 1.1]
data_p_mpsc = [0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0]
data_mdot_fpsc = np.array([
0.00116667, 0., 0.02765, 0.03511667, 0.04258333, 0.04993333,
0.04993333, 0.00116667, 0., 0.02765, 0.03511667, 0.04258333,
0.04993333, 0.04993333, 0.0035, 0., 0.08343, 0.11165333, 0.13568,
0.15895333, 0.15895333, 0.005, 0., 0.12508167, 0.16568, 0.20563333,
0.2397, 0.2397, 0.0055, 0., 0.13999333, 0.1859, 0.22790167,
0.26488, 0.26488, 0.006, 0., 0.14969167, 0.19844, 0.24394167,
0.28695333, 0.28695333, 0.00633333, 0., 0.15706667, 0.21070833,
0.25807, 0.310775, 0.310775, 0.0075, 0., 0.17047833, 0.229775,
0.28826667, 0.34190833, 0.34190833, 0.0095, 0., 0.20687333,
0.27500667, 0.331775, 0.37235333, 0.37235333, 0.013, 0.,
0.24111667, 0.31581, 0.38300833, 0.44705167, 0.44705167, 0.013, 0.,
0.24111667, 0.31581, 0.38300833, 0.44705167, 0.44705167
])
iota_mdot_fpsc = ca.interpolant('iota_mdot_fpsc', 'bspline', [data_v_ppsc, data_p_mpsc], \
data_mdot_fpsc)
mdot_fpsc = iota_mdot_fpsc(ca.veccat(v_ppsc, p_mpsc))
Qdot_fpsc = self.p["eta_fpsc"] * self.p["A_fpsc"] * I_fpsc
Qdot_fpsc_amb = (self.p["alpha_fpsc"] +
dalpha_fpsc) * self.p["A_fpsc"] * (T_fpsc - T_amb)
f.append( \
(1.0 / self.p["C_fpsc"]) * \
(mdot_fpsc * self.p["c_sl"] * (T_pscf - T_fpsc) + \
Qdot_fpsc - Qdot_fpsc_amb))
f.append((1.0 / (self.p["V_fpsc_s"] * self.p["rho_sl"] * self.p["c_sl"])) * ( \
mdot_fpsc * self.p["c_sl"] * (T_fpsc - T_fpsc_s) - \
self.p["lambda_fpsc_s"] * (T_fpsc_s - T_amb))
)
# Tube collectors
data_mdot_vtsc = np.array([
0.0155, 0., 0.36735, 0.46655, 0.56575, 0.6634, 0.6634, 0.0155, 0.,
0.36735, 0.46655, 0.56575, 0.6634, 0.6634, 0.01316667, 0., 0.32157,
0.41501333, 0.50432, 0.59438, 0.59438, 0.01166667, 0., 0.29325167,
0.37932, 0.4577, 0.54363333, 0.54363333, 0.01116667, 0.,
0.28167333, 0.3641, 0.44043167, 0.52345333, 0.52345333, 0.01066667,
0., 0.271975, 0.34822667, 0.42439167, 0.50138, 0.50138, 0.01033333,
0., 0.25626667, 0.33095833, 0.39859667, 0.464225, 0.464225,
0.00916667, 0., 0.217855, 0.275225, 0.3384, 0.39975833, 0.39975833,
0.00716667, 0., 0.15479333, 0.19832667, 0.243225, 0.30098, 0.30098,
0.00366667, 0., 0.06721667, 0.08752333, 0.10865833, 0.13128167,
0.13128167, 0.00366667, 0., 0.06721667, 0.08752333, 0.10865833,
0.13128167, 0.13128167
])
iota_mdot_vtsc = ca.interpolant('iota_mdot_vtsc', 'bspline', [data_v_ppsc, data_p_mpsc], \
data_mdot_vtsc)
mdot_vtsc = iota_mdot_vtsc(ca.veccat(v_ppsc, p_mpsc))
Qdot_vtsc = self.p["eta_vtsc"] * self.p["A_vtsc"] * I_vtsc
Qdot_vtsc_amb = (self.p["alpha_vtsc"] +
dalpha_vtsc) * self.p["A_vtsc"] * (T_vtsc - T_amb)
f.append( \
(1.0 / self.p["C_vtsc"]) * \
(mdot_vtsc * self.p["c_sl"] * (T_pscf - T_vtsc) + \
Qdot_vtsc - Qdot_vtsc_amb))
f.append((1.0 / (self.p["V_vtsc_s"] * self.p["rho_sl"] * self.p["c_sl"])) * ( \
mdot_vtsc * self.p["c_sl"] * (T_vtsc - T_vtsc_s) - \
self.p["lambda_vtsc_s"] * (T_vtsc_s - T_amb))
)
# Pipes connecting solar collectors and solar heat exchanger
f.append(1.0 / self.p["C_psc"] * ((mdot_fpsc + mdot_vtsc) * \
self.p["c_sl"] * (T_shx_psc[-1] - T_pscf) - \
self.p["lambda_psc"] * (T_pscf - T_amb)))
f.append(1.0 / self.p["C_psc"] * (mdot_fpsc * self.p["c_sl"] * T_fpsc_s \
+ mdot_vtsc * self.p["c_sl"] * T_vtsc_s \
- (mdot_fpsc + mdot_vtsc) * self.p["c_sl"] * T_pscr \
- self.p["lambda_psc"] * (T_pscr - T_amb)))
# Solar heat exchanger
m_shx_psc = self.p["V_shx"] * self.p["rho_sl"] / T_shx_psc.numel()
m_shx_ssc = self.p["V_shx"] * self.p["rho_w"] / T_shx_psc.numel()
A_shx_k = self.p["A_shx"] / T_shx_psc.numel()
f.append( \
(1.0 / (m_shx_psc * self.p["c_sl"])) * ( \
(mdot_fpsc + mdot_vtsc) \
* self.p["c_sl"] * (T_pscr - T_shx_psc[0]) \
- (A_shx_k * self.p["alpha_shx"] * (T_shx_psc[0] - T_shx_ssc[-1]))))
for k in range(1, T_shx_psc.numel()):
f.append( \
(1.0 / (m_shx_psc * self.p["c_sl"])) * ( \
(mdot_fpsc + mdot_vtsc) \
* self.p["c_sl"] * (T_shx_psc[k-1] - T_shx_psc[k]) \
- (A_shx_k * self.p["alpha_shx"] * (T_shx_psc[k] - T_shx_ssc[-1-k]))))
f.append( \
(1.0 / (m_shx_ssc * self.p["c_w"])) * ( \
(mdot_ssc - mdot_o_hts_b) * self.p["c_w"] * (T_hts[1] - T_shx_ssc[0]) \
+ mdot_o_hts_b * self.p["c_w"] * (T_hts[-1] - T_shx_ssc[0]) \
+ (A_shx_k * self.p["alpha_shx"] * (T_shx_psc[-1] - T_shx_ssc[0]))))
for k in range(1, T_shx_ssc.numel()):
f.append( \
(1.0 / (m_shx_ssc * self.p["c_w"])) * ( \
mdot_ssc * self.p["c_w"] * (T_shx_ssc[k-1] - T_shx_ssc[k]) \
+ (A_shx_k * self.p["alpha_shx"] * (T_shx_psc[-1-k] - T_shx_ssc[k]))))
# Cooling system model
Qdot_fcu_r = self.p["lambda_fcu"] * (T_fcu_a - T_fcu_w)
f.append((1.0 / self.p["C_fcu_a"]) * \
(self.p["c_a"] * self.p["mdot_fcu_a"] * (T_r_a[0] - T_fcu_a) - Qdot_fcu_r))
f.append((1.0 / self.p["C_fcu_w"]) * \
(self.p["c_w"] * mdot_lc * (T_lts[-1] - T_fcu_w) + Qdot_fcu_r))
# Building model
Qdot_r_a_c = []
for k in range(3):
Qdot_r_a_c.append(
(1.0 / self.p["R_r_a_c"][k]) * (T_r_a[k] - T_r_c[k]))
f.append((1.0 / self.p["C_r_c"][k]) * (Qdot_r_a_c[k] \
+ (1.0 / self.p["R_r_c_amb"][k]) * (T_amb - T_r_c[k]) \
+ self.p["r_I_dir_c"][k] * I_r_dir + self.p["r_I_diff_c"][k] * I_r_diff \
+ Qdot_n_c[k]))
Qdot_r_a_a = []
Qdot_r_a_a.append((1.0 / self.p["R_r_a_a"]) * (T_r_a[1] - T_r_a[0]))
Qdot_r_a_a.append((1.0 / self.p["R_r_a_a"]) * (T_r_a[2] - T_r_a[1]))
f.append((1.0 / self.p["C_r_a"]) * (-Qdot_r_a_c[0] \
+ (1.0 / self.p["R_r_a_amb"]) * (T_amb - T_r_a[0]) \
+ Qdot_r_a_a[0] - self.p["r_Qdot_fcu_a"][0] * Qdot_fcu_r \
+ self.p["r_I_dir_a"][0] * I_r_dir + self.p["r_I_diff_a"][0] * I_r_diff \
+ Qdot_n_a[0]))
f.append((1.0 / self.p["C_r_a"]) * (-Qdot_r_a_c[1] \
+ (1.0 / self.p["R_r_a_amb"]) * (T_amb - T_r_a[1]) \
- Qdot_r_a_a[0] + Qdot_r_a_a[1] - self.p["r_Qdot_fcu_a"][1] * Qdot_fcu_r \
+ self.p["r_I_dir_a"][1] * I_r_dir + self.p["r_I_diff_a"][1] * I_r_diff \
+ Qdot_n_a[1]))
f.append((1.0 / self.p["C_r_a"]) * (-Qdot_r_a_c[2] \
+ (1.0 / self.p["R_r_a_amb"]) * (T_amb - T_r_a[2]) \
- Qdot_r_a_a[1] - self.p["r_Qdot_fcu_a"][2] * Qdot_fcu_r \
+ self.p["r_I_dir_a"][2] * I_r_dir + self.p["r_I_diff_a"][2] * I_r_diff \
+ Qdot_n_a[2]))
# Ambient data deviations
f.append((-dT_amb / self.p["tau_T_amb"]) + w_dT_amb)
f.append((-dI_vtsc / self.p["tau_I_vtsc"]) + w_dI_vtsc)
f.append((-dI_fpsc / self.p["tau_I_fpsc"]) + w_dI_fpsc)
# Heat flow noise on building model
for k in range(3):
f.append((-Qdot_n_c[k] / self.p["tau_Qdot_n_c"]) + w_Qdot_n_c[k])
for k in range(3):
f.append((-Qdot_n_a[k] / self.p["tau_Qdot_n_a"]) + w_Qdot_n_a[k])
# Heat loss noise for solar collectors
f.append(w_dalpha_vtsc)
f.append(w_dalpha_fpsc)
self.f = ca.veccat(*f)
# Measurement function
self.y = ca.MX.sym("y", self.ny)
h = []
h.append(T_hts[0])
h.append(T_hts[1])
h.append(T_hts[2])
h.append(T_hts[3])
h.append(T_lts[0])
h.append(T_lts[1])
h.append(T_fpsc_s)
h.append(T_vtsc_s)
h.append(T_shx_psc[0])
h.append(T_shx_psc[3])
h.append(T_shx_ssc[0])
h.append(T_shx_ssc[3])
h.append(T_fcu_w)
h.append(T_r_c[0])
h.append(T_r_c[1])
h.append(T_r_c[2])
h.append(T_r_a[0])
h.append(T_r_a[1])
h.append(T_r_a[2])
h.append(T_amb)
h.append(I_vtsc)
h.append(I_fpsc)
self.h = ca.veccat(*h)
def _convert(self, symbol, t, y, y_dot, inputs):
""" See :meth:`CasadiConverter.convert()`. """
if isinstance(
symbol,
(
pybamm.Scalar,
pybamm.Array,
pybamm.Time,
pybamm.InputParameter,
pybamm.ExternalVariable,
),
):
return casadi.MX(symbol.evaluate(t, y, y_dot, inputs))
elif isinstance(symbol, pybamm.StateVector):
if y is None:
raise ValueError("Must provide a 'y' for converting state vectors")
return casadi.vertcat(*[y[y_slice] for y_slice in symbol.y_slices])
elif isinstance(symbol, pybamm.StateVectorDot):
if y_dot is None:
raise ValueError("Must provide a 'y_dot' for converting state vectors")
return casadi.vertcat(*[y_dot[y_slice] for y_slice in symbol.y_slices])
elif isinstance(symbol, pybamm.BinaryOperator):
left, right = symbol.children
# process children
converted_left = self.convert(left, t, y, y_dot, inputs)
converted_right = self.convert(right, t, y, y_dot, inputs)
if isinstance(symbol, pybamm.Minimum):
return casadi.fmin(converted_left, converted_right)
if isinstance(symbol, pybamm.Maximum):
return casadi.fmax(converted_left, converted_right)
# _binary_evaluate defined in derived classes for specific rules
return symbol._binary_evaluate(converted_left, converted_right)
elif isinstance(symbol, pybamm.UnaryOperator):
converted_child = self.convert(symbol.child, t, y, y_dot, inputs)
if isinstance(symbol, pybamm.AbsoluteValue):
return casadi.fabs(converted_child)
return symbol._unary_evaluate(converted_child)
elif isinstance(symbol, pybamm.Function):
converted_children = [
self.convert(child, t, y, y_dot, inputs) for child in symbol.children
]
# Special functions
if symbol.function == np.min:
return casadi.mmin(*converted_children)
elif symbol.function == np.max:
return casadi.mmax(*converted_children)
elif symbol.function == np.abs:
return casadi.fabs(*converted_children)
elif symbol.function == np.sqrt:
return casadi.sqrt(*converted_children)
elif symbol.function == np.sin:
return casadi.sin(*converted_children)
elif symbol.function == np.arcsinh:
return casadi.arcsinh(*converted_children)
elif symbol.function == np.arccosh:
return casadi.arccosh(*converted_children)
elif symbol.function == np.tanh:
return casadi.tanh(*converted_children)
elif symbol.function == np.cosh:
return casadi.cosh(*converted_children)
elif symbol.function == np.sinh:
return casadi.sinh(*converted_children)
elif symbol.function == np.cos:
return casadi.cos(*converted_children)
elif symbol.function == np.exp:
return casadi.exp(*converted_children)
elif symbol.function == np.log:
return casadi.log(*converted_children)
elif symbol.function == np.sign:
return casadi.sign(*converted_children)
elif isinstance(symbol.function, (PchipInterpolator, CubicSpline)):
return casadi.interpolant("LUT", "bspline", [symbol.x], symbol.y)(
*converted_children
)
elif symbol.function.__name__.startswith("elementwise_grad_of_"):
differentiating_child_idx = int(symbol.function.__name__[-1])
# Create dummy symbolic variables in order to differentiate using CasADi
dummy_vars = [
casadi.MX.sym("y_" + str(i)) for i in range(len(converted_children))
]
func_diff = casadi.gradient(
symbol.differentiated_function(*dummy_vars),
dummy_vars[differentiating_child_idx],
)
# Create function and evaluate it using the children
casadi_func_diff = casadi.Function("func_diff", dummy_vars, [func_diff])
return casadi_func_diff(*converted_children)
# Other functions
else:
return symbol._function_evaluate(converted_children)
elif isinstance(symbol, pybamm.Concatenation):
converted_children = [
self.convert(child, t, y, y_dot, inputs) for child in symbol.children
]
if isinstance(symbol, (pybamm.NumpyConcatenation, pybamm.SparseStack)):
return casadi.vertcat(*converted_children)
# DomainConcatenation specifies a particular ordering for the concatenation,
# which we must follow
elif isinstance(symbol, pybamm.DomainConcatenation):
slice_starts = []
all_child_vectors = []
for i in range(symbol.secondary_dimensions_npts):
child_vectors = []
for child_var, slices in zip(
converted_children, symbol._children_slices
):
for child_dom, child_slice in slices.items():
slice_starts.append(symbol._slices[child_dom][i].start)
child_vectors.append(
child_var[child_slice[i].start : child_slice[i].stop]
)
all_child_vectors.extend(
[v for _, v in sorted(zip(slice_starts, child_vectors))]
)
return casadi.vertcat(*all_child_vectors)
else:
raise TypeError(
"""
Cannot convert symbol of type '{}' to CasADi. Symbols must all be
'linear algebra' at this stage.
""".format(
type(symbol)
)
)
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]
def rpm_sweep(prop_name, v0, kv0, i0_motor0, V_inf0, R_motor0):
prop_data = prop_db[prop_name]
key0 = list(prop_data['dynamic'].keys())[0]
my_prop = prop_data['dynamic'][key0]['data']
CT_lut = ca.interpolant('CT', 'bspline', [my_prop.index], my_prop.CT)
CP_lut = ca.interpolant('CP', 'bspline', [my_prop.index], my_prop.CP)
eta_lut = ca.interpolant('CP', 'bspline', [my_prop.index], my_prop.eta)
eta_prop_lut = ca.interpolant('eta', 'bspline', [my_prop.index],
my_prop.eta)
Q_prop_lut = ca.substitute(Q_prop, CP, CP_lut(J))
T_prop_lut = ca.substitute(T_prop, CT, CT_lut(J))
states = ca.vertcat(rpm_prop)
params = ca.vertcat(rho, r_prop_in, v_batt, Kv_motor, i0_motor, V_inf,
R_motor)
p0 = [1.225, prop_data['D'] / 2, v0, kv0, i0_motor0, V_inf0, R_motor0]
f_Q_prop = ca.Function('Q_prop', [states, params], [Q_prop_lut])
f_Q_motor = ca.Function('Q_motor', [states, params], [Q_motor])
f_T_prop = ca.Function('T_prop', [states, params], [T_prop_lut])
f_eta_motor = ca.Function('eta_motor', [states, params], [eta_motor])
f_eta_prop = ca.Function('eta_prop', [states, params], [eta_prop_lut(J)])
f_eta = ca.Function('eta', [states, params], [eta_prop_lut(J) * eta_motor])
rpm = np.array([np.linspace(0, 4000, 1000)])
plt.figure()
plt.subplot(311)
plt.plot(rpm.T, f_Q_motor(rpm, p0).T, label='motor')
plt.plot(rpm.T, f_Q_prop(rpm, p0).T, label='prop')
plt.title('prop motor matching')
plt.legend()
plt.ylabel('torque, N-m')
plt.grid(True)
plt.subplot(312)
plt.plot(rpm.T, f_T_prop(rpm, p0).T, label='prop')
plt.legend()
plt.ylabel('thrust, N')
plt.grid(True)
plt.subplot(313)
plt.plot(rpm.T, f_eta_motor(rpm, p0).T, label='motor')
plt.plot(rpm.T, f_eta_prop(rpm, p0).T, label='prop')
plt.plot(rpm.T, f_eta(rpm, p0).T, label='total')
plt.xlabel('prop angular velocity, rpm')
plt.ylabel('efficiency')
plt.grid(True)
plt.legend()
plt.gca().set_ylim(0, 1)
plt.show()
## scipy.sparse.csc_matrix is said to be better by casadi author
f_eta_prop_array = np.array(f_eta_prop(rpm, p0))[0]
max_eta_prop = np.nanmax(f_eta_prop_array)
index = np.where(f_eta_prop_array == max_eta_prop)
omega = rpm[0][index][0]
T = np.array(f_T_prop(omega, p0))[0]
Q = np.array(f_Q_prop(omega, p0))[0]
display(Math(r'\eta_{prop, max} = ' + str(max_eta_prop) \
+ r'\ at\ \Omega =\ ' + str(omega) + r'\ \frac{rev}{min}'))
## TODO get ideal motor properties in a dictionary and store them, including motor eta
data = {
'prop_name': prop_name,
'max_eta_prop': max_eta_prop,
'omega': omega,
'T': T,
'Q': Q
}
store(data)
def __interpolate__(self):
self.x_B = []
self.dx_B = []
self.z_B = []
self.dz_B = []
self.q_H = []
self.dq_H = []
self.q_K = []
self.dq_K = []
self.u_K = []
self.u_H = []
if self.debug:
if self.var_dt:
self.dt = self.opti.debug.value(self.h)
else:
self.dt = self.h
for i in range(self.N):
self.x_B.append(self.opti.debug.value(self.states[0, i]))
self.z_B.append(self.opti.debug.value(self.states[1, i]))
self.q_H.append(self.opti.debug.value(self.states[2, i]))
self.q_K.append(self.opti.debug.value(self.states[3, i]))
self.dx_B.append(self.opti.debug.value(self.dstates[0, i]))
self.dz_B.append(self.opti.debug.value(self.dstates[1, i]))
self.dq_H.append(self.opti.debug.value(self.dstates[2, i]))
self.dq_K.append(self.opti.debug.value(self.dstates[3, i]))
self.u_H.append(self.opti.debug.value(self.actions[0, i]))
self.u_K.append(self.opti.debug.value(self.actions[1, i]))
else:
if self.var_dt:
self.dt = self.solution.value(self.h)
else:
self.dt = self.h
for i in range(self.N):
self.x_B.append(self.solution.value(self.states[0, i]))
self.z_B.append(self.solution.value(self.states[1, i]))
self.q_H.append(self.solution.value(self.states[2, i]))
self.q_K.append(self.solution.value(self.states[3, i]))
self.dx_B.append(self.solution.value(self.dstates[0, i]))
self.dz_B.append(self.solution.value(self.dstates[1, i]))
self.dq_H.append(self.solution.value(self.dstates[2, i]))
self.dq_K.append(self.solution.value(self.dstates[3, i]))
self.u_H.append(self.solution.value(self.actions[0, i]))
self.u_K.append(self.solution.value(self.actions[1, i]))
self.k = self.model.dt / 8
self.t = np.linspace(0, self.N * self.dt,
int(self.N * self.dt / self.k))
print(self.dt)
self.t_points = np.linspace(0, self.N * self.dt, self.N)
x_B_spline_function = ca.interpolant('LUT', 'bspline', [self.t_points],
self.x_B)
self.x_B_spline = x_B_spline_function(self.t)
z_B_spline_function = ca.interpolant('LUT', 'bspline', [self.t_points],
self.z_B)
self.z_B_spline = z_B_spline_function(self.t)
q_H_spline_function = ca.interpolant('LUT', 'bspline', [self.t_points],
self.q_H)
self.q_H_spline = q_H_spline_function(self.t)
q_K_spline_function = ca.interpolant('LUT', 'bspline', [self.t_points],
self.q_K)
self.q_K_spline = q_K_spline_function(self.t)
dx_B_spline_function = ca.interpolant('LUT', 'bspline',
[self.t_points], self.dx_B)
self.dx_B_spline = dx_B_spline_function(self.t)
dz_B_spline_function = ca.interpolant('LUT', 'bspline',
[self.t_points], self.dz_B)
self.dz_B_spline = dz_B_spline_function(self.t)
dq_H_spline_function = ca.interpolant('LUT', 'bspline',
[self.t_points], self.dq_H)
self.dq_H_spline = dq_H_spline_function(self.t)
dq_K_spline_function = ca.interpolant('LUT', 'bspline',
[self.t_points], self.dq_K)
self.dq_K_spline = dq_K_spline_function(self.t)
self.__plot__()
def lin_interpolant(self, x: np.ndarray, y: np.ndarray) -> ca.interpolant:
"""Creates a differentiable Casadi interpolant object to linearly interpolate y from x"""
return ca.interpolant('LUT', 'linear', [x], y)
from sys import path
# temp intallation folder of CasADi (pre-compiled version)
path.append(r"/home/theo/software/tools/CasADi/test/casadi-linux-py27-58aa427")
import casadi as ca
import numpy as np
xgrid = np.linspace(1,6,6)
V = [-1,-1,-2,-3,0,2]
lut = ca.interpolant('LUT','bspline',[xgrid],V)
print(lut(2.5))
#############################################
xgrid = np.linspace(-5,5,11)
ygrid = np.linspace(-4,4,9)
X,Y = np.meshgrid(xgrid,ygrid,indexing='ij')
R = np.sqrt(5*X**2 + Y**2)+ 1
data = np.sin(R)/R
data_flat = data.ravel(order='F')
print data.shape
dataXY = np.array([X,Y])
dataXY_flat = dataXY.ravel(order='F')
print dataXY_flat.shape
# lut = ca.interpolant('name','bspline',[xgrid,ygrid],data_flat)
lut = ca.interpolant('name','bspline',data_flat, [X,Y])
print(lut([0.5,1]))
env_map = RaceMap('RaceTrack1020')
s = env_map.start
e = env_map.end
path_finder = PathFinder(env_map.obs_hm._check_line, env_map.start,
env_map.end)
path = path_finder.run_search(5)
# env_map.obs_hm.show_map(True, path)
xgrid = np.linspace(0, 100, 100)
ygrid = np.linspace(0, 100, 100)
# flat_track = track.ravel(order='F')
flat_track = env_map.obs_hm.race_map
flat_track = flat_track.ravel('F')
flat_track = [1 if flat_track[i] else 0 for i in range(len(flat_track))]
lut = ca.interpolant('lut', 'bspline', [xgrid, ygrid], flat_track)
print(lut([90, 48]))
print(lut([48, 90]))
N = len(path)
x = ca.MX.sym('x', N)
y = ca.MX.sym('y', N)
th = ca.MX.sym('th', N)
ds = ca.MX.sym('ds', N)
nlp = {\
'x': ca.vertcat(x, y, th, ds),
'f': ca.sum1(ds) ,#+ ca.sumsqr(th[1:] - th[:-1]) * 1000, # a curve term will be added here
# 'f': ca.sumsqr(th[1:] - th[:-1]),
tunnel_s1 = np.linspace(0, 1, tlength1)
bx = ocp.parameter(NB)
by = ocp.parameter(NB)
br = ocp.parameter(NB)
ocp.set_value(bx, shifted_midpoints_x)
ocp.set_value(by, shifted_midpoints_y)
ocp.set_value(br, shifted_radii)
ocp.subject_to(
ocp.at_tf(s_obs) <= 1
) # effect unclear === the path given is long = first ocp iteration will not get there if dt is small
obs_spline_x = interpolant('x', 'bspline', [tunnel_s1], 1, {
"algorithm": "smooth_linear",
"smooth_linear_frac": 0.49
})
obs_spline_y = interpolant('y', 'bspline', [tunnel_s1], 1, {
"algorithm": "smooth_linear",
"smooth_linear_frac": 0.49
})
obs_spline_r = interpolant('r', 'bspline', [tunnel_s1], 1, {
"algorithm": "smooth_linear",
"smooth_linear_frac": 0.49
})
ocp.subject_to(x > 0)
ocp.subject_to((x - obs_spline_x(s_obs, bx))**2 +
(y - obs_spline_y(s_obs, by))**2 -
(obs_spline_r(s_obs, br))**2 < 0)
def prepare(prop_name, V_inf0, thrust_required, margin, max_volts):
prop_data = prop_db[prop_name]
key0 = list(prop_data['dynamic'].keys())[0]
my_prop = prop_data['dynamic'][key0]['data']
CT_lut = ca.interpolant('CT', 'bspline', [my_prop.index], my_prop.CT)
CP_lut = ca.interpolant('CP', 'bspline', [my_prop.index], my_prop.CP)
eta_prop_lut = ca.interpolant('eta', 'bspline', [my_prop.index],
my_prop.eta)
Q_prop_lut = ca.substitute(Q_prop, CP, CP_lut(J))
T_prop_lut = ca.substitute(T_prop, CT, CT_lut(J))
states = ca.vertcat(rpm_prop)
params = ca.vertcat(rho, r_prop_in, V_inf)
p0 = [1.225, prop_data['D'] / 2, V_inf0]
f_Q_prop = ca.Function('Q_prop', [states, params], [Q_prop_lut])
f_T_prop = ca.Function('T_prop', [states, params], [T_prop_lut])
f_eta_prop = ca.Function('eta_prop', [states, params], [eta_prop_lut(J)])
f_thrustC_prop = ca.Function('CT_prop', [states, params], [CT_lut(J)])
f_powerC_prop = ca.Function('CP_prop', [states, params], [CP_lut(J)])
rpm = np.array([np.linspace(1, 5000, 1000)])
#ct_J = np.around([V_inf0/(r*rpm_to_rps*prop_data['D']) for r in ct_rpm[0]], decimals=4)
fig = plt.figure(figsize=(8, 8))
###ax1 = plt.subplot(511)
###plt.plot(rpm.T, f_Q_prop(rpm, p0).T, label='prop')
#plt.plot(motor_omega, Q_m, '--', label='motor')
###plt.ylabel('torque, N-m')
###plt.legend()
###plt.grid(True)
#ax2 = plt.subplot(512)
#plt.plot(rpm.T, f_T_prop(rpm, p0).T, label='prop')
#plt.ylabel('thrust, N')
#plt.grid(True)
#ax3 = plt.subplot(513)
#plt.plot(rpm.T, f_eta_prop(rpm, p0).T, label='prop')
#plt.xlabel('prop angular velocity, rpm')
#plt.ylabel('efficiency')
#plt.grid(True)
ax4 = plt.subplot(514)
plt.plot(rpm.T, f_thrustC_prop(rpm, p0).T, label='prop')
plt.xlabel('Advance Ratio not')
plt.ylabel('CT')
plt.grid(True)
ax5 = plt.subplot(515)
plt.plot(rpm.T, f_powerC_prop(rpm, p0).T, label='prop')
plt.xlabel('Advance Ratio not')
plt.ylabel('CP')
plt.grid(True)
#ax1.set_ylim([0, 0.01])
#ax1.set_xlim([1,3000])
#ax2.set_ylim([0, 0.5])
#ax2.set_xlim([1,3000])
#ax3.set_ylim([0, 1])
#ax3.set_xlim([1,3000])
ax4.set_ylim([0, 0.2])
ax4.set_xlim([3000, 1])
ax5.set_ylim([0, 0.2])
ax5.set_xlim([3000, 1])
plt.show()
## scipy.sparse.csc_matrix is said to be better by casadi author
f_eta_prop_array = np.array(f_eta_prop(rpm, p0))[0]
max_eta_prop = np.nanmax(f_eta_prop_array)
index = np.where(f_eta_prop_array == max_eta_prop)
omega = rpm[0][index][0]
T = np.array(f_T_prop(omega, p0))[0]
Q = np.array(f_Q_prop(omega, p0))[0]
## values at required thrust
T_array = np.array(f_T_prop(rpm, p0))[0]
proximity = (T_array - thrust_required)**2
index1 = np.where(proximity == np.nanmin(proximity))
T_required = T_array[index1]
omega_required = rpm[0][index1][0]
eta_required = f_eta_prop_array[index1]
Q_required = np.array(f_Q_prop(rpm, p0))[0][index1]
rpm_proximity = (eta_required - max_eta_prop)**2
if T_required > thrust_required - margin:
motordat = md.prepare_m(Q_required, omega_required, max_volts)
#motor0 = list(motordat.keys())[0]
#motor_omega = motordat[motor0]['omega'] / rpm_to_rps
##plot Q_m vs Q_prop??
data = {'prop_name': prop_name, 'max_eta_prop': max_eta_prop, 'omega_max': omega, 'T_max': T,\
'Q_max': Q, 'diameter [in]': prop_data['D'], 'T_required': T_required, 'omega_required': omega_required,\
'eta_required': eta_required, 'Q_required': Q_required, 'rpm_proximity': rpm_proximity}
plot(data, motordat)
store(data)
def __interpolate__(self):
self.x1 = []
self.dx1 = []
self.x2 = []
self.dx2 = []
self.x3 = []
self.dx3 = []
self.u1 = []
self.u2 = []
if self.debug:
if self.var_dt:
self.dt = self.opti.debug.value(self.h)
else:
self.dt = self.h
for i in range(self.N):
self.x1.append(self.opti.debug.value(self.states[i][0]))
self.x2.append(self.opti.debug.value(self.states[i][1]))
self.x3.append(self.opti.debug.value(self.states[i][2]))
self.dx1.append(self.solution.value(self.dstates[i][0]))
self.dx2.append(self.solution.value(self.dstates[i][1]))
self.dx3.append(self.solution.value(self.dstates[i][2]))
self.u1.append(self.opti.debug.value(self.actions[i][0]))
self.u2.append(self.opti.debug.value(self.actions[i][1]))
else:
if self.var_dt:
self.dt = self.solution.value(self.h)
else:
self.dt = self.h
for i in range(self.N):
self.x1.append(self.solution.value(self.states[i][0]))
self.x2.append(self.solution.value(self.states[i][1]))
self.x3.append(self.solution.value(self.states[i][2]))
self.dx1.append(self.solution.value(self.dstates[i][0]))
self.dx2.append(self.solution.value(self.dstates[i][1]))
self.dx3.append(self.solution.value(self.dstates[i][2]))
self.u1.append(self.solution.value(self.actions[i][0]))
self.u2.append(self.solution.value(self.actions[i][1]))
self.k = self.dt / 4
self.t = np.linspace(0, self.T, int(self.T / self.k))
self.t_points = np.linspace(0, self.N * self.dt, self.N)
x1_spline_function = ca.interpolant('LUT', 'bspline', [self.t_points],
self.x1)
self.x1_spline = x1_spline_function(self.t)
x2_spline_function = ca.interpolant('LUT', 'bspline', [self.t_points],
self.x2)
self.x2_spline = x2_spline_function(self.t)
x3_spline_function = ca.interpolant('LUT', 'bspline', [self.t_points],
self.x3)
self.x3_spline = x3_spline_function(self.t)
dx1_spline_function = ca.interpolant('LUT', 'bspline', [self.t_points],
self.dx1)
self.dx1_spline = dx1_spline_function(self.t)
dx2_spline_function = ca.interpolant('LUT', 'bspline', [self.t_points],
self.dx2)
self.dx2_spline = dx2_spline_function(self.t)
dx3_spline_function = ca.interpolant('LUT', 'bspline', [self.t_points],
self.dx3)
self.dx3_spline = dx3_spline_function(self.t)
self.__plot__()
def interpn(points: Tuple[_onp.ndarray],
values: _onp.ndarray,
xi: _onp.ndarray,
method: str = "linear",
bounds_error=True,
fill_value=_onp.NaN) -> _onp.ndarray:
"""
Performs multidimensional interpolation on regular grids. Analogue to scipy.interpolate.interpn().
See syntax here: https://docs.scipy.org/doc/scipy/reference/generated/scipy.interpolate.interpn.html
Args:
points: The points defining the regular grid in n dimensions. Tuple of coordinates of each axis.
values: The data on the regular grid in n dimensions.
xi: The coordinates to sample the gridded data at.
method: The method of interpolation to perform. one of:
* "bspline" (Note: differentiable and suitable for optimization - made of piecewise-cubics. For other
applications, other interpolators may be faster. Not monotonicity-preserving - may overshoot.)
* "linear" (Note: differentiable, but not suitable for use in optimization w/o subgradient treatment due
to C1-discontinuity)
* "nearest" (Note: NOT differentiable, don't use in optimization. Fast.)
bounds_error: If True, when interpolated values are requested outside of the domain of the input data,
a ValueError is raised. If False, then fill_value is used.
fill_value: If provided, the value to use for points outside of the interpolation domain. If None,
values outside the domain are extrapolated.
Returns: Interpolated values at input coordinates.
"""
### Check input types for points and values
if is_casadi_type([points, values], recursive=True):
raise TypeError(
"The underlying dataset (points, values) must consist of NumPy arrays."
)
### Check dimensions of points
for points_axis in points:
points_axis = array(points_axis)
if not len(points_axis.shape) == 1:
raise ValueError(
"`points` must consist of a tuple of 1D ndarrays defining the coordinates of each axis."
)
### Check dimensions of values
implied_values_shape = tuple(len(points_axis) for points_axis in points)
if not values.shape == implied_values_shape:
raise ValueError(f"""
The shape of `values` should be {implied_values_shape}.
""")
if ( ### NumPy implementation
not is_casadi_type([points, values, xi], recursive=True)) and (
(method == "linear") or (method == "nearest")):
xi = _onp.array(xi).reshape((-1, len(implied_values_shape)))
return _interpolate.interpn(points=points,
values=values,
xi=xi,
method=method,
bounds_error=bounds_error,
fill_value=fill_value)
elif ( ### CasADi implementation
(method == "linear") or (method == "bspline")):
### Add handling to patch a specific bug in CasADi that occurs when `values` is all zeros.
### For more information, see: https://github.com/casadi/casadi/issues/2837
if method == "bspline" and all(values == 0):
return zeros_like(xi)
### If xi is an int or float, promote it to an array
if isinstance(xi, int) or isinstance(xi, float):
xi = array([xi])
### If xi is a NumPy array and 1D, convert it to 2D for this.
if not is_casadi_type(xi, recursive=False) and len(xi.shape) != 2:
xi = _onp.reshape(xi, (-1, 1))
### Check that xi is now 2D
if not len(xi.shape) == 2:
raise ValueError(
"`xi` must have the shape (n_points, n_dimensions)!")
### Transpose xi so that xi.shape is [n_points, n_dimensions].
n_dimensions = len(points)
if not len(points) in xi.shape:
raise ValueError(
"`xi` must have the shape (n_points, n_dimensions)!")
if not xi.shape[1] == n_dimensions:
xi = xi.T
assert xi.shape[1] == n_dimensions
### Calculate the minimum and maximum values along each axis.
axis_values_min = [_onp.min(axis_values) for axis_values in points]
axis_values_max = [_onp.max(axis_values) for axis_values in points]
### If fill_value is None, project the xi back onto the nearest point in the domain.
if fill_value is None:
for axis in range(n_dimensions):
xi[:, axis] = where(xi[:, axis] > axis_values_max[axis],
axis_values_max[axis], xi[:, axis])
xi[:, axis] = where(xi[:, axis] < axis_values_min[axis],
axis_values_min[axis], xi[:, axis])
### Check bounds_error
if bounds_error:
if isinstance(xi, _cas.MX):
raise ValueError(
"Can't have the `bounds_error` flag as True if `xi` is of cas.MX type."
)
for axis in range(n_dimensions):
if any(
logical_or(xi[:, axis] > axis_values_max[axis],
xi[:, axis] < axis_values_min[axis])):
raise ValueError(
f"One of the requested xi is out of bounds in dimension {axis}"
)
### Do the interpolation
values_flattened = _onp.ravel(values, order='F')
interpolator = _cas.interpolant('Interpolator', method, points,
values_flattened)
fi = interpolator(xi.T).T
### If fill_value is a scalar, replace all out-of-bounds xi with that value.
if fill_value is not None:
for axis in range(n_dimensions):
fi = where(xi[:, axis] > axis_values_max[axis], fill_value, fi)
fi = where(xi[:, axis] < axis_values_min[axis], fill_value, fi)
### If DM output (i.e. a numeric value), convert that back to an array
if isinstance(fi, _cas.DM):
if fi.shape == (1, 1):
return float(fi)
else:
return _onp.array(fi, dtype=float).reshape(-1)
return fi
else:
raise ValueError("Bad value of `method`!")
def exitExpression(self, tree):
if isinstance(tree.operator, ast.ComponentRef):
op = tree.operator.name
else:
op = tree.operator
if op == '*':
op = 'mtimes' # .* differs from *
if op.startswith('.'):
op = op[1:]
logger.debug('exitExpression')
n_operands = len(tree.operands)
if op == 'der':
v = self.get_mx(tree.operands[0])
src = self.get_derivative(v)
elif op == '-' and n_operands == 1:
src = -self.get_mx(tree.operands[0])
elif op == 'not' and n_operands == 1:
src = ca.if_else(self.get_mx(tree.operands[0]), 0, 1, True)
elif op == 'mtimes':
assert n_operands >= 2
src = self.get_mx(tree.operands[0])
for i in tree.operands[1:]:
src = ca.mtimes(src, self.get_mx(i))
elif op == 'transpose' and n_operands == 1:
src = self.get_mx(tree.operands[0]).T
elif op == 'sum' and n_operands == 1:
v = self.get_mx(tree.operands[0])
src = ca.sum1(v)
elif op == 'linspace' and n_operands == 3:
a = self.get_mx(tree.operands[0])
b = self.get_mx(tree.operands[1])
n_steps = self.get_integer(tree.operands[2])
src = ca.linspace(a, b, n_steps)
elif op == 'fill' and n_operands == 2:
val = self.get_mx(tree.operands[0])
n_row = self.get_integer(tree.operands[1])
src = val * ca.DM.ones(n_row)
elif op == 'fill' and n_operands == 3:
val = self.get_mx(tree.operands[0])
n_row = self.get_integer(tree.operands[1])
n_col = self.get_integer(tree.operands[2])
src = val * ca.DM.ones(n_row, n_col)
elif op == 'zeros' and n_operands == 1:
n_row = self.get_integer(tree.operands[0])
src = ca.DM.zeros(n_row)
elif op == 'zeros' and n_operands == 2:
n_row = self.get_integer(tree.operands[0])
n_col = self.get_integer(tree.operands[1])
src = ca.DM.zeros(n_row, n_col)
elif op == 'ones' and n_operands == 1:
n_row = self.get_integer(tree.operands[0])
src = ca.DM.ones(n_row)
elif op == 'ones' and n_operands == 2:
n_row = self.get_integer(tree.operands[0])
n_col = self.get_integer(tree.operands[1])
src = ca.DM.ones(n_row, n_col)
elif op == 'identity' and n_operands == 1:
n = self.get_integer(tree.operands[0])
src = ca.DM.eye(n)
elif op == 'diagonal' and n_operands == 1:
diag = self.get_mx(tree.operands[0])
n = len(diag)
indices = list(range(n))
src = ca.DM.triplet(indices, indices, diag, n, n)
elif op == 'cat':
axis = self.get_integer(tree.operands[0])
assert axis == 1, "Currently only concatenation on first axis is supported"
entries = []
for sym in [self.get_mx(op) for op in tree.operands[1:]]:
if isinstance(sym, list):
for e in sym:
entries.append(e)
else:
entries.append(sym)
src = ca.vertcat(*entries)
elif op == 'delay' and n_operands == 2:
expr = self.get_mx(tree.operands[0])
duration = self.get_mx(tree.operands[1])
src = _new_mx('_pymoca_delay_{}'.format(self.delay_counter), *expr.size())
self.delay_counter += 1
for f in self.for_loops:
syms = set(ca.symvar(expr))
if syms.intersection(f.indexed_symbols):
f.register_indexed_symbol(src, lambda i: i, True, tree.operands[0], f.index_variable)
self.model.delay_states.append(src.name())
self.model.inputs.append(Variable(src))
delay_argument = DelayArgument(expr, duration)
self.model.delay_arguments.append(delay_argument)
elif op == '_pymoca_interp1d' and n_operands >= 3 and n_operands <= 4:
entered_class = self.entered_classes[-1]
if isinstance(tree.operands[0], ast.ComponentRef):
xp = self.get_mx(entered_class.symbols[tree.operands[0].name].value)
else:
xp = self.get_mx(tree.operands[0])
if isinstance(tree.operands[1], ast.ComponentRef):
yp = self.get_mx(entered_class.symbols[tree.operands[1].name].value)
else:
yp = self.get_mx(tree.operands[1])
arg = self.get_mx(tree.operands[2])
if n_operands == 4:
assert isinstance(tree.operands[3], ast.Primary)
mode = tree.operands[3].value
else:
mode = 'linear'
func = ca.interpolant('interpolant', mode, [xp], yp)
src = func(arg)
elif op == '_pymoca_interp2d' and n_operands >= 5 and n_operands <= 6:
entered_class = self.entered_classes[-1]
if isinstance(tree.operands[0], ast.ComponentRef):
xp = self.get_mx(entered_class.symbols[tree.operands[0].name].value)
else:
xp = self.get_mx(tree.operands[0])
if isinstance(tree.operands[1], ast.ComponentRef):
yp = self.get_mx(entered_class.symbols[tree.operands[1].name].value)
else:
yp = self.get_mx(tree.operands[1])
if isinstance(tree.operands[2], ast.ComponentRef):
zp = self.get_mx(entered_class.symbols[tree.operands[2].name].value)
else:
zp = self.get_mx(tree.operands[2])
arg_1 = self.get_mx(tree.operands[3])
arg_2 = self.get_mx(tree.operands[4])
if n_operands == 6:
assert isinstance(tree.operands[5], ast.Primary)
mode = tree.operands[5].value
else:
mode = 'linear'
func = ca.interpolant('interpolant', mode, [xp, yp], np.array(zp).ravel(order='F'))
src = func(ca.vertcat(arg_1, arg_2))
elif op in OP_MAP and n_operands == 2:
lhs = ca.MX(self.get_mx(tree.operands[0]))
rhs = ca.MX(self.get_mx(tree.operands[1]))
lhs_op = getattr(lhs, OP_MAP[op])
src = lhs_op(rhs)
elif op in OP_MAP and n_operands == 1:
lhs = ca.MX(self.get_mx(tree.operands[0]))
lhs_op = getattr(lhs, OP_MAP[op])
src = lhs_op()
else:
src = ca.MX(self.get_mx(tree.operands[0]))
# Check for built-in operations, such as the
# elementary functions, first.
if hasattr(src, op) and n_operands <= 2:
if n_operands == 1:
src = ca.MX(self.get_mx(tree.operands[0]))
src = getattr(src, op)()
else:
lhs = ca.MX(self.get_mx(tree.operands[0]))
rhs = ca.MX(self.get_mx(tree.operands[1]))
lhs_op = getattr(lhs, op)
src = lhs_op(rhs)
else:
func = self.get_function(op)
src = ca.vertcat(*func.call([self.get_mx(operand) for operand in tree.operands], *self.function_mode))
self.src[tree] = src
def exitExpression(self, tree):
if isinstance(tree.operator, ast.ComponentRef):
op = tree.operator.name
else:
op = tree.operator
if op == '*':
op = 'mtimes' # .* differs from *
if op.startswith('.'):
op = op[1:]
logger.debug('exitExpression')
n_operands = len(tree.operands)
if op == 'der':
v = self.get_mx(tree.operands[0])
src = self.get_derivative(v)
elif op == '-' and n_operands == 1:
src = -self.get_mx(tree.operands[0])
elif op == 'not' and n_operands == 1:
src = ca.if_else(self.get_mx(tree.operands[0]), 0, 1, True)
elif op == 'mtimes':
assert n_operands >= 2
src = self.get_mx(tree.operands[0])
for i in tree.operands[1:]:
src = ca.mtimes(src, self.get_mx(i))
elif op == 'transpose' and n_operands == 1:
src = self.get_mx(tree.operands[0]).T
elif op == 'sum' and n_operands == 1:
v = self.get_mx(tree.operands[0])
src = ca.sum1(v)
elif op == 'linspace' and n_operands == 3:
a = self.get_mx(tree.operands[0])
b = self.get_mx(tree.operands[1])
n_steps = self.get_integer(tree.operands[2])
src = ca.linspace(a, b, n_steps)
elif op == 'fill' and n_operands == 2:
val = self.get_mx(tree.operands[0])
n_row = self.get_integer(tree.operands[1])
src = val * ca.DM.ones(n_row)
elif op == 'fill' and n_operands == 3:
val = self.get_mx(tree.operands[0])
n_row = self.get_integer(tree.operands[1])
n_col = self.get_integer(tree.operands[2])
src = val * ca.DM.ones(n_row, n_col)
elif op == 'zeros' and n_operands == 1:
n_row = self.get_integer(tree.operands[0])
src = ca.DM.zeros(n_row)
elif op == 'zeros' and n_operands == 2:
n_row = self.get_integer(tree.operands[0])
n_col = self.get_integer(tree.operands[1])
src = ca.DM.zeros(n_row, n_col)
elif op == 'ones' and n_operands == 1:
n_row = self.get_integer(tree.operands[0])
src = ca.DM.ones(n_row)
elif op == 'ones' and n_operands == 2:
n_row = self.get_integer(tree.operands[0])
n_col = self.get_integer(tree.operands[1])
src = ca.DM.ones(n_row, n_col)
elif op == 'identity' and n_operands == 1:
n = self.get_integer(tree.operands[0])
src = ca.DM.eye(n)
elif op == 'diagonal' and n_operands == 1:
diag = self.get_mx(tree.operands[0])
n = len(diag)
indices = list(range(n))
src = ca.DM.triplet(indices, indices, diag, n, n)
elif op == 'cat':
axis = self.get_integer(tree.operands[0])
assert axis == 1, "Currently only concatenation on first axis is supported"
entries = []
for sym in [self.get_mx(op) for op in tree.operands[1:]]:
if isinstance(sym, list):
for e in sym:
entries.append(e)
else:
entries.append(sym)
src = ca.vertcat(*entries)
elif op == 'delay' and n_operands == 2:
expr = self.get_mx(tree.operands[0])
duration = self.get_mx(tree.operands[1])
src = _new_mx('_pymoca_delay_{}'.format(self.delay_counter), *expr.size())
self.delay_counter += 1
for f in self.for_loops:
syms = set(ca.symvar(expr))
if syms.intersection(f.indexed_symbols):
f.register_indexed_symbol(src, lambda i: i, True, tree.operands[0], f.index_variable)
self.model.delay_states.append(src.name())
self.model.inputs.append(Variable(src))
delay_argument = DelayArgument(expr, duration)
self.model.delay_arguments.append(delay_argument)
elif op == '_pymoca_interp1d' and n_operands >= 3 and n_operands <= 4:
entered_class = self.entered_classes[-1]
if isinstance(tree.operands[0], ast.ComponentRef):
xp = self.get_mx(entered_class.symbols[tree.operands[0].name].value)
else:
xp = self.get_mx(tree.operands[0])
if isinstance(tree.operands[1], ast.ComponentRef):
yp = self.get_mx(entered_class.symbols[tree.operands[1].name].value)
else:
yp = self.get_mx(tree.operands[1])
arg = self.get_mx(tree.operands[2])
if n_operands == 4:
assert isinstance(tree.operands[3], ast.Primary)
mode = tree.operands[3].value
else:
mode = 'linear'
func = ca.interpolant('interpolant', mode, [xp], yp)
src = func(arg)
elif op == '_pymoca_interp2d' and n_operands >= 5 and n_operands <= 6:
entered_class = self.entered_classes[-1]
if isinstance(tree.operands[0], ast.ComponentRef):
xp = self.get_mx(entered_class.symbols[tree.operands[0].name].value)
else:
xp = self.get_mx(tree.operands[0])
if isinstance(tree.operands[1], ast.ComponentRef):
yp = self.get_mx(entered_class.symbols[tree.operands[1].name].value)
else:
yp = self.get_mx(tree.operands[1])
if isinstance(tree.operands[2], ast.ComponentRef):
zp = self.get_mx(entered_class.symbols[tree.operands[2].name].value)
else:
zp = self.get_mx(tree.operands[2])
arg_1 = self.get_mx(tree.operands[3])
arg_2 = self.get_mx(tree.operands[4])
if n_operands == 6:
assert isinstance(tree.operands[5], ast.Primary)
mode = tree.operands[5].value
else:
mode = 'linear'
func = ca.interpolant('interpolant', mode, [xp, yp], np.array(zp).ravel(order='F'))
src = func(ca.vertcat(arg_1, arg_2))
elif op in OP_MAP and n_operands == 2:
lhs = ca.MX(self.get_mx(tree.operands[0]))
rhs = ca.MX(self.get_mx(tree.operands[1]))
lhs_op = getattr(lhs, OP_MAP[op])
src = lhs_op(rhs)
elif op in OP_MAP and n_operands == 1:
lhs = ca.MX(self.get_mx(tree.operands[0]))
lhs_op = getattr(lhs, OP_MAP[op])
src = lhs_op()
else:
src = ca.MX(self.get_mx(tree.operands[0]))
# Check for built-in operations, such as the
# elementary functions, first.
if hasattr(src, op) and n_operands <= 2:
if n_operands == 1:
src = ca.MX(self.get_mx(tree.operands[0]))
src = getattr(src, op)()
else:
lhs = ca.MX(self.get_mx(tree.operands[0]))
rhs = ca.MX(self.get_mx(tree.operands[1]))
lhs_op = getattr(lhs, op)
src = lhs_op(rhs)
else:
try: # Check if there is a component named as the operation. In that case we are dealing with a time access
# Should we check for symbol as well?
v = self.get_mx(ast.ComponentRef(name=op))
t = self.get_mx(tree.operands[0])
src = self.get_symbol_time_access(v, t)
except KeyError:
func = self.get_function(op)
src = ca.vertcat(*func.call([self.get_mx(operand) for operand in tree.operands], *self.function_mode))
self.src[tree] = src
def MinCurvatureTrajectory(track, obs_map=None):
w_min = -track[:, 4] * 0.9
w_max = track[:, 5] * 0.9
nvecs = track[:, 2:4]
th_ns = [lib.get_bearing([0, 0], nvecs[i, 0:2]) for i in range(len(nvecs))]
xgrid = np.arange(0, obs_map.shape[1])
ygrid = np.arange(0, obs_map.shape[0])
data_flat = np.array(obs_map).ravel(order='F')
lut = ca.interpolant('lut', 'bspline', [xgrid, ygrid], data_flat)
print(lut([10, 20]))
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 = 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.atan2(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]))])
d_n = ca.Function('d_n', [n_f_a, th_f],
[track_length(n_f_a) / ca.tan(get_th_n(th_f))])
# objective
real1 = ca.Function(
'real1', [th1_f1, th2_f1],
[ca.cos(th1_f1) * ca.cos(th2_f1) + ca.sin(th1_f1) * ca.sin(th2_f1)])
im1 = ca.Function(
'im1', [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_cpx1', [th1_f1, th2_f1],
[ca.atan2(im1(th1_f1, th2_f1), real1(th1_f1, th2_f1))])
# define symbols
n = ca.MX.sym('n', N)
th = ca.MX.sym('th', N - 1)
nlp = {\
'x': ca.vertcat(n, th),
'f': ca.sumsqr(sub_cmplx1(th[1:], th[:-1])),
# 'f': ca.sumsqr(track_length(n)),
'g': ca.vertcat(
# dynamic constraints
n[1:] - (n[:-1] + d_n(n, th)),
# lut(ca.horzcat(o_x_s(n[:-1]), o_y_s(n[:-1])).T).T,
# boundary constraints
n[0], #th[0],
n[-1], #th[-1],
) \
}
S = ca.nlpsol('S', 'ipopt', nlp, {'ipopt': {'print_level': 5}})
# S = ca.nlpsol('S', 'ipopt', nlp, {'ipopt':{'print_level':0}})
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 = np.array(th0)
x0 = ca.vertcat(n0, th0)
# lbx = [-n_max] * N + [-np.pi]*(N-1)
# ubx = [n_max] * N + [np.pi]*(N-1)
lbx = list(w_min) + [-np.pi] * (N - 1)
ubx = list(w_max) + [np.pi] * (N - 1)
r = S(x0=x0, lbg=0, ubg=0, lbx=lbx, ubx=ubx)
x_opt = r['x']
n_set = np.array(x_opt[:N])
thetas = np.array(x_opt[1 * N:2 * (N - 1)])
# lib.plot_race_line(np.array(track), n_set, wait=True)
return n_set
