From dc16307917870085831b65b9e3c9ab6d71fc7d8c Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Mon, 21 Feb 2022 16:14:41 +0100 Subject: [PATCH] longitudinal planner: disable change cost when stopped. not engaged or gas pressed (#23639) * disable change cost completely on standstill and gas press * cleanup * set accel to zero * clean up logic around standstill * update ref old-commit-hash: 428d412c1f5480c527b296ed85185de34f5082c2 --- .../lib/longitudinal_mpc_lib/long_mpc.py | 19 ++++++++---------- .../controls/lib/longitudinal_planner.py | 20 ++++++++++++------- .../test/longitudinal_maneuvers/plant.py | 3 +-- selfdrive/test/process_replay/ref_commit | 2 +- 4 files changed, 23 insertions(+), 21 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 8625df745a..f9f9c31bb4 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -219,19 +219,20 @@ class LongitudinalMpc: self.x0 = np.zeros(X_DIM) self.set_weights() - def set_weights(self): + def set_weights(self, prev_accel_constraint=True): if self.e2e: self.set_weights_for_xva_policy() self.params[:,0] = -10. self.params[:,1] = 10. self.params[:,2] = 1e5 else: - self.set_weights_for_lead_policy() + self.set_weights_for_lead_policy(prev_accel_constraint) - 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])) + def set_weights_for_lead_policy(self, prev_accel_constraint=True): + a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0 + W = np.asfortranarray(np.diag([X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, a_change_cost, J_EGO_COST])) 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] = a_change_cost * 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. @@ -300,9 +301,8 @@ class LongitudinalMpc: self.cruise_min_a = min_a self.cruise_max_a = max_a - def update(self, carstate, radarstate, v_cruise, prev_accel_constraint=False): + def update(self, carstate, radarstate, v_cruise): v_ego = self.x0[1] - a_ego = self.x0[2] self.status = radarstate.leadOne.status or radarstate.leadTwo.status lead_xv_0 = self.process_lead(radarstate.leadOne) @@ -330,10 +330,7 @@ class LongitudinalMpc: 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) - if prev_accel_constraint: - self.params[:,3] = np.copy(self.prev_a) - else: - self.params[:,3] = a_ego + self.params[:,3] = np.copy(self.prev_a) 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 8c79404058..865442f735 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -58,7 +58,6 @@ class Planner: def update(self, sm): v_ego = sm['carState'].vEgo - a_ego = sm['carState'].aEgo v_cruise_kph = sm['controlsState'].vCruise v_cruise_kph = min(v_cruise_kph, V_CRUISE_MAX) @@ -67,12 +66,16 @@ class Planner: long_control_state = sm['controlsState'].longControlState force_slow_decel = sm['controlsState'].forceDecel - prev_accel_constraint = True - if long_control_state == LongCtrlState.off or sm['carState'].gasPressed: + # Reset current state when not engaged, or user is controlling the speed + reset_state = long_control_state == LongCtrlState.off + reset_state = reset_state or sm['carState'].gasPressed + + # No change cost when user is controlling the speed, or when standstill + prev_accel_constraint = not (reset_state or sm['carState'].standstill) + + if reset_state: self.v_desired_filter.x = v_ego - self.a_desired = a_ego - # Smoothly changing between accel trajectory is only relevant when OP is driving - prev_accel_constraint = False + self.a_desired = 0.0 # Prevent divergence, smooth in current v_ego self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego)) @@ -86,9 +89,12 @@ class Planner: # 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_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) - self.mpc.update(sm['carState'], sm['radarState'], v_cruise, prev_accel_constraint=prev_accel_constraint) + 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) self.j_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC[:-1], self.mpc.j_solution) diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index beb544b1b1..13025a9f03 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -86,11 +86,10 @@ class Plant(): radar.radarState.leadOne = lead radar.radarState.leadTwo = lead - control.controlsState.longControlState = LongCtrlState.pid control.controlsState.vCruise = float(v_cruise * 3.6) car_state.carState.vEgo = float(self.speed) - + car_state.carState.standstill = self.speed < 0.01 # ******** get controlsState messages for plotting *** sm = {'radarState': radar.radarState, diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 41c6c2aa59..d730a50ea7 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -24c4d6b8d49b42416f2ca6a59d563f7b8c984e2b \ No newline at end of file +60f58f58dae514eb8b956c03afa5f9b91c12a122 \ No newline at end of file