Gate acceleration on model gas press predictions (#33643)

* no ui squash

* already calibrated

* Already calibrated

* Add longitudinal maneuver tests

* Fix linter errors

* Added get_coast_accel function

---------

Co-authored-by: Bruce Wayne <harald.the.engineer@gmail.com>
pull/33685/head
Mitchell Goff 11 months ago committed by GitHub
parent 7ebd7b8b4d
commit 47cc314b14
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
  1. 27
      selfdrive/controls/lib/longitudinal_planner.py
  2. 16
      selfdrive/test/longitudinal_maneuvers/maneuver.py
  3. 12
      selfdrive/test/longitudinal_maneuvers/plant.py
  4. 38
      selfdrive/test/longitudinal_maneuvers/test_longitudinal.py

@ -21,6 +21,8 @@ A_CRUISE_MIN = -1.2
A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6]
A_CRUISE_MAX_BP = [0., 10.0, 25., 40.]
CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
ALLOW_THROTTLE_THRESHOLD = 0.5
ACCEL_LIMIT_MARGIN = 0.05
# Lookup table for turns
_A_TOTAL_MAX_V = [1.7, 3.2]
@ -30,6 +32,9 @@ _A_TOTAL_MAX_BP = [20., 40.]
def get_max_accel(v_ego):
return interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS)
def get_coast_accel(pitch):
return np.sin(pitch) * -5.65 - 0.3 # fitted from data using xx/projects/allow_throttle/compute_coast_accel.py
def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
"""
@ -69,6 +74,7 @@ class LongitudinalPlanner:
self.mpc = LongitudinalMpc(dt=dt)
self.fcw = False
self.dt = dt
self.allow_throttle = True
self.a_desired = init_a
self.v_desired_filter = FirstOrderFilter(init_v, 2.0, self.dt)
@ -93,11 +99,20 @@ class LongitudinalPlanner:
v = np.zeros(len(T_IDXS_MPC))
a = np.zeros(len(T_IDXS_MPC))
j = np.zeros(len(T_IDXS_MPC))
return x, v, a, j
if len(model_msg.meta.disengagePredictions.gasPressProbs) > 1:
throttle_prob = model_msg.meta.disengagePredictions.gasPressProbs[1]
else:
throttle_prob = 1.0
return x, v, a, j, throttle_prob
def update(self, sm):
self.mpc.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc'
if len(sm['carControl'].orientationNED) == 3:
accel_coast = get_coast_accel(sm['carControl'].orientationNED[1])
else:
accel_coast = ACCEL_MAX
v_ego = sm['carState'].vEgo
v_cruise_kph = min(sm['carState'].vCruise, V_CRUISE_MAX)
v_cruise = v_cruise_kph * CV.KPH_TO_MS
@ -130,6 +145,13 @@ class LongitudinalPlanner:
self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))
# Compute model v_ego error
self.v_model_error = get_speed_error(sm['modelV2'], v_ego)
x, v, a, j, throttle_prob = self.parse_model(sm['modelV2'], self.v_model_error)
self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD
if not self.allow_throttle and v_ego > 5.0: # Don't clip at low speeds since throttle_prob doesn't account for creep
# MPC breaks when accel limits would cause negative velocity within the MPC horizon, so we clip the max accel limit at vEgo/T_MAX plus a bit of margin
clipped_accel_coast = max(accel_coast, accel_limits_turns[0], -v_ego / T_IDXS_MPC[-1] + ACCEL_LIMIT_MARGIN)
accel_limits_turns[1] = min(accel_limits_turns[1], clipped_accel_coast)
if force_slow_decel:
v_cruise = 0.0
@ -140,7 +162,6 @@ class LongitudinalPlanner:
self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality)
self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error)
self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['selfdriveState'].personality)
self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
@ -179,6 +200,6 @@ class LongitudinalPlanner:
longitudinalPlan.aTarget = a_target
longitudinalPlan.shouldStop = should_stop
longitudinalPlan.allowBrake = True
longitudinalPlan.allowThrottle = True
longitudinalPlan.allowThrottle = self.allow_throttle
pm.send('longitudinalPlan', plan_send)

@ -12,11 +12,14 @@ class Maneuver:
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.prob_throttle_values = kwargs.get("prob_throttle_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.pitch_values = kwargs.get("pitch_values", [0.0 for i in range(len(self.breakpoints))])
self.only_lead2 = kwargs.get("only_lead2", False)
self.only_radar = kwargs.get("only_radar", False)
self.ensure_start = kwargs.get("ensure_start", False)
self.ensure_slowdown = kwargs.get("ensure_slowdown", False)
self.enabled = kwargs.get("enabled", True)
self.e2e = kwargs.get("e2e", False)
self.personality = kwargs.get("personality", 0)
@ -42,9 +45,11 @@ class Maneuver:
logs = []
while plant.current_time < self.duration:
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)
prob_lead = 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)
pitch = np.interp(plant.current_time, self.breakpoints, self.pitch_values)
prob_throttle = np.interp(plant.current_time, self.breakpoints, self.prob_throttle_values)
log = plant.step(speed_lead, prob_lead, cruise, pitch, prob_throttle)
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.
@ -57,13 +62,18 @@ class Maneuver:
speed_lead,
log['acceleration']]))
if d_rel < .4 and (self.only_radar or prob > 0.5):
if d_rel < .4 and (self.only_radar or prob_lead > 0.5):
print("Crashed!!!!")
valid = False
if self.ensure_start and log['v_rel'] > 0 and log['speeds'][-1] <= 0.1:
print('LongitudinalPlanner not starting!')
valid = False
if self.ensure_slowdown and log['speed'] > 5.5:
print('LongitudinalPlanner not slowing down!')
valid = False
if self.force_decel and log['speed'] > 1e-1 and log['acceleration'] > -0.04:
print('Not stopping with force decel')
valid = False

@ -57,13 +57,14 @@ class Plant:
def current_time(self):
return float(self.rk.frame) / self.rate
def step(self, v_lead=0.0, prob=1.0, v_cruise=50.):
def step(self, v_lead=0.0, prob_lead=1.0, v_cruise=50., pitch=0.0, prob_throttle=1.0):
# ******** 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')
control = messaging.new_message('controlsState')
ss = messaging.new_message('selfdriveState')
car_state = messaging.new_message('carState')
car_control = messaging.new_message('carControl')
model = messaging.new_message('modelV2')
a_lead = (v_lead - self.v_lead_prev)/self.ts
self.v_lead_prev = v_lead
@ -73,14 +74,14 @@ class Plant:
v_rel = v_lead - self.speed
if self.only_radar:
status = True
elif prob > .5:
elif prob_lead > .5:
status = True
else:
status = False
else:
d_rel = 200.
v_rel = 0.
prob = 0.0
prob_lead = 0.0
status = False
lead = log.RadarState.LeadData.new_message()
@ -94,7 +95,7 @@ class Plant:
# TODO use real radard logic for this
lead.aLeadTau = float(_LEAD_ACCEL_TAU)
lead.status = status
lead.modelProb = float(prob)
lead.modelProb = float(prob_lead)
if not self.only_lead2:
radar.radarState.leadOne = lead
radar.radarState.leadTwo = lead
@ -112,6 +113,7 @@ class Plant:
acceleration = log.XYZTData.new_message()
acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)]
model.modelV2.acceleration = acceleration
model.modelV2.meta.disengagePredictions.gasPressProbs = [float(prob_throttle) for _ in range(6)]
control.controlsState.longControlState = LongCtrlState.pid if self.enabled else LongCtrlState.off
ss.selfdriveState.experimentalMode = self.e2e
@ -120,10 +122,12 @@ class Plant:
car_state.carState.vEgo = float(self.speed)
car_state.carState.standstill = self.speed < 0.01
car_state.carState.vCruise = float(v_cruise * 3.6)
car_control.carControl.orientationNED = [0., float(pitch), 0.]
# ******** get controlsState messages for plotting ***
sm = {'radarState': radar.radarState,
'carState': car_state.carState,
'carControl': car_control.carControl,
'controlsState': control.controlsState,
'selfdriveState': ss.selfdriveState,
'modelV2': model.modelV2}

@ -82,6 +82,44 @@ def create_maneuvers(kwargs):
breakpoints=[0.0, 2., 2.01],
**kwargs,
),
Maneuver(
"approach stopped car at 20m/s, with prob_throttle_values and pitch = -0.1",
duration=30.,
initial_speed=20.,
lead_relevancy=True,
initial_distance_lead=120.,
speed_lead_values=[0.0, 0., 0.],
prob_throttle_values=[1., 0., 0.],
cruise_values=[20., 20., 20.],
pitch_values=[0., -0.1, -0.1],
breakpoints=[0.0, 2., 2.01],
**kwargs,
),
Maneuver(
"approach stopped car at 20m/s, with prob_throttle_values and pitch = +0.1",
duration=30.,
initial_speed=20.,
lead_relevancy=True,
initial_distance_lead=120.,
speed_lead_values=[0.0, 0., 0.],
prob_throttle_values=[1., 0., 0.],
cruise_values=[20., 20., 20.],
pitch_values=[0., 0.1, 0.1],
breakpoints=[0.0, 2., 2.01],
**kwargs,
),
Maneuver(
"slow to 5m/s with allow_throttle = False and pitch = +0.1",
duration=25.,
initial_speed=20.,
lead_relevancy=False,
prob_throttle_values=[1., 0., 0.],
cruise_values=[20., 20., 20.],
pitch_values=[0., 0.1, 0.1],
breakpoints=[0.0, 2., 2.01],
ensure_slowdown=True,
**kwargs,
),
Maneuver(
"approach slower cut-in car at 20m/s",
duration=20.,

Loading…
Cancel
Save