openpilot_comma/selfdrive/debug/mpc/tune_lateral.py

#! /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()