Low speed lateral like before (#26022)

* Add explicit cost on steering wheel movement

* Laxer low speed control

* Laxer low speed control

* Lower min speed now there is a cost

* 3m/s

* Similar to old master

* Add cost

* Crazy high

* Update ref

* comment
old-commit-hash: 2c9b150761

selfdrive/controls/lib/latcontrol_torque.py

@@ -17,6 +17,8 @@ from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
 # friction in the steering wheel that needs to be overcome to
 # move it at all, this is compensated for too.
 
+LOW_SPEED_FACTOR = 100
+
 
 class LatControlTorque(LatControl):
   def __init__(self, CP, CI):
@@ -55,13 +57,15 @@ class LatControlTorque(LatControl):
       actual_lateral_accel = actual_curvature * CS.vEgo ** 2
       lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2
 
-      low_speed_factor = interp(CS.vEgo, [0, 10, 20], [500, 500, 200])
-      setpoint = desired_lateral_accel + low_speed_factor * desired_curvature
-      measurement = actual_lateral_accel + low_speed_factor * actual_curvature
+      setpoint = desired_lateral_accel + LOW_SPEED_FACTOR * desired_curvature
+      measurement = actual_lateral_accel + LOW_SPEED_FACTOR * actual_curvature
       error = setpoint - measurement
       gravity_adjusted_lateral_accel = desired_lateral_accel - params.roll * ACCELERATION_DUE_TO_GRAVITY
-      pid_log.error = self.torque_from_lateral_accel(error, self.torque_params, error, lateral_accel_deadzone, friction_compensation=False)
-      ff = self.torque_from_lateral_accel(gravity_adjusted_lateral_accel, self.torque_params, error, lateral_accel_deadzone, friction_compensation=True)
+      pid_log.error = self.torque_from_lateral_accel(error, self.torque_params, error,
+                                                     lateral_accel_deadzone, friction_compensation=False)
+      ff = self.torque_from_lateral_accel(gravity_adjusted_lateral_accel, self.torque_params,
+                                          desired_lateral_accel - actual_lateral_accel,
+                                          lateral_accel_deadzone, friction_compensation=True)
 
       freeze_integrator = steer_limited or CS.steeringPressed or CS.vEgo < 5
       output_torque = self.pid.update(pid_log.error,

selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py

@@ -19,7 +19,7 @@ X_DIM = 4
 P_DIM = 2
 N = 16
 COST_E_DIM = 3
-COST_DIM = COST_E_DIM + 1
+COST_DIM = COST_E_DIM + 2
 SPEED_OFFSET = 10.0
 MODEL_NAME = 'lat'
 ACADOS_SOLVER_TYPE = 'SQP_RTI'
@@ -89,13 +89,21 @@ def gen_lat_ocp():
 
   ocp.cost.yref = np.zeros((COST_DIM, ))
   ocp.cost.yref_e = np.zeros((COST_E_DIM, ))
+  # Add offset to smooth out low speed control
+  # TODO unclear if this right solution long term
+  v_ego_offset = v_ego + SPEED_OFFSET
+  # TODO there are two costs on psi_rate_ego_dot, one
+  # is correlated to jerk the other to steering wheel movement
+  # the steering wheel movement cost is added to prevent excessive
+  # wheel movements
   ocp.model.cost_y_expr = vertcat(y_ego,
-                                  ((v_ego + SPEED_OFFSET) * psi_ego),
-                                  ((v_ego + SPEED_OFFSET) * psi_rate_ego),
-                                  ((v_ego + SPEED_OFFSET) * psi_rate_ego_dot))
+                                  v_ego_offset * psi_ego,
+                                  v_ego_offset * psi_rate_ego,
+                                  v_ego_offset * psi_rate_ego_dot,
+                                  psi_rate_ego_dot / (v_ego + 0.1))
   ocp.model.cost_y_expr_e = vertcat(y_ego,
-                                  ((v_ego + SPEED_OFFSET) * psi_ego),
-                                  ((v_ego + SPEED_OFFSET) * psi_rate_ego))
+                                   v_ego_offset * psi_ego,
+                                   v_ego_offset * psi_rate_ego)
 
   # set constraints
   ocp.constraints.constr_type = 'BGH'
