From 7099bd18e3ca5936cdd3c8de8a8c679cc7edb053 Mon Sep 17 00:00:00 2001 From: felsager <76905857+felsager@users.noreply.github.com> Date: Fri, 6 Feb 2026 10:35:54 -0800 Subject: [PATCH] longitudinal mpc tuning: behind if main (#37099) --- .../mpc_longitudinal_tuning_report.py | 419 +++++++++--------- 1 file changed, 210 insertions(+), 209 deletions(-) diff --git a/tools/longitudinal_maneuvers/mpc_longitudinal_tuning_report.py b/tools/longitudinal_maneuvers/mpc_longitudinal_tuning_report.py index 8c1a60f5b..51e38112f 100644 --- a/tools/longitudinal_maneuvers/mpc_longitudinal_tuning_report.py +++ b/tools/longitudinal_maneuvers/mpc_longitudinal_tuning_report.py @@ -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() + '
' +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)