Fix cruise MPC convergence bug and add test (#22295)
* fix tests * fix cruise convergence bug * update refs * Update selfdrive/controls/tests/test_cruise_speed.py Co-authored-by: Willem Melching <willem.melching@gmail.com> * Update selfdrive/controls/tests/test_cruise_speed.py * add msgh Co-authored-by: Willem Melching <willem.melching@gmail.com>pull/22299/head
parent
8a836b7e1e
commit
fcda55fbf1
|
@ -115,7 +115,7 @@ class LongitudinalMpc():
|
|||
self.v_solution = [0.0 for i in range(len(T_IDXS))]
|
||||
self.a_solution = [0.0 for i in range(len(T_IDXS))]
|
||||
self.j_solution = [0.0 for i in range(len(T_IDXS)-1)]
|
||||
self.yref = np.ones((N+1, 4))
|
||||
self.yref = np.zeros((N+1, 4))
|
||||
self.solver.cost_set_slice(0, N, "yref", self.yref[:N])
|
||||
self.solver.cost_set(N, "yref", self.yref[N][:3])
|
||||
self.T_IDXS = np.array(T_IDXS[:N+1])
|
||||
|
|
|
@ -0,0 +1,33 @@
|
|||
#!/usr/bin/env python3
|
||||
import unittest
|
||||
import numpy as np
|
||||
from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
|
||||
|
||||
def run_cruise_simulation(cruise, t_end=100.):
|
||||
man = Maneuver(
|
||||
'',
|
||||
duration=t_end,
|
||||
initial_speed=float(0.),
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=100,
|
||||
cruise_values=[cruise],
|
||||
prob_lead_values=[0.0],
|
||||
breakpoints=[0.],
|
||||
)
|
||||
valid, output = man.evaluate()
|
||||
assert valid
|
||||
return output[-1,3]
|
||||
|
||||
|
||||
class TestCruiseSpeed(unittest.TestCase):
|
||||
def test_cruise_speed(self):
|
||||
for speed in np.arange(5, 40, 5):
|
||||
print(f'Testing {speed} m/s')
|
||||
cruise_speed = float(speed)
|
||||
|
||||
simulation_steady_state = run_cruise_simulation(cruise_speed)
|
||||
self.assertAlmostEqual(simulation_steady_state, cruise_speed, delta=.01, msg=f'Did not reach {speed} m/s')
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
|
@ -2,70 +2,29 @@
|
|||
import unittest
|
||||
import numpy as np
|
||||
|
||||
from cereal import log
|
||||
import cereal.messaging as messaging
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.controls.lib.lead_mpc_lib.lead_mpc import desired_follow_distance, LeadMpc
|
||||
from selfdrive.controls.lib.lead_mpc_lib.lead_mpc import desired_follow_distance
|
||||
from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
|
||||
|
||||
|
||||
class FakePubMaster():
|
||||
def send(self, s, data):
|
||||
assert data
|
||||
|
||||
|
||||
def run_following_distance_simulation(v_lead, t_end=200.0):
|
||||
dt = 0.2
|
||||
t = 0.
|
||||
|
||||
x_lead = 200.0
|
||||
|
||||
x_ego = 0.0
|
||||
v_ego = v_lead
|
||||
a_ego = 0.0
|
||||
|
||||
mpc = LeadMpc(0)
|
||||
|
||||
first = True
|
||||
while t < t_end:
|
||||
|
||||
# Setup CarState
|
||||
CS = messaging.new_message('carState')
|
||||
CS.carState.vEgo = float(v_ego)
|
||||
CS.carState.aEgo = float(a_ego)
|
||||
|
||||
# Setup model packet
|
||||
radarstate = messaging.new_message('radarState')
|
||||
lead = log.RadarState.LeadData.new_message()
|
||||
lead.modelProb = .75
|
||||
lead.dRel = x_lead - x_ego
|
||||
lead.vLead = v_lead
|
||||
lead.aLeadK = 0.0
|
||||
lead.status = True
|
||||
radarstate.radarState.leadOne = lead
|
||||
|
||||
# Run MPC
|
||||
mpc.set_cur_state(v_ego, a_ego)
|
||||
if first: # Make sure MPC is converged on first timestep
|
||||
for _ in range(20):
|
||||
mpc.update(CS.carState, radarstate.radarState, 0)
|
||||
mpc.update(CS.carState, radarstate.radarState, 0)
|
||||
|
||||
# Choose slowest of two solutions
|
||||
v_ego, a_ego = float(mpc.v_solution[5]), float(mpc.a_solution[5])
|
||||
|
||||
# Update state
|
||||
x_lead += v_lead * dt
|
||||
x_ego += v_ego * dt
|
||||
t += dt
|
||||
first = False
|
||||
|
||||
return x_lead - x_ego
|
||||
def run_following_distance_simulation(v_lead, t_end=150.0):
|
||||
man = Maneuver(
|
||||
'',
|
||||
duration=t_end,
|
||||
initial_speed=float(v_lead),
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=100,
|
||||
speed_lead_values=[v_lead],
|
||||
breakpoints=[0.],
|
||||
)
|
||||
valid, output = man.evaluate()
|
||||
assert valid
|
||||
return output[-1,2] - output[-1,1]
|
||||
|
||||
|
||||
class TestFollowingDistance(unittest.TestCase):
|
||||
def test_following_distanc(self):
|
||||
for speed_mph in np.linspace(10, 100, num=10):
|
||||
v_lead = float(speed_mph * CV.MPH_TO_MS)
|
||||
for speed in np.arange(0, 40, 5):
|
||||
print(f'Testing {speed} m/s')
|
||||
v_lead = float(speed)
|
||||
|
||||
simulation_steady_state = run_following_distance_simulation(v_lead)
|
||||
correct_steady_state = desired_follow_distance(v_lead, v_lead)
|
||||
|
|
|
@ -9,9 +9,10 @@ class Maneuver():
|
|||
self.speed = kwargs.get("initial_speed", 0.0)
|
||||
self.lead_relevancy = kwargs.get("lead_relevancy", 0)
|
||||
|
||||
self.speed_lead_breakpoints = kwargs.get("speed_lead_breakpoints", [0.0, duration])
|
||||
self.speed_lead_values = kwargs.get("speed_lead_values", [0.0, 0.0])
|
||||
self.prob_lead_values = kwargs.get("prob_lead_values", [1.0 for i in range(len(self.speed_lead_breakpoints))])
|
||||
self.breakpoints = kwargs.get("breakpoints", [0.0, duration])
|
||||
self.speed_lead_values = kwargs.get("speed_lead_values", [0.0 for i in range(len(self.breakpoints))])
|
||||
self.prob_lead_values = kwargs.get("prob_lead_values", [1.0 for i in range(len(self.breakpoints))])
|
||||
self.cruise_values = kwargs.get("cruise_values", [50.0 for i in range(len(self.breakpoints))])
|
||||
|
||||
self.only_lead2 = kwargs.get("only_lead2", False)
|
||||
self.only_radar = kwargs.get("only_radar", False)
|
||||
|
@ -31,9 +32,10 @@ class Maneuver():
|
|||
valid = True
|
||||
logs = []
|
||||
while plant.current_time() < self.duration:
|
||||
speed_lead = np.interp(plant.current_time(), self.speed_lead_breakpoints, self.speed_lead_values)
|
||||
prob = np.interp(plant.current_time(), self.speed_lead_breakpoints, self.prob_lead_values)
|
||||
log = plant.step(speed_lead, prob)
|
||||
speed_lead = np.interp(plant.current_time(), self.breakpoints, self.speed_lead_values)
|
||||
prob = np.interp(plant.current_time(), self.breakpoints, self.prob_lead_values)
|
||||
cruise = np.interp(plant.current_time(), self.breakpoints, self.cruise_values)
|
||||
log = plant.step(speed_lead, prob, cruise)
|
||||
|
||||
d_rel = log['distance_lead'] - log['distance'] if self.lead_relevancy else 200.
|
||||
v_rel = speed_lead - log['speed'] if self.lead_relevancy else 0.
|
||||
|
@ -46,7 +48,7 @@ class Maneuver():
|
|||
speed_lead,
|
||||
log['acceleration']]))
|
||||
|
||||
if d_rel < 1.0:
|
||||
if d_rel < 1.0 and (self.only_radar or prob > 0.5):
|
||||
print("Crashed!!!!")
|
||||
valid = False
|
||||
|
||||
|
|
|
@ -48,7 +48,7 @@ class Plant():
|
|||
def current_time(self):
|
||||
return float(self.rk.frame) / self.rate
|
||||
|
||||
def step(self, v_lead=0.0, prob=1.0):
|
||||
def step(self, v_lead=0.0, prob=1.0, v_cruise=50.):
|
||||
# ******** publish a fake model going straight and fake calibration ********
|
||||
# note that this is worst case for MPC, since model will delay long mpc by one time step
|
||||
radar = messaging.new_message('radarState')
|
||||
|
@ -89,7 +89,7 @@ class Plant():
|
|||
|
||||
|
||||
control.controlsState.longControlState = LongCtrlState.pid
|
||||
control.controlsState.vCruise = 130
|
||||
control.controlsState.vCruise = float(v_cruise * 3.6)
|
||||
car_state.carState.vEgo = float(self.speed)
|
||||
|
||||
|
||||
|
|
|
@ -15,7 +15,7 @@ maneuvers = [
|
|||
lead_relevancy=True,
|
||||
initial_distance_lead=120.,
|
||||
speed_lead_values=[30., 0.],
|
||||
speed_lead_breakpoints=[0., 1.],
|
||||
breakpoints=[0., 1.],
|
||||
),
|
||||
Maneuver(
|
||||
'approach stopped car at 20m/s',
|
||||
|
@ -24,7 +24,7 @@ maneuvers = [
|
|||
lead_relevancy=True,
|
||||
initial_distance_lead=60.,
|
||||
speed_lead_values=[20., 0.],
|
||||
speed_lead_breakpoints=[0., 1.],
|
||||
breakpoints=[0., 1.],
|
||||
),
|
||||
Maneuver(
|
||||
'steady state following a car at 20m/s, then lead decel to 0mph at 1m/s^2',
|
||||
|
@ -33,7 +33,7 @@ maneuvers = [
|
|||
lead_relevancy=True,
|
||||
initial_distance_lead=35.,
|
||||
speed_lead_values=[20., 20., 0.],
|
||||
speed_lead_breakpoints=[0., 15., 35.0],
|
||||
breakpoints=[0., 15., 35.0],
|
||||
),
|
||||
Maneuver(
|
||||
'steady state following a car at 20m/s, then lead decel to 0mph at 2m/s^2',
|
||||
|
@ -42,7 +42,7 @@ maneuvers = [
|
|||
lead_relevancy=True,
|
||||
initial_distance_lead=35.,
|
||||
speed_lead_values=[20., 20., 0.],
|
||||
speed_lead_breakpoints=[0., 15., 25.0],
|
||||
breakpoints=[0., 15., 25.0],
|
||||
),
|
||||
Maneuver(
|
||||
'steady state following a car at 20m/s, then lead decel to 0mph at 3m/s^2',
|
||||
|
@ -51,7 +51,7 @@ maneuvers = [
|
|||
lead_relevancy=True,
|
||||
initial_distance_lead=35.,
|
||||
speed_lead_values=[20., 20., 0.],
|
||||
speed_lead_breakpoints=[0., 15., 21.66],
|
||||
breakpoints=[0., 15., 21.66],
|
||||
),
|
||||
Maneuver(
|
||||
'steady state following a car at 20m/s, then lead decel to 0mph at 5m/s^2',
|
||||
|
@ -60,7 +60,7 @@ maneuvers = [
|
|||
lead_relevancy=True,
|
||||
initial_distance_lead=35.,
|
||||
speed_lead_values=[20., 20., 0.],
|
||||
speed_lead_breakpoints=[0., 15., 19.],
|
||||
breakpoints=[0., 15., 19.],
|
||||
),
|
||||
Maneuver(
|
||||
"approach stopped car at 20m/s",
|
||||
|
@ -69,7 +69,7 @@ maneuvers = [
|
|||
lead_relevancy=True,
|
||||
initial_distance_lead=50.,
|
||||
speed_lead_values=[0., 0.],
|
||||
speed_lead_breakpoints=[1., 11.],
|
||||
breakpoints=[1., 11.],
|
||||
),
|
||||
Maneuver(
|
||||
"approach slower cut-in car at 20m/s",
|
||||
|
@ -78,7 +78,7 @@ maneuvers = [
|
|||
lead_relevancy=True,
|
||||
initial_distance_lead=50.,
|
||||
speed_lead_values=[15., 15.],
|
||||
speed_lead_breakpoints=[1., 11.],
|
||||
breakpoints=[1., 11.],
|
||||
only_lead2=True,
|
||||
),
|
||||
Maneuver(
|
||||
|
@ -89,7 +89,7 @@ maneuvers = [
|
|||
initial_distance_lead=10.,
|
||||
speed_lead_values=[0., 0.],
|
||||
prob_lead_values=[0., 0.],
|
||||
speed_lead_breakpoints=[1., 11.],
|
||||
breakpoints=[1., 11.],
|
||||
only_radar=True,
|
||||
),
|
||||
]
|
||||
|
|
|
@ -1 +1 @@
|
|||
03585a6e067ff58f1030450efc55cd225dc2ffc3
|
||||
1e029954854211b6277f98e6b0c3438fd11e6667
|
Loading…
Reference in New Issue