New Toyota Long Tune; Add Cost to Long Plan Change

This is a combination of 16 commits.

Add cost to long plan change for smoother lag comp (commaai#22923)

* add plan changing cost

* fix compile

* set weights

* try this cost

* horizon problem

* looks pretty good

* update refs

* update refs

* smoother plan changes

try new tune

simple and ok

general cleanup

unused

add lag

small touches

integrators are bad

only rav4 has sensitive gas

gravity compensation

this seems to not work

whitespace

tss2 as well

cant trust toyota api

HIGHLANDER is like RAV4

v_pid being from the future makes little sense

selfdrive/car/toyota/carcontroller.py

@@ -5,63 +5,36 @@
                                            create_accel_command, create_acc_cancel_command, \
                                            create_fcw_command, create_lta_steer_command
 from selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \
-                                        MIN_ACC_SPEED, PEDAL_HYST_GAP, PEDAL_SCALE, CarControllerParams
+                                        MIN_ACC_SPEED, PEDAL_TRANSITION, CarControllerParams
 from opendbc.can.packer import CANPacker
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 
 
-def accel_hysteresis(accel, accel_steady, enabled):
-
-  # for small accel oscillations within ACCEL_HYST_GAP, don't change the accel command
-  if not enabled:
-    # send 0 when disabled, otherwise acc faults
-    accel_steady = 0.
-  elif accel > accel_steady + CarControllerParams.ACCEL_HYST_GAP:
-    accel_steady = accel - CarControllerParams.ACCEL_HYST_GAP
-  elif accel < accel_steady - CarControllerParams.ACCEL_HYST_GAP:
-    accel_steady = accel + CarControllerParams.ACCEL_HYST_GAP
-  accel = accel_steady
-
-  return accel, accel_steady
-
-
 class CarController():
   def __init__(self, dbc_name, CP, VM):
     self.last_steer = 0
-    self.accel_steady = 0.
     self.alert_active = False
     self.last_standstill = False
     self.standstill_req = False
     self.steer_rate_limited = False
-    self.use_interceptor = False
 
     self.packer = CANPacker(dbc_name)
 
   def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
              left_line, right_line, lead, left_lane_depart, right_lane_depart):
 
     # *** compute control surfaces ***
-
-    # gas and brake
-    interceptor_gas_cmd = 0.
-    pcm_accel_cmd = actuators.accel
-
-    if CS.CP.enableGasInterceptor:
-      # handle hysteresis when around the minimum acc speed
-      if CS.out.vEgo < MIN_ACC_SPEED:
-        self.use_interceptor = True
-      elif CS.out.vEgo > MIN_ACC_SPEED + PEDAL_HYST_GAP:
-        self.use_interceptor = False
-
-      if self.use_interceptor and enabled:
-        # only send negative accel when using interceptor. gas handles acceleration
-        # +0.18 m/s^2 offset to reduce ABS pump usage when OP is engaged
-        MAX_INTERCEPTOR_GAS = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED], [0.2, 0.5])
-        interceptor_gas_cmd = clip(actuators.accel / PEDAL_SCALE, 0., MAX_INTERCEPTOR_GAS)
-        pcm_accel_cmd = 0.18 - max(0, -actuators.accel)
-
-    pcm_accel_cmd, self.accel_steady = accel_hysteresis(pcm_accel_cmd, self.accel_steady, enabled)
-    pcm_accel_cmd = clip(pcm_accel_cmd, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
+    if CS.CP.enableGasInterceptor and enabled:
+      MAX_INTERCEPTOR_GAS = 0.5
+      # RAV4 has very sensitive has pedal
+      if CS.CP.carFingerprint in [CAR.RAV4, CAR.RAV4H, CAR.HIGHLANDER, CAR.HIGHLANDERH]:
+        PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.1, 0.3, 0.0])
+      else:
+        PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.4, 0.5, 0.0])
+      interceptor_gas_cmd = clip(PEDAL_SCALE * actuators.accel, 0., MAX_INTERCEPTOR_GAS)
+    else:
+      interceptor_gas_cmd = 0.
+    pcm_accel_cmd = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
 
     # steer torque
     new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
