|
|
|
|
#! /usr/bin/env python
|
|
|
|
|
# type: ignore
|
|
|
|
|
import matplotlib.pyplot as plt
|
|
|
|
|
from selfdrive.controls.lib.lateral_mpc import libmpc_py
|
|
|
|
|
from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT
|
|
|
|
|
import math
|
|
|
|
|
|
|
|
|
|
libmpc = libmpc_py.libmpc
|
|
|
|
|
libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, 1.)
|
|
|
|
|
|
|
|
|
|
cur_state = libmpc_py.ffi.new("state_t *")
|
|
|
|
|
cur_state[0].x = 0.0
|
|
|
|
|
cur_state[0].y = 0.0
|
|
|
|
|
cur_state[0].psi = 0.0
|
|
|
|
|
cur_state[0].delta = 0.0
|
|
|
|
|
|
|
|
|
|
mpc_solution = libmpc_py.ffi.new("log_t *")
|
|
|
|
|
xx = []
|
|
|
|
|
yy = []
|
|
|
|
|
deltas = []
|
|
|
|
|
psis = []
|
|
|
|
|
times = []
|
|
|
|
|
|
|
|
|
|
curvature_factor = 0.3
|
|
|
|
|
v_ref = 1.0 * 20.12 # 45 mph
|
|
|
|
|
|
|
|
|
|
LANE_WIDTH = 3.7
|
|
|
|
|
p = [0.0, 0.0, 0.0, 0.0]
|
|
|
|
|
p_l = p[:]
|
|
|
|
|
p_l[3] += LANE_WIDTH / 2.0
|
|
|
|
|
|
|
|
|
|
p_r = p[:]
|
|
|
|
|
p_r[3] -= LANE_WIDTH / 2.0
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
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)
|
|
|
|
|
|
|
|
|
|
l_prob = 1.0
|
|
|
|
|
r_prob = 1.0
|
|
|
|
|
p_prob = 1.0
|
|
|
|
|
|
|
|
|
|
for i in range(1):
|
|
|
|
|
cur_state[0].delta = math.radians(510. / 13.)
|
|
|
|
|
libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, p_poly, l_prob, r_prob,
|
|
|
|
|
curvature_factor, v_ref, LANE_WIDTH)
|
|
|
|
|
|
|
|
|
|
timesi = []
|
|
|
|
|
ct = 0
|
|
|
|
|
for i in range(21):
|
|
|
|
|
timesi.append(ct)
|
|
|
|
|
if i <= 4:
|
|
|
|
|
ct += 0.05
|
|
|
|
|
else:
|
|
|
|
|
ct += 0.15
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
xi = list(mpc_solution[0].x)
|
|
|
|
|
yi = list(mpc_solution[0].y)
|
|
|
|
|
psii = list(mpc_solution[0].psi)
|
|
|
|
|
deltai = list(mpc_solution[0].delta)
|
|
|
|
|
print("COST: ", mpc_solution[0].cost)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
plt.figure(0)
|
|
|
|
|
plt.subplot(3, 1, 1)
|
|
|
|
|
plt.plot(timesi, psii)
|
|
|
|
|
plt.ylabel('psi')
|
|
|
|
|
plt.grid(True)
|
|
|
|
|
plt.subplot(3, 1, 2)
|
|
|
|
|
plt.plot(timesi, deltai)
|
|
|
|
|
plt.ylabel('delta')
|
|
|
|
|
plt.grid(True)
|
|
|
|
|
plt.subplot(3, 1, 3)
|
|
|
|
|
plt.plot(timesi, yi)
|
|
|
|
|
plt.ylabel('y')
|
|
|
|
|
plt.grid(True)
|
|
|
|
|
plt.show()
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#### UNCOMMENT TO CHECK ITERATIVE SOLUTION
|
|
|
|
|
####
|
|
|
|
|
####for i in range(100):
|
|
|
|
|
#### libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, p_poly, l_prob, r_prob,
|
|
|
|
|
#### curvature_factor, v_ref, LANE_WIDTH)
|
|
|
|
|
#### print "x", list(mpc_solution[0].x)
|
|
|
|
|
#### print "y", list(mpc_solution[0].y)
|
|
|
|
|
#### print "delta", list(mpc_solution[0].delta)
|
|
|
|
|
#### print "psi", list(mpc_solution[0].psi)
|
|
|
|
|
#### # cur_state[0].x = mpc_solution[0].x[1]
|
|
|
|
|
#### # cur_state[0].y = mpc_solution[0].y[1]
|
|
|
|
|
#### # cur_state[0].psi = mpc_solution[0].psi[1]
|
|
|
|
|
#### cur_state[0].delta = radians(200 / 13.)#mpc_solution[0].delta[1]
|
|
|
|
|
####
|
|
|
|
|
#### xx.append(cur_state[0].x)
|
|
|
|
|
#### yy.append(cur_state[0].y)
|
|
|
|
|
#### psis.append(cur_state[0].psi)
|
|
|
|
|
#### deltas.append(cur_state[0].delta)
|
|
|
|
|
#### times.append(i * 0.05)
|
|
|
|
|
####
|
|
|
|
|
####
|
|
|
|
|
####def f(x):
|
|
|
|
|
#### return p_poly[0] * x**3 + p_poly[1] * x**2 + p_poly[2] * x + p_poly[3]
|
|
|
|
|
####
|
|
|
|
|
####
|
|
|
|
|
##### planned = map(f, xx)
|
|
|
|
|
##### plt.figure(1)
|
|
|
|
|
##### plt.plot(yy, xx, 'r-')
|
|
|
|
|
##### plt.plot(planned, xx, 'b--', linewidth=0.5)
|
|
|
|
|
##### plt.axes().set_aspect('equal', 'datalim')
|
|
|
|
|
##### plt.gca().invert_xaxis()
|
|
|
|
|
####
|
|
|
|
|
##### planned = map(f, map(float, list(mpc_solution[0].x)[1:]))
|
|
|
|
|
##### plt.figure(1)
|
|
|
|
|
##### plt.plot(map(float, list(mpc_solution[0].y)[1:]), map(float, list(mpc_solution[0].x)[1:]), 'r-')
|
|
|
|
|
##### plt.plot(planned, map(float, list(mpc_solution[0].x)[1:]), 'b--', linewidth=0.5)
|
|
|
|
|
##### plt.axes().set_aspect('equal', 'datalim')
|
|
|
|
|
##### plt.gca().invert_xaxis()
|
|
|
|
|
####
|
|
|
|
|
####plt.figure(2)
|
|
|
|
|
####plt.subplot(2, 1, 1)
|
|
|
|
|
####plt.plot(times, psis)
|
|
|
|
|
####plt.ylabel('psi')
|
|
|
|
|
####plt.subplot(2, 1, 2)
|
|
|
|
|
####plt.plot(times, deltas)
|
|
|
|
|
####plt.ylabel('delta')
|
|
|
|
|
####
|
|
|
|
|
####
|
|
|
|
|
####plt.show()
|
