Revert "Merge remote-tracking branch 'openpilot/longitudinal_dont_cre…

…ep' into master-dev"

This reverts commit 89d4404, reversing
changes made to 2f14fe8.

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -34,7 +34,7 @@
 V_EGO_COST = 0.
 A_EGO_COST = 0.
 J_EGO_COST = 5.0
-A_CHANGE_COST = 200.
+A_CHANGE_COST = .5
 DANGER_ZONE_COST = 100.
 CRASH_DISTANCE = .5
 LIMIT_COST = 1e6
@@ -57,13 +57,7 @@ def get_stopped_equivalence_factor(v_lead):
   return (v_lead**2) / (2 * COMFORT_BRAKE)
 
 def get_safe_obstacle_distance(v_ego):
-  v_min = .5  # below this speed the follow distance does not decrease further
-  eps = 1e-3
-  # smooth_max(v_min, v_ego)
-  vego_smooth_max = (v_ego + v_min + np.sqrt(eps+ (v_ego-v_min)**2))/2
-  # NOTE: one could add  "-v_min * T_FOLLOW"
-  #       to achieve desired_follow_distance(0, 0) = STOP_DISTANCE
-  return (v_ego**2) / (2 * COMFORT_BRAKE) + T_FOLLOW * (vego_smooth_max) + STOP_DISTANCE
+  return (v_ego**2) / (2 * COMFORT_BRAKE) + T_FOLLOW * v_ego + STOP_DISTANCE
 
 def desired_follow_distance(v_ego, v_lead):
   return get_safe_obstacle_distance(v_ego) - get_stopped_equivalence_factor(v_lead)
@@ -142,7 +136,7 @@ def gen_long_mpc_solver():
            x_ego,
            v_ego,
            a_ego,
-           a_ego - prev_a,
+           20*(a_ego - prev_a),
            j_ego]
   ocp.model.cost_y_expr = vertcat(*costs)
   ocp.model.cost_y_expr_e = vertcat(*costs[:-1])