longitudinal mpc tuning: behind if main (#37099)
This commit is contained in:
felsager
@@ -3,8 +3,8 @@ import sys
|
||||
import markdown
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
|
||||
from openpilot.selfdrive.controls.tests.test_following_distance import desired_follow_distance
|
||||
from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
|
||||
|
||||
TIME = 0
|
||||
LEAD_DISTANCE= 2
|
||||
@@ -21,7 +21,6 @@ axis_labels = ['Time (s)',
|
||||
'Lead distance (m)'
|
||||
]
|
||||
|
||||
|
||||
def get_html_from_results(results, labels, AXIS):
|
||||
fig, ax = plt.subplots(figsize=(16, 8))
|
||||
for idx, speed in enumerate(list(results.keys())):
|
||||
@@ -38,242 +37,244 @@ def get_html_from_results(results, labels, AXIS):
|
||||
plt.close(fig)
|
||||
return fig_buffer.getvalue() + '<br/>'
|
||||
|
||||
def generate_mpc_tuning_report():
|
||||
htmls = []
|
||||
|
||||
htmls = []
|
||||
results = {}
|
||||
name = 'Resuming behind lead'
|
||||
labels = []
|
||||
for lead_accel in np.linspace(1.0, 4.0, 4):
|
||||
man = Maneuver(
|
||||
'',
|
||||
duration=11,
|
||||
initial_speed=0.0,
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=desired_follow_distance(0.0, 0.0),
|
||||
speed_lead_values=[0.0, 10 * lead_accel],
|
||||
cruise_values=[100, 100],
|
||||
prob_lead_values=[1.0, 1.0],
|
||||
breakpoints=[1., 11],
|
||||
)
|
||||
valid, results[lead_accel] = man.evaluate()
|
||||
labels.append(f'{lead_accel} m/s^2 lead acceleration')
|
||||
|
||||
results = {}
|
||||
name = 'Resuming behind lead'
|
||||
labels = []
|
||||
for lead_accel in np.linspace(1.0, 4.0, 4):
|
||||
man = Maneuver(
|
||||
'',
|
||||
duration=11,
|
||||
initial_speed=0.0,
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=desired_follow_distance(0.0, 0.0),
|
||||
speed_lead_values=[0.0, 10 * lead_accel],
|
||||
cruise_values=[100, 100],
|
||||
prob_lead_values=[1.0, 1.0],
|
||||
breakpoints=[1., 11],
|
||||
)
|
||||
valid, results[lead_accel] = man.evaluate()
|
||||
labels.append(f'{lead_accel} m/s^2 lead acceleration')
|
||||
|
||||
htmls.append(markdown.markdown('# ' + name))
|
||||
htmls.append(get_html_from_results(results, labels, EGO_V))
|
||||
htmls.append(get_html_from_results(results, labels, EGO_A))
|
||||
htmls.append(markdown.markdown('# ' + name))
|
||||
htmls.append(get_html_from_results(results, labels, EGO_V))
|
||||
htmls.append(get_html_from_results(results, labels, EGO_A))
|
||||
|
||||
|
||||
results = {}
|
||||
name = 'Approaching stopped car from 140m'
|
||||
labels = []
|
||||
for speed in np.arange(0,45,5):
|
||||
man = Maneuver(
|
||||
name,
|
||||
duration=30.,
|
||||
initial_speed=float(speed),
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=140.,
|
||||
speed_lead_values=[0.0, 0.],
|
||||
breakpoints=[0., 30.],
|
||||
)
|
||||
valid, results[speed] = man.evaluate()
|
||||
results[speed][:,2] = results[speed][:,2] - results[speed][:,1]
|
||||
labels.append(f'{speed} m/s approach speed')
|
||||
results = {}
|
||||
name = 'Approaching stopped car from 140m'
|
||||
labels = []
|
||||
for speed in np.arange(0,45,5):
|
||||
man = Maneuver(
|
||||
name,
|
||||
duration=30.,
|
||||
initial_speed=float(speed),
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=140.,
|
||||
speed_lead_values=[0.0, 0.],
|
||||
breakpoints=[0., 30.],
|
||||
)
|
||||
valid, results[speed] = man.evaluate()
|
||||
results[speed][:,2] = results[speed][:,2] - results[speed][:,1]
|
||||
labels.append(f'{speed} m/s approach speed')
|
||||
|
||||
htmls.append(markdown.markdown('# ' + name))
|
||||
htmls.append(get_html_from_results(results, labels, EGO_A))
|
||||
htmls.append(get_html_from_results(results, labels, D_REL))
|
||||
htmls.append(markdown.markdown('# ' + name))
|
||||
htmls.append(get_html_from_results(results, labels, EGO_A))
|
||||
htmls.append(get_html_from_results(results, labels, D_REL))
|
||||
|
||||
|
||||
results = {}
|
||||
name = 'Following 5s oscillating lead'
|
||||
labels = []
|
||||
speed = np.int64(10)
|
||||
for oscil in np.arange(0, 10, 1):
|
||||
man = Maneuver(
|
||||
'',
|
||||
duration=30.,
|
||||
initial_speed=float(speed),
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=desired_follow_distance(speed, speed),
|
||||
speed_lead_values=[speed, speed, speed - oscil, speed + oscil, speed - oscil, speed + oscil, speed - oscil],
|
||||
breakpoints=[0.,2., 5, 8, 15, 18, 25.],
|
||||
)
|
||||
valid, results[oscil] = man.evaluate()
|
||||
labels.append(f'{oscil} m/s oscilliation size')
|
||||
results = {}
|
||||
name = 'Following 5s (triangular) oscillating lead'
|
||||
labels = []
|
||||
speed = np.int64(10)
|
||||
for oscil in np.arange(0, 10, 1):
|
||||
man = Maneuver(
|
||||
'',
|
||||
duration=30.,
|
||||
initial_speed=float(speed),
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=desired_follow_distance(speed, speed),
|
||||
speed_lead_values=[speed, speed, speed - oscil, speed + oscil, speed - oscil, speed + oscil, speed - oscil],
|
||||
breakpoints=[0.,2., 5, 8, 15, 18, 25.],
|
||||
)
|
||||
valid, results[oscil] = man.evaluate()
|
||||
labels.append(f'{oscil} m/s oscillation size')
|
||||
|
||||
htmls.append(markdown.markdown('# ' + name))
|
||||
htmls.append(get_html_from_results(results, labels, D_REL))
|
||||
htmls.append(get_html_from_results(results, labels, EGO_V))
|
||||
htmls.append(get_html_from_results(results, labels, EGO_A))
|
||||
htmls.append(markdown.markdown('# ' + name))
|
||||
htmls.append(get_html_from_results(results, labels, D_REL))
|
||||
htmls.append(get_html_from_results(results, labels, EGO_V))
|
||||
htmls.append(get_html_from_results(results, labels, EGO_A))
|
||||
|
||||
|
||||
results = {}
|
||||
name = 'Speed profile when converging to steady state lead at 30m/s'
|
||||
labels = []
|
||||
for distance in np.arange(20, 140, 10):
|
||||
man = Maneuver(
|
||||
'',
|
||||
duration=50,
|
||||
initial_speed=30.0,
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=distance,
|
||||
speed_lead_values=[30.0],
|
||||
breakpoints=[0.],
|
||||
)
|
||||
valid, results[distance] = man.evaluate()
|
||||
results[distance][:,2] = results[distance][:,2] - results[distance][:,1]
|
||||
labels.append(f'{distance} m initial distance')
|
||||
|
||||
results = {}
|
||||
name = 'Speed profile when converging to steady state lead at 30m/s'
|
||||
labels = []
|
||||
for distance in np.arange(20, 140, 10):
|
||||
man = Maneuver(
|
||||
'',
|
||||
duration=50,
|
||||
initial_speed=30.0,
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=distance,
|
||||
speed_lead_values=[30.0],
|
||||
breakpoints=[0.],
|
||||
)
|
||||
valid, results[distance] = man.evaluate()
|
||||
results[distance][:,2] = results[distance][:,2] - results[distance][:,1]
|
||||
labels.append(f'{distance} m initial distance')
|
||||
|
||||
htmls.append(markdown.markdown('# ' + name))
|
||||
htmls.append(get_html_from_results(results, labels, EGO_V))
|
||||
htmls.append(get_html_from_results(results, labels, D_REL))
|
||||
htmls.append(markdown.markdown('# ' + name))
|
||||
htmls.append(get_html_from_results(results, labels, EGO_V))
|
||||
htmls.append(get_html_from_results(results, labels, D_REL))
|
||||
|
||||
|
||||
results = {}
|
||||
name = 'Speed profile when converging to steady state lead at 20m/s'
|
||||
labels = []
|
||||
for distance in np.arange(20, 140, 10):
|
||||
man = Maneuver(
|
||||
'',
|
||||
duration=50,
|
||||
initial_speed=20.0,
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=distance,
|
||||
speed_lead_values=[20.0],
|
||||
breakpoints=[0.],
|
||||
)
|
||||
valid, results[distance] = man.evaluate()
|
||||
results[distance][:,2] = results[distance][:,2] - results[distance][:,1]
|
||||
labels.append(f'{distance} m initial distance')
|
||||
results = {}
|
||||
name = 'Speed profile when converging to steady state lead at 20m/s'
|
||||
labels = []
|
||||
for distance in np.arange(20, 140, 10):
|
||||
man = Maneuver(
|
||||
'',
|
||||
duration=50,
|
||||
initial_speed=20.0,
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=distance,
|
||||
speed_lead_values=[20.0],
|
||||
breakpoints=[0.],
|
||||
)
|
||||
valid, results[distance] = man.evaluate()
|
||||
results[distance][:,2] = results[distance][:,2] - results[distance][:,1]
|
||||
labels.append(f'{distance} m initial distance')
|
||||
|
||||
htmls.append(markdown.markdown('# ' + name))
|
||||
htmls.append(get_html_from_results(results, labels, EGO_V))
|
||||
htmls.append(get_html_from_results(results, labels, D_REL))
|
||||
htmls.append(markdown.markdown('# ' + name))
|
||||
htmls.append(get_html_from_results(results, labels, EGO_V))
|
||||
htmls.append(get_html_from_results(results, labels, D_REL))
|
||||
|
||||
|
||||
results = {}
|
||||
name = 'Following car at 30m/s that comes to a stop'
|
||||
labels = []
|
||||
for stop_time in np.arange(4, 14, 1):
|
||||
man = Maneuver(
|
||||
'',
|
||||
duration=50,
|
||||
initial_speed=30.0,
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=60.0,
|
||||
speed_lead_values=[30.0, 30.0, 0.0, 0.0],
|
||||
breakpoints=[0., 20., 20 + stop_time, 30 + stop_time],
|
||||
)
|
||||
valid, results[stop_time] = man.evaluate()
|
||||
results[stop_time][:,2] = results[stop_time][:,2] - results[stop_time][:,1]
|
||||
labels.append(f'{stop_time} seconds stop time')
|
||||
results = {}
|
||||
name = 'Following car at 30m/s that comes to a stop'
|
||||
labels = []
|
||||
for stop_time in np.arange(4, 14, 1):
|
||||
man = Maneuver(
|
||||
'',
|
||||
duration=50,
|
||||
initial_speed=30.0,
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=60.0,
|
||||
speed_lead_values=[30.0, 30.0, 0.0, 0.0],
|
||||
breakpoints=[0., 20., 20 + stop_time, 30 + stop_time],
|
||||
)
|
||||
valid, results[stop_time] = man.evaluate()
|
||||
results[stop_time][:,2] = results[stop_time][:,2] - results[stop_time][:,1]
|
||||
labels.append(f'{stop_time} seconds stop time')
|
||||
|
||||
htmls.append(markdown.markdown('# ' + name))
|
||||
htmls.append(get_html_from_results(results, labels, EGO_A))
|
||||
htmls.append(get_html_from_results(results, labels, D_REL))
|
||||
htmls.append(markdown.markdown('# ' + name))
|
||||
htmls.append(get_html_from_results(results, labels, EGO_A))
|
||||
htmls.append(get_html_from_results(results, labels, D_REL))
|
||||
|
||||
|
||||
results = {}
|
||||
name = 'Response to cut-in at half follow distance'
|
||||
labels = []
|
||||
for speed in np.arange(0, 40, 5):
|
||||
man = Maneuver(
|
||||
'',
|
||||
duration=10,
|
||||
initial_speed=float(speed),
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=desired_follow_distance(speed, speed)/2,
|
||||
speed_lead_values=[speed, speed, speed],
|
||||
cruise_values=[speed, speed, speed],
|
||||
prob_lead_values=[0.0, 0.0, 1.0],
|
||||
breakpoints=[0., 5.0, 5.01],
|
||||
)
|
||||
valid, results[speed] = man.evaluate()
|
||||
labels.append(f'{speed} m/s speed')
|
||||
results = {}
|
||||
name = 'Response to cut-in at half follow distance'
|
||||
labels = []
|
||||
for speed in np.arange(0, 40, 5):
|
||||
man = Maneuver(
|
||||
'',
|
||||
duration=10,
|
||||
initial_speed=float(speed),
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=desired_follow_distance(speed, speed)/2,
|
||||
speed_lead_values=[speed, speed, speed],
|
||||
cruise_values=[speed, speed, speed],
|
||||
prob_lead_values=[0.0, 0.0, 1.0],
|
||||
breakpoints=[0., 5.0, 5.01],
|
||||
)
|
||||
valid, results[speed] = man.evaluate()
|
||||
labels.append(f'{speed} m/s speed')
|
||||
|
||||
htmls.append(markdown.markdown('# ' + name))
|
||||
htmls.append(get_html_from_results(results, labels, EGO_A))
|
||||
htmls.append(get_html_from_results(results, labels, D_REL))
|
||||
htmls.append(markdown.markdown('# ' + name))
|
||||
htmls.append(get_html_from_results(results, labels, EGO_A))
|
||||
htmls.append(get_html_from_results(results, labels, D_REL))
|
||||
|
||||
|
||||
results = {}
|
||||
name = 'Follow a lead that accelerates at 2m/s^2 until steady state speed'
|
||||
labels = []
|
||||
for speed in np.arange(0, 40, 5):
|
||||
man = Maneuver(
|
||||
'',
|
||||
duration=50,
|
||||
initial_speed=0.0,
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=desired_follow_distance(0.0, 0.0),
|
||||
speed_lead_values=[0.0, 0.0, speed],
|
||||
prob_lead_values=[1.0, 1.0, 1.0],
|
||||
breakpoints=[0., 1.0, speed/2],
|
||||
)
|
||||
valid, results[speed] = man.evaluate()
|
||||
labels.append(f'{speed} m/s speed')
|
||||
results = {}
|
||||
name = 'Follow a lead that accelerates at 2m/s^2 until steady state speed'
|
||||
labels = []
|
||||
for speed in np.arange(0, 40, 5):
|
||||
man = Maneuver(
|
||||
'',
|
||||
duration=50,
|
||||
initial_speed=0.0,
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=desired_follow_distance(0.0, 0.0),
|
||||
speed_lead_values=[0.0, 0.0, speed],
|
||||
prob_lead_values=[1.0, 1.0, 1.0],
|
||||
breakpoints=[0., 1.0, speed/2],
|
||||
)
|
||||
valid, results[speed] = man.evaluate()
|
||||
labels.append(f'{speed} m/s speed')
|
||||
|
||||
htmls.append(markdown.markdown('# ' + name))
|
||||
htmls.append(get_html_from_results(results, labels, EGO_V))
|
||||
htmls.append(get_html_from_results(results, labels, EGO_A))
|
||||
htmls.append(markdown.markdown('# ' + name))
|
||||
htmls.append(get_html_from_results(results, labels, EGO_V))
|
||||
htmls.append(get_html_from_results(results, labels, EGO_A))
|
||||
|
||||
|
||||
results = {}
|
||||
name = 'From stop to cruise'
|
||||
labels = []
|
||||
for speed in np.arange(0, 40, 5):
|
||||
man = Maneuver(
|
||||
'',
|
||||
duration=50,
|
||||
initial_speed=0.0,
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=desired_follow_distance(0.0, 0.0),
|
||||
speed_lead_values=[0.0, 0.0],
|
||||
cruise_values=[0.0, speed],
|
||||
prob_lead_values=[0.0, 0.0],
|
||||
breakpoints=[1., 1.01],
|
||||
)
|
||||
valid, results[speed] = man.evaluate()
|
||||
labels.append(f'{speed} m/s speed')
|
||||
results = {}
|
||||
name = 'From stop to cruise'
|
||||
labels = []
|
||||
for speed in np.arange(0, 40, 5):
|
||||
man = Maneuver(
|
||||
'',
|
||||
duration=50,
|
||||
initial_speed=0.0,
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=desired_follow_distance(0.0, 0.0),
|
||||
speed_lead_values=[0.0, 0.0],
|
||||
cruise_values=[0.0, speed],
|
||||
prob_lead_values=[0.0, 0.0],
|
||||
breakpoints=[1., 1.01],
|
||||
)
|
||||
valid, results[speed] = man.evaluate()
|
||||
labels.append(f'{speed} m/s speed')
|
||||
|
||||
htmls.append(markdown.markdown('# ' + name))
|
||||
htmls.append(get_html_from_results(results, labels, EGO_V))
|
||||
htmls.append(get_html_from_results(results, labels, EGO_A))
|
||||
htmls.append(markdown.markdown('# ' + name))
|
||||
htmls.append(get_html_from_results(results, labels, EGO_V))
|
||||
htmls.append(get_html_from_results(results, labels, EGO_A))
|
||||
|
||||
|
||||
results = {}
|
||||
name = 'From cruise to min'
|
||||
labels = []
|
||||
for speed in np.arange(10, 40, 5):
|
||||
man = Maneuver(
|
||||
'',
|
||||
duration=50,
|
||||
initial_speed=float(speed),
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=desired_follow_distance(0.0, 0.0),
|
||||
speed_lead_values=[0.0, 0.0],
|
||||
cruise_values=[speed, 10.0],
|
||||
prob_lead_values=[0.0, 0.0],
|
||||
breakpoints=[1., 1.01],
|
||||
)
|
||||
valid, results[speed] = man.evaluate()
|
||||
labels.append(f'{speed} m/s speed')
|
||||
results = {}
|
||||
name = 'From cruise to min'
|
||||
labels = []
|
||||
for speed in np.arange(10, 40, 5):
|
||||
man = Maneuver(
|
||||
'',
|
||||
duration=50,
|
||||
initial_speed=float(speed),
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=desired_follow_distance(0.0, 0.0),
|
||||
speed_lead_values=[0.0, 0.0],
|
||||
cruise_values=[speed, 10.0],
|
||||
prob_lead_values=[0.0, 0.0],
|
||||
breakpoints=[1., 1.01],
|
||||
)
|
||||
valid, results[speed] = man.evaluate()
|
||||
labels.append(f'{speed} m/s speed')
|
||||
|
||||
htmls.append(markdown.markdown('# ' + name))
|
||||
htmls.append(get_html_from_results(results, labels, EGO_V))
|
||||
htmls.append(get_html_from_results(results, labels, EGO_A))
|
||||
htmls.append(markdown.markdown('# ' + name))
|
||||
htmls.append(get_html_from_results(results, labels, EGO_V))
|
||||
htmls.append(get_html_from_results(results, labels, EGO_A))
|
||||
|
||||
if len(sys.argv) < 2:
|
||||
file_name = 'long_mpc_tune_report.html'
|
||||
else:
|
||||
file_name = sys.argv[1]
|
||||
return htmls
|
||||
|
||||
with open(file_name, 'w') as f:
|
||||
f.write(markdown.markdown('# MPC longitudinal tuning report'))
|
||||
if __name__ == '__main__':
|
||||
htmls = generate_mpc_tuning_report()
|
||||
|
||||
with open(file_name, 'a') as f:
|
||||
for html in htmls:
|
||||
f.write(html)
|
||||
if len(sys.argv) < 2:
|
||||
file_name = 'long_mpc_tune_report.html'
|
||||
else:
|
||||
file_name = sys.argv[1]
|
||||
|
||||
with open(file_name, 'w') as f:
|
||||
f.write(markdown.markdown('# MPC longitudinal tuning report'))
|
||||
for html in htmls:
|
||||
f.write(html)
|
||||
|
||||
Reference in New Issue
Block a user