@@ -88,7 +61,6 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
       self.standstill_req = False
 
     self.last_steer = apply_steer
-    self.last_accel = pcm_accel_cmd
     self.last_standstill = CS.out.standstill
 
     can_sends = []

selfdrive/car/toyota/interface.py

@@ -1,7 +1,7 @@
 #!/usr/bin/env python3
 from cereal import car
 from selfdrive.config import Conversions as CV
-from selfdrive.car.toyota.values import Ecu, CAR, TSS2_CAR, NO_DSU_CAR, MIN_ACC_SPEED, PEDAL_HYST_GAP, CarControllerParams
+from selfdrive.car.toyota.values import Ecu, CAR, TSS2_CAR, NO_DSU_CAR, MIN_ACC_SPEED, CarControllerParams
 from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
 from selfdrive.car.interfaces import CarInterfaceBase
 
@@ -315,30 +315,15 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]):  # py
     # intercepting the DSU is a community feature since it requires unofficial hardware
     ret.communityFeature = ret.enableGasInterceptor or ret.enableDsu or smartDsu
 
-    if ret.enableGasInterceptor:
-      ret.longitudinalTuning.kpBP = [0., 5., MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_HYST_GAP, 35.]
-      ret.longitudinalTuning.kpV = [1.2, 0.8, 0.765, 2.255, 1.5]
-      ret.longitudinalTuning.kiBP = [0., MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_HYST_GAP, 35.]
-      ret.longitudinalTuning.kiV = [0.18, 0.165, 0.489, 0.36]
-    elif candidate in [CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.RAV4_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_NX_TSS2]:
-      # Improved longitudinal tune
-      ret.longitudinalTuning.deadzoneBP = [0., 8.05]
-      ret.longitudinalTuning.deadzoneV = [.0, .14]
-      ret.longitudinalTuning.kpBP = [0., 5., 20.]
-      ret.longitudinalTuning.kpV = [1.3, 1.0, 0.7]
-      ret.longitudinalTuning.kiBP = [0., 5., 12., 20., 27.]
-      ret.longitudinalTuning.kiV = [.35, .23, .20, .17, .1]
-      ret.stoppingDecelRate = 0.3  # reach stopping target smoothly
-      ret.startingAccelRate = 6.0  # release brakes fast
-    else:
-      # Default longitudinal tune
-      ret.longitudinalTuning.deadzoneBP = [0., 9.]
-      ret.longitudinalTuning.deadzoneV = [0., .15]
-      ret.longitudinalTuning.kpBP = [0., 5., 35.]
-      ret.longitudinalTuning.kiBP = [0., 35.]
-      ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5]
-      ret.longitudinalTuning.kiV = [0.54, 0.36]
-
+    # Default longitudinal tune
+    ret.longitudinalTuning.kpBP = [0., 5., 10.]
+    ret.longitudinalTuning.kpV = [.5, .5, 0.0]
+    ret.longitudinalTuning.kiBP = [0.]
+    ret.longitudinalTuning.kiV = [0.]
+    ret.longitudinalActuatorDelayLowerBound = 0.7
+    ret.longitudinalActuatorDelayUpperBound = 0.7
+    ret.stoppingDecelRate = 0.3  # reach stopping target smoothly
+    ret.startingAccelRate = 6.0  # release brakes fast
     return ret
 
   # returns a car.CarState

selfdrive/car/toyota/values.py

@@ -6,9 +6,7 @@
 
 Ecu = car.CarParams.Ecu
 MIN_ACC_SPEED = 19. * CV.MPH_TO_MS
-
-PEDAL_HYST_GAP = 3. * CV.MPH_TO_MS
-PEDAL_SCALE = 3.0
+PEDAL_TRANSITION = 10. * CV.MPH_TO_MS
 
 class CarControllerParams:
   ACCEL_HYST_GAP = 0.06  # don't change accel command for small oscilalitons within this value

