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. |
import numpy as np |
||||||
LANE_OFFSET = 1.8 |
IDX_N = 33 |
||||||
MAX_REL_V = 10. |
|
||||||
|
|
||||||
LEAD_X_SCALE = 10 |
def index_function(idx, max_val=192): |
||||||
LEAD_Y_SCALE = 10 |
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