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>pull/21445/head
HaraldSchafer
4 years ago
committed by
GitHub
parent
f1ee79ef14
commit
6838e1c82c
21 changed files with 144 additions and 661 deletions
@ -1 +1 @@ |
||||
Subproject commit e1793a1854eb5f76a304b30ee96dee5a0f0b2cc4 |
||||
Subproject commit e232a014579ba3a45bdba1968502c11ae2005f1a |
||||
@ -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() |
||||
@ -1,6 +1,8 @@ |
||||
MAX_DISTANCE = 140. |
||||
LANE_OFFSET = 1.8 |
||||
MAX_REL_V = 10. |
||||
import numpy as np |
||||
IDX_N = 33 |
||||
|
||||
LEAD_X_SCALE = 10 |
||||
LEAD_Y_SCALE = 10 |
||||
def index_function(idx, max_val=192): |
||||
return (max_val/1024)*(idx**2) |
||||
|
||||
|
||||
T_IDXS = np.array([index_function(idx, max_val=10.0) for idx in range(IDX_N)], dtype=np.float64) |
||||
|
||||
@ -1 +1 @@ |
||||
1ac9a43631a3d6c7316220897ab17f33a66bb05f |
||||
999749c3955d504712564db3faf541f8c21c069d |
||||
@ -0,0 +1,21 @@ |
||||
#!/usr/bin/env python3 |
||||
from selfdrive.test.process_replay.regen import regen_and_save |
||||
from selfdrive.test.process_replay.test_processes import original_segments as segments |
||||
|
||||
if __name__ == "__main__": |
||||
new_segments = [] |
||||
for segment in segments: |
||||
route = segment[1].rsplit('--', 1)[0] |
||||
sidx = int(segment[1].rsplit('--', 1)[1]) |
||||
relr = regen_and_save(route, sidx, upload=True, use_route_meta=False) |
||||
|
||||
print("\n\n", "*"*30, "\n\n") |
||||
print("New route:", relr, "\n") |
||||
relr = relr.replace('/', '|') |
||||
new_segments.append(f'("{segment[0]}", "{relr}"), ') |
||||
print() |
||||
print() |
||||
print() |
||||
print('COPY THIS INTO test_processes.py') |
||||
for seg in new_segments: |
||||
print(seg) |
||||
Loading…
Reference in new issue