diff --git a/models/supercombo.dlc b/models/supercombo.dlc index 2ebf4fa828c01b..063dbb2681a756 100644 --- a/models/supercombo.dlc +++ b/models/supercombo.dlc @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:209e9544e456dbc2a7d60490da65154e129bc84830909d8d931f97b3df93949b -size 56684955 +oid sha256:cd3e47f88e7c3b58fa25912c35680b56ba12c4cb4adc1ec33d962688d82a3f28 +size 56685051 diff --git a/models/supercombo.onnx b/models/supercombo.onnx index 17d233dad7bc5c..4f067360d2da11 100644 --- a/models/supercombo.onnx +++ b/models/supercombo.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:2365bae967cce21ce68707c30bf2981bb7081ee5c3e6a3dff793e660f23ff622 -size 57554657 +oid sha256:d28a836c0e7c5c594b7648d54b0f244797b05abb120523632a7f3d6922c16b52 +size 57554673 diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 3a8234706fc34a..681ed917598595 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -3,7 +3,7 @@ import numpy as np from common.realtime import sec_since_boot -from common.numpy_fast import clip, interp +from common.numpy_fast import interp, clip from selfdrive.swaglog import cloudlog from selfdrive.modeld.constants import index_function from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU @@ -35,7 +35,8 @@ A_EGO_COST = 0. J_EGO_COST = 5.0 A_CHANGE_COST = .5 -DANGER_ZONE_COST = 100. +J_EGO_COST = 5. +DANGER_ZONE_COST = 10. CRASH_DISTANCE = .5 LIMIT_COST = 1e6 @@ -228,12 +229,10 @@ 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, A_CHANGE_COST, J_EGO_COST])) + W = np.diag([0., .03, .0, 10., 0.0, 1.]) 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]) + W[4,4] = .1 * 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])) # Set L2 slack cost on lower bound constraints @@ -242,11 +241,9 @@ def set_weights_for_lead_policy(self): self.solver.cost_set(i, 'Zl', Zl) def set_weights_for_xva_policy(self): - W = np.asfortranarray(np.diag([0., 10., 1., 10., 0.0, 1.])) + W = np.diag([0., 0., .0, 1., 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])) # Set L2 slack cost on lower bound constraints @@ -299,15 +296,18 @@ def set_accel_limits(self, min_a, max_a): self.cruise_max_a = max_a def update(self, carstate, radarstate, v_cruise, prev_accel_constraint=False): - v_ego = self.x0[1] + #v_ego = self.x0[1] a_ego = self.x0[2] + self.yref[:,1] = x + self.yref[:,2] = v + self.yref[:,3] = a self.status = radarstate.leadOne.status or radarstate.leadTwo.status lead_xv_0 = self.process_lead(radarstate.leadOne) lead_xv_1 = self.process_lead(radarstate.leadTwo) # set accel limits in params - self.params[:,0] = interp(float(self.status), [0.0, 1.0], [self.cruise_min_a, MIN_ACCEL]) + self.params[:,0] = self.cruise_min_a self.params[:,1] = self.cruise_max_a # To estimate a safe distance from a moving lead, we calculate how much stopping @@ -316,22 +316,23 @@ def update(self, carstate, radarstate, v_cruise, prev_accel_constraint=False): 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]) - # Fake an obstacle for cruise, this ensures smooth acceleration to set speed - # when the leads are no factor. - v_lower = v_ego + (T_IDXS * self.cruise_min_a * 1.05) - v_upper = v_ego + (T_IDXS * self.cruise_max_a * 1.05) - v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), - v_lower, - v_upper) - cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped) - - 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) + cruise_target = T_IDXS * v_cruise + x[0] + + x_targets = np.column_stack([x, + lead_0_obstacle - (3/4) * get_safe_obstacle_distance(v), + lead_1_obstacle - (3/4) * get_safe_obstacle_distance(v), + cruise_target]) + #self.source = SOURCES[np.argmin(x_obstacles[0])] + self.params[:,2] = 1e3 if prev_accel_constraint: - self.params[:,3] = np.copy(self.prev_a) + self.params[:, 3] = np.copy(self.prev_a) else: - self.params[:,3] = a_ego + self.params[:, 3] = a_ego + + self.yref[:,1] = np.min(x_targets, axis=1) + for i in range(N): + self.solver.set(i, "yref", self.yref[i]) + self.solver.set(N, "yref", self.yref[N][:COST_E_DIM]) self.run() if (np.any(lead_xv_0[:,0] - self.x_sol[:,0] < CRASH_DISTANCE) and diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 41bae4c47518b8..6a372ca827483d 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -87,17 +87,27 @@ def update(self, sm): # clip limits, cannot init MPC outside of bounds accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05) 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_accel_limits(-3.5, 2.) self.mpc.set_cur_state(self.v_desired, self.a_desired) - self.mpc.update(sm['carState'], sm['radarState'], v_cruise, prev_accel_constraint=prev_accel_constraint) + if (len(sm['modelV2'].position.x) == 33 and + len(sm['modelV2'].velocity.x) == 33 and + len(sm['modelV2'].acceleration.x) == 33): + x = np.interp(T_IDXS_MPC, T_IDXS, sm['modelV2'].position.x) + v = np.interp(T_IDXS_MPC, T_IDXS, sm['modelV2'].velocity.x) + a = np.interp(T_IDXS_MPC, T_IDXS, sm['modelV2'].acceleration.x) + else: + x = np.zeros(len(T_IDXS_MPC)) + v = np.zeros(len(T_IDXS_MPC)) + a = np.zeros(len(T_IDXS_MPC)) + self.mpc.update(sm['carState'], sm['radarState'], v_cruise, x, v, a) 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) self.j_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC[:-1], self.mpc.j_solution) - # TODO counter is only needed because radar is glitchy, remove once radar is gone - self.fcw = self.mpc.crash_cnt > 5 - if self.fcw: - cloudlog.info("FCW triggered") + #TODO counter is only needed because radar is glitchy, remove once radar is gone + self.fcw = False #self.mpc.crash_cnt > 5 + #if self.fcw: + # cloudlog.info("FCW triggered") # Interpolate 0.05 seconds and save as starting point for next iteration a_prev = self.a_desired @@ -118,7 +128,7 @@ def publish(self, sm, pm): longitudinalPlan.jerks = [float(x) for x in self.j_desired_trajectory] longitudinalPlan.hasLead = sm['radarState'].leadOne.status - longitudinalPlan.longitudinalPlanSource = self.mpc.source + #longitudinalPlan.longitudinalPlanSource = self.mpc.source longitudinalPlan.fcw = self.fcw pm.send('longitudinalPlan', plan_send) diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index 22d0716f67140f..8491753adc9bec 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -182,6 +182,7 @@ void fill_plan(cereal::ModelDataV2::Builder &framed, const ModelOutputPlanPredic std::array pos_x, pos_y, pos_z; std::array pos_x_std, pos_y_std, pos_z_std; std::array vel_x, vel_y, vel_z; + std::array accel_x, accel_y, accel_z; std::array rot_x, rot_y, rot_z; std::array rot_rate_x, rot_rate_y, rot_rate_z; @@ -195,6 +196,9 @@ void fill_plan(cereal::ModelDataV2::Builder &framed, const ModelOutputPlanPredic vel_x[i] = plan.mean[i].velocity.x; vel_y[i] = plan.mean[i].velocity.y; vel_z[i] = plan.mean[i].velocity.z; + accel_x[i] = plan.mean[i].acceleration.x; + accel_y[i] = plan.mean[i].acceleration.y; + accel_z[i] = plan.mean[i].acceleration.z; rot_x[i] = plan.mean[i].rotation.x; rot_y[i] = plan.mean[i].rotation.y; rot_z[i] = plan.mean[i].rotation.z; @@ -205,6 +209,7 @@ void fill_plan(cereal::ModelDataV2::Builder &framed, const ModelOutputPlanPredic fill_xyzt(framed.initPosition(), T_IDXS_FLOAT, pos_x, pos_y, pos_z, pos_x_std, pos_y_std, pos_z_std); fill_xyzt(framed.initVelocity(), T_IDXS_FLOAT, vel_x, vel_y, vel_z); + fill_xyzt(framed.initAcceleration(), T_IDXS_FLOAT, accel_x, accel_y, accel_z); fill_xyzt(framed.initOrientation(), T_IDXS_FLOAT, rot_x, rot_y, rot_z); fill_xyzt(framed.initOrientationRate(), T_IDXS_FLOAT, rot_rate_x, rot_rate_y, rot_rate_z); }