selfdrive/controls/lib/longcontrol.py

@@ -76,9 +76,9 @@ def update(self, active, CS, CP, long_plan, accel_limits):
       v_target_upper = interp(CP.longitudinalActuatorDelayUpperBound, T_IDXS[:CONTROL_N], long_plan.speeds)
       a_target_upper = 2 * (v_target_upper - long_plan.speeds[0])/CP.longitudinalActuatorDelayUpperBound - long_plan.accels[0]
 
-      v_target = min(v_target_lower, v_target_upper)
       a_target = min(a_target_lower, a_target_upper)
 
+      v_target = long_plan.speeds[0]
       v_target_future = long_plan.speeds[-1]
     else:
       v_target = 0.0

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -19,15 +19,17 @@
 
 X_DIM = 3
 U_DIM = 1
-COST_E_DIM = 4
+PARAM_DIM= 5
+COST_E_DIM = 5
 COST_DIM = COST_E_DIM + 1
 CONSTR_DIM = 4
 
 X_EGO_OBSTACLE_COST = 3.
-V_EGO_COST = 0.
 X_EGO_COST = 0.
+V_EGO_COST = 0.
 A_EGO_COST = 0.
-J_EGO_COST = 10.
+J_EGO_COST = 5.0
+A_CHANGE_COST = .5
 DANGER_ZONE_COST = 100.
 CRASH_DISTANCE = .5
 LIMIT_COST = 1e6
@@ -80,8 +82,9 @@ def gen_long_model():
   x_obstacle = SX.sym('x_obstacle')
   a_min = SX.sym('a_min')
   a_max = SX.sym('a_max')
+  prev_a = SX.sym('prev_a')
   desired_TR = SX.sym('desired_TR')
-  model.p = vertcat(a_min, a_max, x_obstacle, desired_TR)
+  model.p = vertcat(a_min, a_max, x_obstacle, prev_a, desired_TR)
 
   # dynamics model
   f_expl = vertcat(v_ego, a_ego, j_ego)
@@ -114,7 +117,8 @@ def gen_long_mpc_solver():
 
   a_min, a_max = ocp.model.p[0], ocp.model.p[1]
   x_obstacle = ocp.model.p[2]
-  desired_TR = ocp.model.p[3]
+  prev_a = ocp.model.p[3]
+  desired_TR = ocp.model.p[4]
 
   ocp.cost.yref = np.zeros((COST_DIM, ))
   ocp.cost.yref_e = np.zeros((COST_E_DIM, ))
@@ -129,6 +133,7 @@ def gen_long_mpc_solver():
            x_ego,
            v_ego,
            a_ego,
+           20*(a_ego - prev_a),
            j_ego]
   ocp.model.cost_y_expr = vertcat(*costs)
   ocp.model.cost_y_expr_e = vertcat(*costs[:-1])
@@ -145,7 +150,7 @@ def gen_long_mpc_solver():
 
   x0 = np.zeros(X_DIM)
   ocp.constraints.x0 = x0
-  ocp.parameter_values = np.array([-1.2, 1.2, 0.0, T_REACT])
+  ocp.parameter_values = np.array([-1.2, 1.2, 0.0, 0.0, T_REACT])
 
   # We put all constraint cost weights to 0 and only set them at runtime
   cost_weights = np.zeros(CONSTR_DIM)
@@ -195,13 +200,14 @@ def reset(self):
     self.solver = AcadosOcpSolver('long', N, EXPORT_DIR)
     self.v_solution = [0.0 for i in range(N+1)]
     self.a_solution = [0.0 for i in range(N+1)]
+    self.prev_a = self.a_solution
     self.j_solution = [0.0 for i in range(N)]
     self.yref = np.zeros((N+1, COST_DIM))
     self.solver.cost_set_slice(0, N, "yref", self.yref[:N])
     self.solver.set(N, "yref", self.yref[N][:COST_E_DIM])
     self.x_sol = np.zeros((N+1, X_DIM))
     self.u_sol = np.zeros((N,1))
