189 lines
5.9 KiB
Python
Executable File
189 lines
5.9 KiB
Python
Executable File
#! /usr/bin/env python
|
|
# type: ignore
|
|
|
|
import numpy as np
|
|
from collections import OrderedDict
|
|
import matplotlib.pyplot as plt
|
|
from selfdrive.car.honda.interface import CarInterface
|
|
from selfdrive.controls.lib.lateral_mpc import libmpc_py
|
|
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
|
|
|
# plot lateral MPC trajectory by defining boundary conditions:
|
|
# lane lines, p_poly and vehicle states. Use this script to tune MPC costs
|
|
|
|
libmpc = libmpc_py.libmpc
|
|
|
|
mpc_solution = libmpc_py.ffi.new("log_t *")
|
|
|
|
points_l = np.array([1.1049711, 1.1053879, 1.1073375, 1.1096942, 1.1124474, 1.1154714, 1.1192677, 1.1245866, 1.1321017, 1.1396152, 1.146443, 1.1555313, 1.1662073, 1.1774249, 1.1888939, 1.2009926, 1.2149779, 1.2300836, 1.2450289, 1.2617753, 1.2785473, 1.2974714, 1.3151019, 1.3331807, 1.3545501, 1.3763691, 1.3983455, 1.4215056, 1.4446729, 1.4691089, 1.4927692, 1.5175346, 1.5429921, 1.568854, 1.5968665, 1.6268958, 1.657122, 1.6853137, 1.7152609, 1.7477539, 1.7793678, 1.8098511, 1.8428392, 1.8746407, 1.9089606, 1.9426043, 1.9775689, 2.0136933, 2.0520134, 2.0891454])
|
|
|
|
points_r = np.array([-2.4442139, -2.4449506, -2.4448867, -2.44377, -2.4422617, -2.4393811, -2.4374201, -2.4334245, -2.4286852, -2.4238286, -2.4177458, -2.4094386, -2.3994849, -2.3904033, -2.380136, -2.3699453, -2.3594661, -2.3474073, -2.3342307, -2.3194637, -2.3046403, -2.2881098, -2.2706163, -2.2530098, -2.235604, -2.2160542, -2.1967411, -2.1758952, -2.1544619, -2.1325269, -2.1091819, -2.0850561, -2.0621953, -2.0364127, -2.0119917, -1.9851667, -1.9590458, -1.9306552, -1.9024918, -1.8745357, -1.8432863, -1.8131843, -1.7822732, -1.7507075, -1.7180918, -1.6845931, -1.650871, -1.6157099, -1.5787286, -1.5418037])
|
|
|
|
|
|
points_c = (points_l + points_r) / 2.0
|
|
|
|
def compute_path_pinv():
|
|
deg = 3
|
|
x = np.arange(50.0)
|
|
X = np.vstack(tuple(x**n for n in range(deg, -1, -1))).T
|
|
pinv = np.linalg.pinv(X)
|
|
return pinv
|
|
|
|
|
|
def model_polyfit(points):
|
|
path_pinv = compute_path_pinv()
|
|
return np.dot(path_pinv, map(float, points))
|
|
|
|
|
|
xx = []
|
|
yy = []
|
|
deltas = []
|
|
psis = []
|
|
times = []
|
|
|
|
CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING")
|
|
VM = VehicleModel(CP)
|
|
|
|
v_ref = 32.00 # 45 mph
|
|
curvature_factor = VM.curvature_factor(v_ref)
|
|
print(curvature_factor)
|
|
|
|
LANE_WIDTH = 3.9
|
|
p_l = map(float, model_polyfit(points_l))
|
|
p_r = map(float, model_polyfit(points_r))
|
|
p_p = map(float, model_polyfit(points_c))
|
|
|
|
l_poly = libmpc_py.ffi.new("double[4]", p_l)
|
|
r_poly = libmpc_py.ffi.new("double[4]", p_r)
|
|
p_poly = libmpc_py.ffi.new("double[4]", p_p)
|
|
l_prob = 1.0
|
|
r_prob = 1.0
|
|
p_prob = 1.0 # This is always 1
|
|
|
|
|
|
mpc_x_points = np.linspace(0., 2.5*v_ref, num=50)
|
|
points_poly_l = np.polyval(p_l, mpc_x_points)
|
|
points_poly_r = np.polyval(p_r, mpc_x_points)
|
|
points_poly_p = np.polyval(p_p, mpc_x_points)
|
|
print(points_poly_l)
|
|
|
|
lanes_x = np.linspace(0, 49)
|
|
|
|
cur_state = libmpc_py.ffi.new("state_t *")
|
|
cur_state[0].x = 0.0
|
|
cur_state[0].y = 0.5
|
|
cur_state[0].psi = 0.0
|
|
cur_state[0].delta = 0.0
|
|
|
|
xs = []
|
|
ys = []
|
|
deltas = []
|
|
titles = [
|
|
'Steer rate cost',
|
|
'Heading cost',
|
|
'Lane cost',
|
|
'Path cost',
|
|
]
|
|
|
|
# Steer rate cost
|
|
sol_x = OrderedDict()
|
|
sol_y = OrderedDict()
|
|
delta = OrderedDict()
|
|
for cost in np.logspace(-1, 1.0, 5):
|
|
libmpc.init(1.0, 3.0, 1.0, cost)
|
|
for _ in range(10):
|
|
libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, p_poly, l_prob, r_prob,
|
|
curvature_factor, v_ref, LANE_WIDTH)
|
|
sol_x[cost] = map(float, list(mpc_solution[0].x))
|
|
sol_y[cost] = map(float, list(mpc_solution[0].y))
|
|
delta[cost] = map(float, list(mpc_solution[0].delta))
|
|
xs.append(sol_x)
|
|
ys.append(sol_y)
|
|
deltas.append(delta)
|
|
|
|
# Heading cost
|
|
sol_x = OrderedDict()
|
|
sol_y = OrderedDict()
|
|
delta = OrderedDict()
|
|
for cost in np.logspace(-1, 1.0, 5):
|
|
libmpc.init(1.0, 3.0, cost, 1.0)
|
|
for _ in range(10):
|
|
libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, p_poly, l_prob, r_prob,
|
|
curvature_factor, v_ref, LANE_WIDTH)
|
|
sol_x[cost] = map(float, list(mpc_solution[0].x))
|
|
sol_y[cost] = map(float, list(mpc_solution[0].y))
|
|
delta[cost] = map(float, list(mpc_solution[0].delta))
|
|
xs.append(sol_x)
|
|
ys.append(sol_y)
|
|
deltas.append(delta)
|
|
|
|
# Lane cost
|
|
sol_x = OrderedDict()
|
|
sol_y = OrderedDict()
|
|
delta = OrderedDict()
|
|
for cost in np.logspace(-1, 2.0, 5):
|
|
libmpc.init(1.0, cost, 1.0, 1.0)
|
|
for _ in range(10):
|
|
libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, p_poly, l_prob, r_prob,
|
|
curvature_factor, v_ref, LANE_WIDTH)
|
|
sol_x[cost] = map(float, list(mpc_solution[0].x))
|
|
sol_y[cost] = map(float, list(mpc_solution[0].y))
|
|
delta[cost] = map(float, list(mpc_solution[0].delta))
|
|
xs.append(sol_x)
|
|
ys.append(sol_y)
|
|
deltas.append(delta)
|
|
|
|
|
|
# Path cost
|
|
sol_x = OrderedDict()
|
|
sol_y = OrderedDict()
|
|
delta = OrderedDict()
|
|
for cost in np.logspace(-1, 1.0, 5):
|
|
libmpc.init(cost, 3.0, 1.0, 1.0)
|
|
for _ in range(10):
|
|
libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, p_poly, l_prob, r_prob,
|
|
curvature_factor, v_ref, LANE_WIDTH)
|
|
sol_x[cost] = map(float, list(mpc_solution[0].x))
|
|
sol_y[cost] = map(float, list(mpc_solution[0].y))
|
|
delta[cost] = map(float, list(mpc_solution[0].delta))
|
|
xs.append(sol_x)
|
|
ys.append(sol_y)
|
|
deltas.append(delta)
|
|
|
|
|
|
|
|
plt.figure()
|
|
|
|
for i in range(len(xs)):
|
|
ax = plt.subplot(2, 2, i + 1)
|
|
sol_x = xs[i]
|
|
sol_y = ys[i]
|
|
for cost in sol_x.keys():
|
|
plt.plot(sol_x[cost], sol_y[cost])
|
|
|
|
plt.plot(lanes_x, points_r, '.b')
|
|
plt.plot(lanes_x, points_l, '.b')
|
|
plt.plot(lanes_x, (points_l + points_r) / 2.0, '--g')
|
|
plt.plot(mpc_x_points, points_poly_l, 'b')
|
|
plt.plot(mpc_x_points, points_poly_r, 'b')
|
|
plt.plot(mpc_x_points, (points_poly_l + points_poly_r) / 2.0, 'g')
|
|
plt.legend(map(lambda x: str(round(x, 2)), sol_x.keys()) + ['right', 'left', 'center'], loc=3)
|
|
plt.title(titles[i])
|
|
plt.grid(True)
|
|
# ax.set_aspect('equal', 'datalim')
|
|
|
|
|
|
plt.figure()
|
|
for i in range(len(xs)):
|
|
plt.subplot(2, 2, i + 1)
|
|
sol_x = xs[i]
|
|
delta = deltas[i]
|
|
|
|
for cost in sol_x.keys():
|
|
plt.plot(delta[cost])
|
|
plt.title(titles[i])
|
|
plt.legend(map(lambda x: str(round(x, 2)), sol_x.keys()), loc=3)
|
|
plt.grid(True)
|
|
|
|
plt.show()
|