mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-08-21 08:14:00 +08:00
Refactor lateral lag compensation (#21334)
* add T_IDXS * refactor * fix test * unused * typo * needs casting * Update selfdrive/controls/lib/drive_helpers.py Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com> * deprecate field * regen all * new segs * add todo * split back * clean * bad names * do in controls * add arg Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com> old-commit-hash: 6838e1c82c5abb9f347228006219d96744b3fbb5
This commit is contained in:
HaraldSchafer
@@ -1,109 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
import matplotlib
|
||||
matplotlib.use('TkAgg')
|
||||
|
||||
import sys
|
||||
import cereal.messaging as messaging
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
# debug liateral MPC by plotting its trajectory. To receive liveLongitudinalMpc packets,
|
||||
# set on LOG_MPC env variable and run plannerd on a replay
|
||||
|
||||
|
||||
def mpc_vwr_thread(addr="127.0.0.1"):
|
||||
|
||||
plt.ion()
|
||||
fig = plt.figure(figsize=(15, 20))
|
||||
ax = fig.add_subplot(131)
|
||||
aa = fig.add_subplot(132, sharey=ax)
|
||||
ap = fig.add_subplot(133, sharey=ax)
|
||||
|
||||
ax.set_xlim([-10, 10])
|
||||
ax.set_ylim([0., 100.])
|
||||
aa.set_xlim([-20., 20])
|
||||
ap.set_xlim([-5, 5])
|
||||
|
||||
ax.set_xlabel('x [m]')
|
||||
ax.set_ylabel('y [m]')
|
||||
aa.set_xlabel('steer_angle [deg]')
|
||||
ap.set_xlabel('asset angle [deg]')
|
||||
ax.grid(True)
|
||||
aa.grid(True)
|
||||
ap.grid(True)
|
||||
|
||||
path_x = np.arange(0, 100)
|
||||
mpc_path_x = np.arange(0, 49)
|
||||
|
||||
p_path_y = np.zeros(100)
|
||||
|
||||
l_path_y = np.zeros(100)
|
||||
r_path_y = np.zeros(100)
|
||||
mpc_path_y = np.zeros(49)
|
||||
mpc_steer_angle = np.zeros(49)
|
||||
mpc_psi = np.zeros(49)
|
||||
|
||||
line1, = ax.plot(mpc_path_y, mpc_path_x)
|
||||
# line1b, = ax.plot(mpc_path_y, mpc_path_x, 'o')
|
||||
|
||||
lineP, = ax.plot(p_path_y, path_x)
|
||||
lineL, = ax.plot(l_path_y, path_x)
|
||||
lineR, = ax.plot(r_path_y, path_x)
|
||||
line3, = aa.plot(mpc_steer_angle, mpc_path_x)
|
||||
line4, = ap.plot(mpc_psi, mpc_path_x)
|
||||
ax.invert_xaxis()
|
||||
aa.invert_xaxis()
|
||||
plt.show()
|
||||
|
||||
# *** log ***
|
||||
livempc = messaging.sub_sock('liveMpc', addr=addr)
|
||||
model = messaging.sub_sock('model', addr=addr)
|
||||
path_plan_sock = messaging.sub_sock('lateralPlan', addr=addr)
|
||||
|
||||
while 1:
|
||||
lMpc = messaging.recv_sock(livempc, wait=True)
|
||||
md = messaging.recv_sock(model)
|
||||
pp = messaging.recv_sock(path_plan_sock)
|
||||
|
||||
if md is not None:
|
||||
p_poly = np.array(md.model.path.poly)
|
||||
l_poly = np.array(md.model.leftLane.poly)
|
||||
r_poly = np.array(md.model.rightLane.poly)
|
||||
|
||||
p_path_y = np.polyval(p_poly, path_x) # lgtm[py/multiple-definition]
|
||||
l_path_y = np.polyval(r_poly, path_x)
|
||||
r_path_y = np.polyval(l_poly, path_x)
|
||||
|
||||
if pp is not None:
|
||||
p_path_y = np.polyval(pp.lateralPlan.dPolyDEPRECATED, path_x)
|
||||
lineP.set_xdata(p_path_y)
|
||||
lineP.set_ydata(path_x)
|
||||
|
||||
if lMpc is not None:
|
||||
mpc_path_x = list(lMpc.liveMpc.x)[1:]
|
||||
mpc_path_y = list(lMpc.liveMpc.y)[1:]
|
||||
mpc_steer_angle = list(lMpc.liveMpc.delta)[1:]
|
||||
mpc_psi = list(lMpc.liveMpc.psi)[1:]
|
||||
|
||||
line1.set_xdata(mpc_path_y)
|
||||
line1.set_ydata(mpc_path_x)
|
||||
lineL.set_xdata(l_path_y)
|
||||
lineL.set_ydata(path_x)
|
||||
lineR.set_xdata(r_path_y)
|
||||
lineR.set_ydata(path_x)
|
||||
line3.set_xdata(np.asarray(mpc_steer_angle)*180./np.pi * 14)
|
||||
line3.set_ydata(mpc_path_x)
|
||||
line4.set_xdata(np.asarray(mpc_psi)*180./np.pi)
|
||||
line4.set_ydata(mpc_path_x)
|
||||
|
||||
aa.relim()
|
||||
aa.autoscale_view(True, scaley=True, scalex=True)
|
||||
|
||||
fig.canvas.draw()
|
||||
fig.canvas.flush_events()
|
||||
|
||||
if __name__ == "__main__":
|
||||
if len(sys.argv) > 1:
|
||||
mpc_vwr_thread(sys.argv[1])
|
||||
else:
|
||||
mpc_vwr_thread()
|
||||
@@ -1,104 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
import sys
|
||||
import cereal.messaging as messaging
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
N = 21
|
||||
|
||||
# debug longitudinal MPC by plotting its trajectory. To receive liveLongitudinalMpc packets,
|
||||
# set on LOG_MPC env variable and run plannerd on a replay
|
||||
|
||||
def plot_longitudinal_mpc(addr="127.0.0.1"):
|
||||
# *** log ***
|
||||
livempc = messaging.sub_sock('liveLongitudinalMpc', addr=addr, conflate=True)
|
||||
radarstate = messaging.sub_sock('radarState', addr=addr, conflate=True)
|
||||
|
||||
plt.ion()
|
||||
fig = plt.figure()
|
||||
|
||||
t = np.hstack([np.arange(0.0, 0.8, 0.2), np.arange(0.8, 10.6, 0.6)])
|
||||
|
||||
p_x_ego = fig.add_subplot(3, 2, 1)
|
||||
p_v_ego = fig.add_subplot(3, 2, 3)
|
||||
p_a_ego = fig.add_subplot(3, 2, 5)
|
||||
# p_x_l = fig.add_subplot(3, 2, 2)
|
||||
# p_a_l = fig.add_subplot(3, 2, 6)
|
||||
p_d_l = fig.add_subplot(3, 2, 2)
|
||||
p_d_l_v = fig.add_subplot(3, 2, 4)
|
||||
p_d_l_vv = fig.add_subplot(3, 2, 6)
|
||||
|
||||
p_v_ego.set_ylim([0, 30])
|
||||
p_a_ego.set_ylim([-4, 4])
|
||||
p_d_l.set_ylim([-1, 10])
|
||||
|
||||
p_x_ego.set_title('x')
|
||||
p_v_ego.set_title('v')
|
||||
p_a_ego.set_title('a')
|
||||
p_d_l.set_title('rel dist')
|
||||
|
||||
l_x_ego, = p_x_ego.plot(t, np.zeros(N))
|
||||
l_v_ego, = p_v_ego.plot(t, np.zeros(N))
|
||||
l_a_ego, = p_a_ego.plot(t, np.zeros(N))
|
||||
l_x_l, = p_x_ego.plot(t, np.zeros(N))
|
||||
l_v_l, = p_v_ego.plot(t, np.zeros(N))
|
||||
l_a_l, = p_a_ego.plot(t, np.zeros(N))
|
||||
l_d_l, = p_d_l.plot(t, np.zeros(N))
|
||||
l_d_l_v, = p_d_l_v.plot(np.zeros(N))
|
||||
l_d_l_vv, = p_d_l_vv.plot(np.zeros(N))
|
||||
p_x_ego.legend(['ego', 'l'])
|
||||
p_v_ego.legend(['ego', 'l'])
|
||||
p_a_ego.legend(['ego', 'l'])
|
||||
p_d_l_v.set_xlabel('d_rel')
|
||||
p_d_l_v.set_ylabel('v_rel')
|
||||
p_d_l_v.set_ylim([-20, 20])
|
||||
p_d_l_v.set_xlim([0, 100])
|
||||
p_d_l_vv.set_xlabel('d_rel')
|
||||
p_d_l_vv.set_ylabel('v_rel')
|
||||
p_d_l_vv.set_ylim([-5, 5])
|
||||
p_d_l_vv.set_xlim([10, 40])
|
||||
|
||||
while True:
|
||||
lMpc = messaging.recv_sock(livempc, wait=True)
|
||||
rs = messaging.recv_sock(radarstate, wait=True)
|
||||
|
||||
if lMpc is not None:
|
||||
|
||||
if lMpc.liveLongitudinalMpc.mpcId != 1:
|
||||
continue
|
||||
|
||||
x_ego = list(lMpc.liveLongitudinalMpc.xEgo)
|
||||
v_ego = list(lMpc.liveLongitudinalMpc.vEgo)
|
||||
a_ego = list(lMpc.liveLongitudinalMpc.aEgo)
|
||||
x_l = list(lMpc.liveLongitudinalMpc.xLead)
|
||||
v_l = list(lMpc.liveLongitudinalMpc.vLead)
|
||||
# a_l = list(lMpc.liveLongitudinalMpc.aLead)
|
||||
a_l = rs.radarState.leadOne.aLeadK * np.exp(-lMpc.liveLongitudinalMpc.aLeadTau * t**2 / 2)
|
||||
#print(min(a_ego), lMpc.liveLongitudinalMpc.qpIterations)
|
||||
|
||||
l_x_ego.set_ydata(x_ego)
|
||||
l_v_ego.set_ydata(v_ego)
|
||||
l_a_ego.set_ydata(a_ego)
|
||||
|
||||
l_x_l.set_ydata(x_l)
|
||||
l_v_l.set_ydata(v_l)
|
||||
l_a_l.set_ydata(a_l)
|
||||
|
||||
l_d_l.set_ydata(np.array(x_l) - np.array(x_ego))
|
||||
l_d_l_v.set_ydata(np.array(v_l) - np.array(v_ego))
|
||||
l_d_l_v.set_xdata(np.array(x_l) - np.array(x_ego))
|
||||
l_d_l_vv.set_ydata(np.array(v_l) - np.array(v_ego))
|
||||
l_d_l_vv.set_xdata(np.array(x_l) - np.array(x_ego))
|
||||
|
||||
p_x_ego.relim()
|
||||
p_x_ego.autoscale_view(True, scaley=True, scalex=True)
|
||||
fig.canvas.draw()
|
||||
fig.canvas.flush_events()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
if len(sys.argv) > 1:
|
||||
plot_longitudinal_mpc(sys.argv[1])
|
||||
else:
|
||||
plot_longitudinal_mpc()
|
||||
@@ -1,75 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
import numpy as np
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
from selfdrive.controls.lib.longitudinal_mpc_model import libmpc_py
|
||||
|
||||
libmpc = libmpc_py.libmpc
|
||||
|
||||
dt = 1
|
||||
speeds = [6.109375, 5.9765625, 6.6367188, 7.6875, 8.7578125, 9.4375, 10.21875, 11.070312, 11.679688, 12.21875]
|
||||
accelerations = [0.15405273, 0.39575195, 0.36669922, 0.29248047, 0.27856445, 0.27832031, 0.29736328, 0.22705078, 0.16003418, 0.15185547]
|
||||
ts = [t * dt for t in range(len(speeds))]
|
||||
|
||||
# TODO: Get from actual model packet
|
||||
x = 0.0
|
||||
positions = []
|
||||
for v in speeds:
|
||||
positions.append(x)
|
||||
x += v * dt
|
||||
|
||||
|
||||
# Polyfit trajectories
|
||||
x_poly = list(map(float, np.polyfit(ts, positions, 3)))
|
||||
v_poly = list(map(float, np.polyfit(ts, speeds, 3)))
|
||||
a_poly = list(map(float, np.polyfit(ts, accelerations, 3)))
|
||||
|
||||
x_poly = libmpc_py.ffi.new("double[4]", x_poly)
|
||||
v_poly = libmpc_py.ffi.new("double[4]", v_poly)
|
||||
a_poly = libmpc_py.ffi.new("double[4]", a_poly)
|
||||
|
||||
cur_state = libmpc_py.ffi.new("state_t *")
|
||||
cur_state[0].x_ego = 0
|
||||
cur_state[0].v_ego = 10
|
||||
cur_state[0].a_ego = 0
|
||||
|
||||
libmpc.init(1.0, 1.0, 1.0, 1.0, 1.0)
|
||||
|
||||
mpc_solution = libmpc_py.ffi.new("log_t *")
|
||||
libmpc.init_with_simulation(cur_state[0].v_ego)
|
||||
|
||||
libmpc.run_mpc(cur_state, mpc_solution, x_poly, v_poly, a_poly)
|
||||
|
||||
# Converge to solution
|
||||
for _ in range(10):
|
||||
libmpc.run_mpc(cur_state, mpc_solution, x_poly, v_poly, a_poly)
|
||||
|
||||
|
||||
ts_sol = list(mpc_solution[0].t)
|
||||
x_sol = list(mpc_solution[0].x_ego)
|
||||
v_sol = list(mpc_solution[0].v_ego)
|
||||
a_sol = list(mpc_solution[0].a_ego)
|
||||
|
||||
|
||||
plt.figure()
|
||||
plt.subplot(3, 1, 1)
|
||||
plt.plot(ts, positions, 'k--')
|
||||
plt.plot(ts_sol, x_sol)
|
||||
plt.ylabel('Position [m]')
|
||||
plt.xlabel('Time [s]')
|
||||
|
||||
plt.subplot(3, 1, 2)
|
||||
plt.plot(ts, speeds, 'k--')
|
||||
plt.plot(ts_sol, v_sol)
|
||||
plt.xlabel('Time [s]')
|
||||
plt.ylabel('Speed [m/s]')
|
||||
|
||||
plt.subplot(3, 1, 3)
|
||||
plt.plot(ts, accelerations, 'k--')
|
||||
plt.plot(ts_sol, a_sol)
|
||||
|
||||
plt.xlabel('Time [s]')
|
||||
plt.ylabel('Acceleration [m/s^2]')
|
||||
|
||||
plt.show()
|
||||
@@ -1,115 +0,0 @@
|
||||
#! /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, MPC_N, CAR_ROTATION_RADIUS
|
||||
import math
|
||||
|
||||
libmpc = libmpc_py.libmpc
|
||||
libmpc.init()
|
||||
libmpc.set_weights(MPC_COST_LAT.PATH, 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
|
||||
|
||||
for i in range(1):
|
||||
cur_state[0].delta = math.radians(510. / 13.)
|
||||
libmpc.run_mpc(cur_state, mpc_solution, [0,0,0,v_ref],
|
||||
curvature_factor, CAR_ROTATION_RADIUS,
|
||||
[0.0]*MPC_N, [0.0]*MPC_N)
|
||||
|
||||
timesi = []
|
||||
ct = 0
|
||||
for i in range(MPC_N + 1):
|
||||
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()
|
||||
@@ -1,168 +0,0 @@
|
||||
#! /usr/bin/env python
|
||||
# type: ignore
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
from selfdrive.controls.lib.longitudinal_mpc import libmpc_py
|
||||
from selfdrive.controls.lib.drive_helpers import MPC_COST_LONG
|
||||
|
||||
# plot liongitudinal MPC trajectory by defining boundary conditions:
|
||||
# ego and lead vehicles state. Use this script to tune MPC costs
|
||||
|
||||
def RW(v_ego, v_l):
|
||||
TR = 1.8
|
||||
G = 9.81
|
||||
return (v_ego * TR - (v_l - v_ego) * TR + v_ego*v_ego/(2*G) - v_l*v_l / (2*G))
|
||||
|
||||
|
||||
def NORM_RW_ERROR(v_ego, v_l, p):
|
||||
return (RW(v_ego, v_l) + 4.0 - p)
|
||||
#return (RW(v_ego, v_l) + 4.0 - p) / (np.sqrt(v_ego + 0.5) + 0.1)
|
||||
|
||||
|
||||
v_ego = 20.0
|
||||
a_ego = 0
|
||||
|
||||
x_lead = 10.0
|
||||
v_lead = 20.0
|
||||
a_lead = -3.0
|
||||
a_lead_tau = 0.
|
||||
|
||||
# v_ego = 7.02661012716
|
||||
# a_ego = -1.26143024772
|
||||
|
||||
# x_lead = 29.625 + 20
|
||||
# v_lead = 0.725235462189 + 1
|
||||
# a_lead = -1.00025629997
|
||||
|
||||
# a_lead_tau = 2.90729817665
|
||||
|
||||
#min_a_lead_tau = (a_lead**2 * math.pi) / (2 * (v_lead + 0.01)**2)
|
||||
min_a_lead_tau = 0.0
|
||||
|
||||
print(a_lead_tau, min_a_lead_tau)
|
||||
a_lead_tau = max(a_lead_tau, min_a_lead_tau)
|
||||
|
||||
ffi, libmpc = libmpc_py.get_libmpc(1)
|
||||
libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
|
||||
libmpc.init_with_simulation(v_ego, x_lead, v_lead, a_lead, a_lead_tau)
|
||||
|
||||
cur_state = ffi.new("state_t *")
|
||||
cur_state[0].x_ego = 0.0
|
||||
cur_state[0].v_ego = v_ego
|
||||
cur_state[0].a_ego = a_ego
|
||||
cur_state[0].x_l = x_lead
|
||||
cur_state[0].v_l = v_lead
|
||||
|
||||
mpc_solution = ffi.new("log_t *")
|
||||
|
||||
for _ in range(10):
|
||||
print(libmpc.run_mpc(cur_state, mpc_solution, a_lead_tau, a_lead))
|
||||
|
||||
|
||||
for i in range(21):
|
||||
print("t: %.2f\t x_e: %.2f\t v_e: %.2f\t a_e: %.2f\t" % (mpc_solution[0].t[i], mpc_solution[0].x_ego[i], mpc_solution[0].v_ego[i], mpc_solution[0].a_ego[i]))
|
||||
print("x_l: %.2f\t v_l: %.2f\t \t" % (mpc_solution[0].x_l[i], mpc_solution[0].v_l[i]))
|
||||
|
||||
t = np.hstack([np.arange(0., 1.0, 0.2), np.arange(1.0, 10.1, 0.6)])
|
||||
|
||||
print(map(float, mpc_solution[0].x_ego)[-1])
|
||||
print(map(float, mpc_solution[0].x_l)[-1] - map(float, mpc_solution[0].x_ego)[-1])
|
||||
|
||||
plt.figure(figsize=(8, 8))
|
||||
|
||||
plt.subplot(4, 1, 1)
|
||||
x_l = np.array(map(float, mpc_solution[0].x_l))
|
||||
plt.plot(t, map(float, mpc_solution[0].x_ego))
|
||||
plt.plot(t, x_l)
|
||||
plt.legend(['ego', 'lead'])
|
||||
plt.title('x')
|
||||
plt.grid()
|
||||
|
||||
plt.subplot(4, 1, 2)
|
||||
v_ego = np.array(map(float, mpc_solution[0].v_ego))
|
||||
v_l = np.array(map(float, mpc_solution[0].v_l))
|
||||
plt.plot(t, v_ego)
|
||||
plt.plot(t, v_l)
|
||||
plt.legend(['ego', 'lead'])
|
||||
plt.ylim([-1, max(max(v_ego), max(v_l))])
|
||||
plt.title('v')
|
||||
plt.grid()
|
||||
|
||||
plt.subplot(4, 1, 3)
|
||||
plt.plot(t, map(float, mpc_solution[0].a_ego))
|
||||
plt.plot(t, map(float, mpc_solution[0].a_l))
|
||||
plt.legend(['ego', 'lead'])
|
||||
plt.title('a')
|
||||
plt.grid()
|
||||
|
||||
|
||||
plt.subplot(4, 1, 4)
|
||||
d_l = np.array(map(float, mpc_solution[0].x_l)) - np.array(map(float, mpc_solution[0].x_ego))
|
||||
desired = 4.0 + RW(v_ego, v_l)
|
||||
|
||||
plt.plot(t, d_l)
|
||||
plt.plot(t, desired, '--')
|
||||
plt.ylim(-1, max(max(desired), max(d_l)))
|
||||
plt.legend(['relative distance', 'desired distance'])
|
||||
plt.grid()
|
||||
|
||||
plt.show()
|
||||
|
||||
# c1 = np.exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l))
|
||||
# c2 = np.exp(4.5 - d_l)
|
||||
# print(c1)
|
||||
# print(c2)
|
||||
|
||||
# plt.figure()
|
||||
# plt.plot(t, c1, label="NORM_RW_ERROR")
|
||||
# plt.plot(t, c2, label="penalty function")
|
||||
# plt.legend()
|
||||
|
||||
# ## OLD MPC
|
||||
# a_lead_tau = 1.5
|
||||
# a_lead_tau = max(a_lead_tau, -a_lead / (v_lead + 0.01))
|
||||
|
||||
# ffi, libmpc = libmpc_py.get_libmpc(1)
|
||||
# libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
|
||||
# libmpc.init_with_simulation(v_ego, x_lead, v_lead, a_lead, a_lead_tau)
|
||||
|
||||
# cur_state = ffi.new("state_t *")
|
||||
# cur_state[0].x_ego = 0.0
|
||||
# cur_state[0].v_ego = v_ego
|
||||
# cur_state[0].a_ego = a_ego
|
||||
# cur_state[0].x_lead = x_lead
|
||||
# cur_state[0].v_lead = v_lead
|
||||
# cur_state[0].a_lead = a_lead
|
||||
|
||||
# mpc_solution = ffi.new("log_t *")
|
||||
|
||||
# for _ in range(10):
|
||||
# print libmpc.run_mpc(cur_state, mpc_solution, a_lead_tau)
|
||||
|
||||
# t = np.hstack([np.arange(0., 1.0, 0.2), np.arange(1.0, 10.1, 0.6)])
|
||||
|
||||
# print(map(float, mpc_solution[0].x_ego)[-1])
|
||||
# print(map(float, mpc_solution[0].x_lead)[-1] - map(float, mpc_solution[0].x_ego)[-1])
|
||||
# plt.subplot(4, 2, 2)
|
||||
# plt.plot(t, map(float, mpc_solution[0].x_ego))
|
||||
# plt.plot(t, map(float, mpc_solution[0].x_lead))
|
||||
# plt.legend(['ego', 'lead'])
|
||||
# plt.title('x')
|
||||
|
||||
# plt.subplot(4, 2, 4)
|
||||
# plt.plot(t, map(float, mpc_solution[0].v_ego))
|
||||
# plt.plot(t, map(float, mpc_solution[0].v_lead))
|
||||
# plt.legend(['ego', 'lead'])
|
||||
# plt.title('v')
|
||||
|
||||
# plt.subplot(4, 2, 6)
|
||||
# plt.plot(t, map(float, mpc_solution[0].a_ego))
|
||||
# plt.plot(t, map(float, mpc_solution[0].a_lead))
|
||||
# plt.legend(['ego', 'lead'])
|
||||
# plt.title('a')
|
||||
|
||||
|
||||
# plt.subplot(4, 2, 8)
|
||||
# plt.plot(t, np.array(map(float, mpc_solution[0].x_lead)) - np.array(map(float, mpc_solution[0].x_ego)))
|
||||
|
||||
# plt.show()
|
||||
Reference in New Issue
Block a user