@@ -144,8 +152,12 @@ class LateralMpc():
     self.solve_time = 0.0
     self.cost = 0
 
-  def set_weights(self, path_weight, heading_weight, yaw_rate_weight, yaw_accel_cost):
-    W = np.asfortranarray(np.diag([path_weight, heading_weight, yaw_rate_weight, yaw_accel_cost]))
+  def set_weights(self, path_weight, heading_weight,
+                  lat_accel_weight, lat_jerk_weight,
+                  steering_rate_weight):
+    W = np.asfortranarray(np.diag([path_weight, heading_weight,
+                                   lat_accel_weight, lat_jerk_weight,
+                                   steering_rate_weight]))
     for i in range(N):
       self.solver.cost_set(i, 'W', W)
     self.solver.cost_set(N, 'W', W[:COST_E_DIM,:COST_E_DIM])

selfdrive/controls/lib/lateral_planner.py

@@ -17,8 +17,13 @@ PATH_COST = 1.0
 LATERAL_MOTION_COST = 0.11
 LATERAL_ACCEL_COST = 0.0
 LATERAL_JERK_COST = 0.05
+# Extreme steering rate is unpleasant, even
+# when it does not cause bad jerk.
+# TODO this cost should be lowered when low
+# speed lateral control is stable on all cars
+STEERING_RATE_COST = 800.0
 
-MIN_SPEED = 1.5
+MIN_SPEED = .3
 
 
 class LateralPlanner:
@@ -67,7 +72,8 @@ class LateralPlanner:
 
     d_path_xyz = self.path_xyz
     self.lat_mpc.set_weights(PATH_COST, LATERAL_MOTION_COST,
-                             LATERAL_ACCEL_COST, LATERAL_JERK_COST)
+                             LATERAL_ACCEL_COST, LATERAL_JERK_COST,
+                             STEERING_RATE_COST)
 
     y_pts = np.interp(self.v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:, 1])
     heading_pts = np.interp(self.v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw)

selfdrive/controls/tests/test_lateral_mpc.py

@@ -10,7 +10,7 @@ def run_mpc(lat_mpc=None, v_ref=30., x_init=0., y_init=0., psi_init=0., curvatur
 
   if lat_mpc is None:
     lat_mpc = LateralMpc()
-  lat_mpc.set_weights(1., 1., 0.0, 1.)
+  lat_mpc.set_weights(1., .1, 0.0, .05, 800)
 
   y_pts = poly_shift * np.ones(LAT_MPC_N + 1)
   heading_pts = np.zeros(LAT_MPC_N + 1)
@@ -77,9 +77,9 @@ class TestLateralMpc(unittest.TestCase):
 
   def test_switch_convergence(self):
     lat_mpc = LateralMpc()
-    sol = run_mpc(lat_mpc=lat_mpc, poly_shift=30.0, v_ref=7.0)
+    sol = run_mpc(lat_mpc=lat_mpc, poly_shift=3.0, v_ref=7.0)
     right_psi_deg = np.degrees(sol[:,2])
-    sol = run_mpc(lat_mpc=lat_mpc, poly_shift=-30.0, v_ref=7.0)
+    sol = run_mpc(lat_mpc=lat_mpc, poly_shift=-3.0, v_ref=7.0)
     left_psi_deg = np.degrees(sol[:,2])
     np.testing.assert_almost_equal(right_psi_deg, -left_psi_deg, decimal=3)
 

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-f636c68e2b4ed88d3731930cf15b6dee984eb6dd
+0f0a9aa8fed425468c488a4f8b7581c48d724e67