From 5c8db326cfdf0c682ecac0d6fffdfb041748fb58 Mon Sep 17 00:00:00 2001 From: Jason Wen <47793918+sunnyhaibin@users.noreply.github.com> Date: Thu, 30 Mar 2023 22:57:22 -0400 Subject: [PATCH] small mpc fix (#68) * small change * use actual long engaged cereal --- selfdrive/controls/lib/lateral_planner.py | 3 ++- .../controls/lib/longitudinal_mpc_lib/long_mpc.py | 15 +++++++-------- selfdrive/controls/lib/longitudinal_planner.py | 5 +++-- selfdrive/ui/ui.h | 2 +- 4 files changed, 13 insertions(+), 12 deletions(-) diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index f197e6b38a..0c1e3ece1c 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -98,12 +98,13 @@ class LateralPlanner: else: d_path_xyz = self.path_xyz self.dynamic_lane_profile_status = True + self.path_xyz = d_path_xyz self.lat_mpc.set_weights(PATH_COST, LATERAL_MOTION_COST, LATERAL_ACCEL_COST, LATERAL_JERK_COST, STEERING_RATE_COST) - y_pts = d_path_xyz[:LAT_MPC_N+1, 1] + y_pts = self.path_xyz[:LAT_MPC_N+1, 1] heading_pts = self.plan_yaw[:LAT_MPC_N+1] yaw_rate_pts = self.plan_yaw_rate[:LAT_MPC_N+1] self.y_pts = y_pts diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 8b7a5c8ab6..76213b6fca 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -263,10 +263,10 @@ class LongitudinalMpc: def set_weights(self, prev_accel_constraint=True): if self.mode == 'acc': - cost_mulitpliers = self.get_cost_multipliers() + cost_multipliers = self.get_cost_multipliers() a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0 - cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, a_change_cost * cost_mulitpliers[0], J_EGO_COST * cost_mulitpliers[1]] - constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST * cost_mulitpliers[2]] + cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, a_change_cost * cost_multipliers[0], J_EGO_COST * cost_multipliers[1]] + constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST * cost_multipliers[2]] elif self.mode == 'blended': a_change_cost = 40.0 if prev_accel_constraint else 0 cost_weights = [0., 0.1, 0.2, 5.0, a_change_cost, 1.0] @@ -329,7 +329,7 @@ class LongitudinalMpc: else: self.desired_TF = T_FOLLOW - def update(self, carstate, radarstate, v_cruise, x, v, a, j, prev_accel_constraint): + def update(self, carstate, radarstate, v_cruise, x, v, a, j): v_ego = self.x0[1] self.status = radarstate.leadOne.status or radarstate.leadTwo.status @@ -337,7 +337,6 @@ class LongitudinalMpc: lead_xv_1 = self.process_lead(radarstate.leadTwo) self.update_TF(carstate) - self.set_weights(prev_accel_constraint) # 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 @@ -345,12 +344,12 @@ class LongitudinalMpc: 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]) - cruise_target = T_IDXS * np.clip(v_cruise, v_ego - 2.0, 1e3) + x[0] + cruise_target_e2ex = T_IDXS * np.clip(v_cruise, v_ego - 2.0, 1e3) + x[0] e2e_xforward = ((v[1:] + v[:-1]) / 2) * (T_IDXS[1:] - T_IDXS[:-1]) e2e_x = np.cumsum(np.insert(e2e_xforward, 0, x[0])) - x_and_cruise = np.column_stack([e2e_x, cruise_target]) - e2e_x = np.min(x_and_cruise, axis=1) + x_and_cruise_e2ex = np.column_stack([e2e_x, cruise_target_e2ex]) + e2e_x = np.min(x_and_cruise_e2ex, axis=1) self.params[:,0] = MIN_ACCEL self.params[:,1] = self.max_a diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 86a59f0ab3..501fd86460 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -96,7 +96,7 @@ class LongitudinalPlanner: force_slow_decel = sm['controlsState'].forceDecel # Reset current state when not engaged, or user is controlling the speed - reset_state = long_control_off if self.CP.openpilotLongitudinalControl else not sm['controlsState'].enabled + reset_state = long_control_off if self.CP.openpilotLongitudinalControl else not sm['carControl'].hudControl.speedVisible # No change cost when user is controlling the speed, or when standstill prev_accel_constraint = not (reset_state or sm['carState'].standstill) @@ -131,10 +131,11 @@ class LongitudinalPlanner: accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05, a_min_sol) accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05) + self.mpc.set_weights(prev_accel_constraint) self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error) - self.mpc.update(sm['carState'], sm['radarState'], v_cruise_sol, x, v, a, j, prev_accel_constraint) + self.mpc.update(sm['carState'], sm['radarState'], v_cruise_sol, x, v, a, j) self.v_desired_trajectory_full = np.interp(T_IDXS, T_IDXS_MPC, self.mpc.v_solution) self.a_desired_trajectory_full = np.interp(T_IDXS, T_IDXS_MPC, self.mpc.a_solution) diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 63dc30de30..eed3f92a6a 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -133,7 +133,7 @@ typedef struct UIScene { // modelV2 float lane_line_probs[4]; float road_edge_stds[2]; - QPolygonF track_vertices;; + QPolygonF track_vertices; QPolygonF track_edge_vertices; QPolygonF lane_line_vertices[4]; QPolygonF road_edge_vertices[2];