diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index df6d8e0fb64316..672282e40bb76f 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -51,14 +51,14 @@ MAX_BRAKE = 9.81 -def get_stopped_equivalence_factor(v_lead): - return T_REACT * v_lead + (v_lead*v_lead) / (2 * MAX_BRAKE) +def get_stopped_equivalence_factor(v_lead, t_react=T_REACT): + return t_react * v_lead + (v_lead*v_lead) / (2 * MAX_BRAKE) -def get_safe_obstacle_distance(v_ego): - return 2 * T_REACT * v_ego + (v_ego*v_ego) / (2 * MAX_BRAKE) + 4.0 +def get_safe_obstacle_distance(v_ego, t_react=T_REACT): + return 2 * t_react * v_ego + (v_ego*v_ego) / (2 * MAX_BRAKE) + 4.0 -def desired_follow_distance(v_ego, v_lead): - return get_safe_obstacle_distance(v_ego) - get_stopped_equivalence_factor(v_lead) +def desired_follow_distance(v_ego, v_lead, t_react=T_REACT): + return get_safe_obstacle_distance(v_ego, t_react) - get_stopped_equivalence_factor(v_lead, t_react) def gen_long_model(): @@ -83,9 +83,10 @@ def gen_long_model(): # live parameters x_obstacle = SX.sym('x_obstacle') + desired_TR = SX.sym('desired_TR') a_min = SX.sym('a_min') a_max = SX.sym('a_max') - model.p = vertcat(a_min, a_max, x_obstacle) + model.p = vertcat(a_min, a_max, x_obstacle, desired_TR) # dynamics model f_expl = vertcat(v_ego, a_ego, j_ego) @@ -118,11 +119,12 @@ 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] ocp.cost.yref = np.zeros((COST_DIM, )) ocp.cost.yref_e = np.zeros((COST_E_DIM, )) - desired_dist_comfort = get_safe_obstacle_distance(v_ego) + desired_dist_comfort = get_safe_obstacle_distance(v_ego, desired_TR) # The main cost in normal operation is how close you are to the "desired" distance # from an obstacle at every timestep. This obstacle can be a lead car @@ -148,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]) + ocp.parameter_values = np.array([-1.2, 1.2, 0.0, T_REACT]) # defaults # We put all constraint cost weights to 0 and only set them at runtime cost_weights = np.zeros(CONSTR_DIM) @@ -186,8 +188,10 @@ def gen_long_mpc_solver(): class LongitudinalMpc(): - def __init__(self, e2e=False): + def __init__(self, e2e=False, desired_TR=T_REACT): self.e2e = e2e + self.desired_TR = desired_TR + self.v_ego = 0. self.reset() self.accel_limit_arr = np.zeros((N+1, 2)) self.accel_limit_arr[:,0] = -1.2 @@ -205,7 +209,7 @@ def reset(self): self.solver.cost_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,3)) + self.params = np.zeros((N+1,4)) for i in range(N+1): self.solver.set(i, 'x', np.zeros(X_DIM)) self.last_cloudlog_t = 0 @@ -222,7 +226,17 @@ def set_weights(self): self.set_weights_for_lead_policy() def set_weights_for_lead_policy(self): - W = np.asfortranarray(np.diag([X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, J_EGO_COST])) + # WARNING: deceleration tests with these costs: + # 1.0 TR fails at 3 m/s/s test + # 1.1 TR fails at 3+ m/s/s test + # 1.2-1.8 TR succeeds at all tests with no FCW + + TRs = [1.2, 1.8, 2.7] + x_ego_obstacle_cost_multiplier = interp(self.desired_TR, TRs, [3., 1.0, 0.1]) + j_ego_cost_multiplier = interp(self.desired_TR, TRs, [0.5, 1.0, 1.0]) + d_zone_cost_multiplier = interp(self.desired_TR, TRs, [4., 1.0, 1.0]) + + W = np.asfortranarray(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])) for i in range(N): self.solver.cost_set(i, 'W', W) # Setting the slice without the copy make the array not contiguous, @@ -230,7 +244,7 @@ def set_weights_for_lead_policy(self): self.solver.cost_set(N, 'W', np.copy(W[:COST_E_DIM, :COST_E_DIM])) # Set L2 slack cost on lower bound constraints - Zl = np.array([LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST]) + Zl = np.array([LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST * d_zone_cost_multiplier]) for i in range(N): self.solver.cost_set(i, 'Zl', Zl) @@ -291,7 +305,12 @@ def set_accel_limits(self, min_a, max_a): self.cruise_min_a = min_a self.cruise_max_a = max_a + def set_desired_TR(self, desired_TR): + self.desired_TR = clip(desired_TR, 1.2, 2.7) + self.set_weights() + def update(self, carstate, radarstate, v_cruise): + self.v_ego = carstate.vEgo v_ego = self.x0[1] self.status = radarstate.leadOne.status or radarstate.leadTwo.status @@ -305,8 +324,8 @@ def update(self, carstate, radarstate, v_cruise): # To estimate a safe distance from a moving lead, we calculate how much stopping # distance that lead needs as a minimum. We can add that to the current distance # and then treat that as a stopped car/obstacle at this new distance. - lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1]) - lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1]) + lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1], self.desired_TR) + lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1], self.desired_TR) # Fake an obstacle for cruise, this ensures smooth acceleration to set speed # when the leads are no factor. @@ -315,11 +334,12 @@ def update(self, carstate, radarstate, v_cruise): v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), cruise_lower_bound, cruise_upper_bound) - cruise_obstacle = T_IDXS*v_cruise_clipped + get_safe_obstacle_distance(v_cruise_clipped) + cruise_obstacle = T_IDXS*v_cruise_clipped + get_safe_obstacle_distance(v_cruise_clipped, self.desired_TR) 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.run() if (np.any(lead_xv_0[:,0] - self.x_sol[:,0] < CRASH_DISTANCE) and @@ -338,8 +358,9 @@ def update_with_xva(self, x, v, a): self.accel_limit_arr[:,0] = -10. self.accel_limit_arr[:,1] = 10. x_obstacle = 1e5*np.ones((N+1)) + desired_TR = T_REACT*np.ones((N+1)) self.params = np.concatenate([self.accel_limit_arr, - x_obstacle[:,None]], axis=1) + x_obstacle[:,None], desired_TR[:,None]], axis=1) self.run() diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 051a68a7415df9..3c0e3f95291a71 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -88,6 +88,7 @@ def update(self, sm, CP): accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05) self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) self.mpc.set_cur_state(self.v_desired, self.a_desired) + # self.mpc.set_desired_TR(1.8) self.mpc.update(sm['carState'], sm['radarState'], v_cruise) self.v_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.v_solution) self.a_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.a_solution)