diff --git a/phonelibs/acados/build.sh b/phonelibs/acados/build.sh index 5a12efa2e8..3301accaa4 100755 --- a/phonelibs/acados/build.sh +++ b/phonelibs/acados/build.sh @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:3adaf7cd3c6bc6bb6e7c8bc35c9107ce618467c46e54e051785e786241281846 +oid sha256:d5c3a2f90f4903e82ea927b13433f01dc0c3fbbe8b7b7f6353fb5981aeefd32f size 2053 diff --git a/phonelibs/acados/x86_64/lib/libacados.so b/phonelibs/acados/x86_64/lib/libacados.so index 52f08c5bc0..37c57be1c0 100644 --- a/phonelibs/acados/x86_64/lib/libacados.so +++ b/phonelibs/acados/x86_64/lib/libacados.so @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:118688fbed76739288ee75c93c9f0bcfe806cae8506532c88102a9fb41d4c797 +oid sha256:13aa7b28a5d778b82c65e88ec3c45a216a29f0ab62b3c989ab4d2ec24abb9ce9 size 525968 diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 824b6032e5..c64c09f32e 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -23,13 +23,15 @@ X_DIM = 3 U_DIM = 1 COST_E_DIM = 3 COST_DIM = COST_E_DIM + 1 +CONSTR_DIM = 4 MIN_ACCEL = -3.5 -X_EGO_COST = 80. -X_EGO_E2E_COST = 100. -A_EGO_COST = .1 -J_EGO_COST = .2 -DANGER_ZONE_COST = 1e3 + +X_EGO_COST = 3. +X_EGO_E2E_COST = 10. +A_EGO_COST = 1. +J_EGO_COST = 10. +DANGER_ZONE_COST = 100. CRASH_DISTANCE = .5 LIMIT_COST = 1e6 T_IDXS = np.array(T_IDXS_LST) @@ -110,21 +112,20 @@ def gen_long_mpc_solver(): ocp.cost.yref_e = np.zeros((COST_E_DIM, )) desired_dist_comfort = get_safe_obstacle_distance(v_ego) - desired_dist_danger = (7/8) * get_safe_obstacle_distance(v_ego) costs = [((x_obstacle - x_ego) - (desired_dist_comfort)) / (v_ego + 10.), x_ego, - a_ego * (v_ego + 10.), - j_ego * (v_ego + 10.)] + a_ego, + j_ego] ocp.model.cost_y_expr = vertcat(*costs) ocp.model.cost_y_expr_e = vertcat(*costs[:-1]) constraints = vertcat((v_ego), (a_ego - a_min), (a_max - a_ego), - ((x_obstacle - x_ego) - (desired_dist_danger)) / (v_ego + 10.)) + ((x_obstacle - x_ego) - (3/4) * (desired_dist_comfort)) / (v_ego + 10.)) ocp.model.con_h_expr = constraints - ocp.model.con_h_expr_e = constraints + ocp.model.con_h_expr_e = vertcat(np.zeros(CONSTR_DIM)) x0 = np.zeros(X_DIM) ocp.constraints.x0 = x0 @@ -134,19 +135,17 @@ def gen_long_mpc_solver(): # acceleration as well as giving an assymetrical # cost on approaching a lead. We only use lower # bounds with an L2 cost. - l1_penalty = 0.0 - l2_penalty = 1.0 - weights = np.array([1e6, 1e6, 1e6, 0.0]) - ocp.cost.zl = l1_penalty * weights - ocp.cost.Zl = l2_penalty * weights - ocp.cost.Zu = 0.0 * weights - ocp.cost.zu = 0.0 * weights + cost_weights = np.zeros(CONSTR_DIM) + ocp.cost.zl = cost_weights + ocp.cost.Zl = cost_weights + ocp.cost.Zu = cost_weights + ocp.cost.zu = cost_weights - ocp.constraints.lh = np.array([0.0, 0.0, 0.0, 0.0]) - ocp.constraints.lh_e = np.array([0.0, 0.0, 0.0, 0.0]) - ocp.constraints.uh = np.array([1e3, 1e3, 1e3, 1e4]) - ocp.constraints.uh_e = np.array([1e3, 1e3, 1e3, 1e6]) - ocp.constraints.idxsh = np.array([0,1,2,3]) + ocp.constraints.lh = np.zeros(CONSTR_DIM) + ocp.constraints.lh_e = np.zeros(CONSTR_DIM) + ocp.constraints.uh = 1e4*np.ones(CONSTR_DIM) + ocp.constraints.uh_e = 1e4*np.ones(CONSTR_DIM) + ocp.constraints.idxsh = np.arange(CONSTR_DIM) ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' @@ -154,7 +153,7 @@ def gen_long_mpc_solver(): ocp.solver_options.integrator_type = 'ERK' ocp.solver_options.nlp_solver_type = 'SQP_RTI' - ocp.solver_options.qp_solver_iter_max = 3 + ocp.solver_options.qp_solver_iter_max = 4 # set prediction horizon ocp.solver_options.tf = Tf @@ -188,10 +187,7 @@ class LongitudinalMpc(): self.solver.set(i, 'x', np.zeros(X_DIM)) self.last_cloudlog_t = 0 self.status = False - self.new_lead = False - self.prev_lead_status = False self.crash_cnt = 0.0 - self.prev_lead_x = 10 self.solution_status = 0 self.x0 = np.zeros(X_DIM) self.set_weights() @@ -265,15 +261,9 @@ class LongitudinalMpc(): a_lead = 0.0 self.a_lead_tau = lead.aLeadTau - self.new_lead = False lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, self.a_lead_tau) - if not self.prev_lead_status or abs(x_lead - self.prev_lead_x) > 2.5: - self.new_lead = True - self.prev_lead_status = True - self.prev_lead_x = x_lead else: - self.prev_lead_status = False # Fake a fast lead car, so mpc can keep running in the same mode x_lead = 50.0 v_lead = v_ego + 10.0 @@ -305,8 +295,8 @@ class LongitudinalMpc(): # Fake an obstacle for cruise # TODO find cleaner way to write hacky fake cruise obstacle - cruise_lower_bound = v_ego - (1.0 + ((v_ego + 15)/60) * T_IDXS) - cruise_upper_bound = v_ego + (.7 + .7*T_IDXS) + cruise_lower_bound = v_ego + (3/4) * self.cruise_min_a * T_IDXS + cruise_upper_bound = v_ego + (3/4) * self.cruise_max_a * T_IDXS v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), cruise_lower_bound, cruise_upper_bound) @@ -354,7 +344,6 @@ class LongitudinalMpc(): self.last_cloudlog_t = t cloudlog.warning("Long mpc reset, solution_status: %s" % ( self.solution_status)) - self.prev_lead_status = False self.reset() diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 339b3c3c5b..411bb71e43 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -4,7 +4,6 @@ import numpy as np from common.numpy_fast import interp import cereal.messaging as messaging -from cereal import log from common.realtime import DT_MDL from selfdrive.modeld.constants import T_IDXS from selfdrive.config import Conversions as CV @@ -51,8 +50,6 @@ class Planner(): self.v_desired = init_v self.a_desired = init_a self.alpha = np.exp(-DT_MDL/2.0) - self.lead_0 = log.ModelDataV2.LeadDataV3.new_message() - self.lead_1 = log.ModelDataV2.LeadDataV3.new_message() self.v_desired_trajectory = np.zeros(CONTROL_N) self.a_desired_trajectory = np.zeros(CONTROL_N) @@ -70,9 +67,6 @@ class Planner(): long_control_state = sm['controlsState'].longControlState force_slow_decel = sm['controlsState'].forceDecel - self.lead_0 = sm['radarState'].leadOne - self.lead_1 = sm['radarState'].leadTwo - enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping) if not enabled or sm['carState'].gasPressed: self.v_desired = v_ego @@ -121,7 +115,7 @@ class Planner(): longitudinalPlan.accels = [float(x) for x in self.a_desired_trajectory] longitudinalPlan.jerks = [float(x) for x in self.j_desired_trajectory] - longitudinalPlan.hasLead = self.mpc.prev_lead_status + longitudinalPlan.hasLead = sm['radarState'].leadOne.status longitudinalPlan.longitudinalPlanSource = self.mpc.source longitudinalPlan.fcw = self.fcw diff --git a/selfdrive/test/longitudinal_maneuvers/maneuver.py b/selfdrive/test/longitudinal_maneuvers/maneuver.py index b3482f4b74..9b4d016430 100644 --- a/selfdrive/test/longitudinal_maneuvers/maneuver.py +++ b/selfdrive/test/longitudinal_maneuvers/maneuver.py @@ -48,7 +48,7 @@ class Maneuver(): speed_lead, log['acceleration']])) - if d_rel < 1.0 and (self.only_radar or prob > 0.5): + if d_rel < .4 and (self.only_radar or prob > 0.5): print("Crashed!!!!") valid = False diff --git a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py index c433f81d7a..c4bca06a65 100755 --- a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py +++ b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py @@ -54,7 +54,7 @@ maneuvers = [ breakpoints=[0., 15., 21.66], ), Maneuver( - 'steady state following a car at 20m/s, then lead decel to 0mph at 4m/s^2', + 'steady state following a car at 20m/s, then lead decel to 0mph at 3.5m/s^2', duration=40., initial_speed=20., lead_relevancy=True, @@ -62,7 +62,7 @@ maneuvers = [ speed_lead_values=[20., 20., 0.], prob_lead_values=[0., 1., 1.], cruise_values=[20., 20., 20.], - breakpoints=[2., 2.01, 7.01], + breakpoints=[2., 2.01, 8.01], ), Maneuver( "approach stopped car at 20m/s", diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 3d2d0179d5..2e2bfb680f 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -7736bb61869f54225a66f6cf1c43086c3fb9b507 \ No newline at end of file +a69e0b5fe4f506b8b9d372bfa68d8a47cf105a3f \ No newline at end of file