-    self.params = np.zeros((N+1,4))
+    self.params = np.zeros((N+1, PARAM_DIM))
     for i in range(N+1):
       self.solver.set(i, 'x', np.zeros(X_DIM))
     self.last_cloudlog_t = 0
@@ -228,10 +234,12 @@ def set_weights_for_lead_policy(self):
     j_ego_cost_multiplier = interp(self.desired_TR, TRs, [.6, .8, 1.])
     d_zone_cost_multiplier = interp(self.desired_TR, TRs, [4., 2., 1.])
 
-    W = np.diag([X_EGO_OBSTACLE_COST * x_ego_obstacle_cost_multiplier, X_EGO_COST, 
-                 V_EGO_COST, A_EGO_COST, J_EGO_COST * j_ego_cost_multiplier])
-    Ws = np.tile(W[None], reps=(N,1,1))
-    self.solver.cost_set_slice(0, N, 'W', Ws, api='old')
+    W = np.asfortranarray(np.diag([X_EGO_OBSTACLE_COST * x_ego_obstacle_cost_multiplier,
+                                   X_EGO_COST, V_EGO_COST, A_EGO_COST, A_CHANGE_COST, 
+                                   J_EGO_COST * j_ego_cost_multiplier]))
+    for i in range(N):
+      W[4,4] = A_CHANGE_COST * np.interp(T_IDXS[i], [0.0, 1.0, 2.0], [1.0, 1.0, 0.0])
+      self.solver.cost_set(i, 'W', W)
     # Setting the slice without the copy make the array not contiguous,
     # causing issues with the C interface.
     self.solver.cost_set(N, 'W', np.copy(W[:COST_E_DIM, :COST_E_DIM]))
@@ -242,9 +250,9 @@ def set_weights_for_lead_policy(self):
     self.solver.cost_set_slice(0, N+1, 'Zl', Zls, api='old')
 
   def set_weights_for_xva_policy(self):
-    W = np.diag([0., 10., 1., 10., 1.])
-    Ws = np.tile(W[None], reps=(N,1,1))
-    self.solver.cost_set_slice(0, N, 'W', Ws, api='old')
+    W = np.asfortranarray(np.diag([0., 10., 1., 10., 0.0, 1.]))
+    for i in range(N):
+      self.solver.cost_set(i, 'W', W)
     # Setting the slice without the copy make the array not contiguous,
     # causing issues with the C interface.
     self.solver.cost_set(N, 'W', np.copy(W[:COST_E_DIM, :COST_E_DIM]))
@@ -342,7 +350,8 @@ def update(self, carstate, radarstate, v_cruise):
     x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle])
     self.source = SOURCES[np.argmin(x_obstacles[0])]
     self.params[:,2] = np.min(x_obstacles, axis=1)
-    self.params[:,3] = self.desired_TR
+    self.params[:,3] = np.copy(self.prev_a)
+    self.params[:,4] = self.desired_TR
 
     self.run()
     if (np.any(lead_xv_0[:,0] - self.x_sol[:,0] < CRASH_DISTANCE) and
@@ -362,7 +371,8 @@ def update_with_xva(self, x, v, a):
     x_obstacle = 1e5*np.ones((N+1))
     # KRKeegan note from sshane: this doesn't consider fourth TR param, set to anything if needed
     self.params = np.concatenate([self.accel_limit_arr,
-                             x_obstacle[:,None]], axis=1)
+                             x_obstacle[:,None],
+                             self.prev_a], axis=1)
     self.run()
 
 
@@ -379,6 +389,8 @@ def run(self):
     self.a_solution = self.x_sol[:,2]
     self.j_solution = self.u_sol[:,0]
 
+    self.prev_a = interp(T_IDXS + 0.05, T_IDXS, self.a_solution)
+
     t = sec_since_boot()
     if self.solution_status != 0:
       if t > self.last_cloudlog_t + 5.0: