From abf2a69513f09caa4da107e2f6b1e22e277adcc1 Mon Sep 17 00:00:00 2001 From: Harald Schafer Date: Mon, 15 Nov 2021 19:07:44 -0800 Subject: [PATCH 1/9] add plan changing cost --- .../lib/longitudinal_mpc_lib/long_mpc.py | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index df6d8e0fb64316..678670f3457236 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -24,15 +24,17 @@ X_DIM = 3 U_DIM = 1 -COST_E_DIM = 4 +PARAM_DIM= 4 +COST_E_DIM = 5 COST_DIM = COST_E_DIM + 1 CONSTR_DIM = 4 X_EGO_OBSTACLE_COST = 3. -V_EGO_COST = 0. X_EGO_COST = 0. +V_EGO_COST = 0. A_EGO_COST = 0. J_EGO_COST = 10. +A_CHANGE_COST = 5. DANGER_ZONE_COST = 100. CRASH_DISTANCE = .5 LIMIT_COST = 1e6 @@ -118,6 +120,7 @@ def gen_long_mpc_solver(): a_min, a_max = ocp.model.p[0], ocp.model.p[1] x_obstacle = ocp.model.p[2] + prev_a = ocp.model.p[3] ocp.cost.yref = np.zeros((COST_DIM, )) ocp.cost.yref_e = np.zeros((COST_E_DIM, )) @@ -132,6 +135,7 @@ def gen_long_mpc_solver(): x_ego, v_ego, a_ego, + a_ego - prev_a, j_ego] ocp.model.cost_y_expr = vertcat(*costs) ocp.model.cost_y_expr_e = vertcat(*costs[:-1]) @@ -198,6 +202,7 @@ def reset(self): self.solver = AcadosOcpSolverFast('long', N, EXPORT_DIR) self.v_solution = [0.0 for i in range(N+1)] self.a_solution = [0.0 for i in range(N+1)] + self.prev_a = self.a_solution self.j_solution = [0.0 for i in range(N)] self.yref = np.zeros((N+1, COST_DIM)) for i in range(N): @@ -205,7 +210,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, PARAM_DIM)) for i in range(N+1): self.solver.set(i, 'x', np.zeros(X_DIM)) self.last_cloudlog_t = 0 @@ -320,6 +325,7 @@ def update(self, carstate, radarstate, v_cruise): 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.prev_a self.run() if (np.any(lead_xv_0[:,0] - self.x_sol[:,0] < CRASH_DISTANCE) and @@ -339,7 +345,8 @@ def update_with_xva(self, x, v, a): self.accel_limit_arr[:,1] = 10. x_obstacle = 1e5*np.ones((N+1)) self.params = np.concatenate([self.accel_limit_arr, - x_obstacle[:,None]], axis=1) + x_obstacle[:,None], + self.prev_a], axis=1) self.run() @@ -358,6 +365,8 @@ def run(self): self.a_solution = self.x_sol[:,2] self.j_solution = self.u_sol[:,0] + self.prev_a = self.a_solution + t = sec_since_boot() if self.solution_status != 0: if t > self.last_cloudlog_t + 5.0: From 060abe2e2c12bc2721f2efd3e98b42302e5b2be9 Mon Sep 17 00:00:00 2001 From: Harald Schafer Date: Mon, 15 Nov 2021 19:43:53 -0800 Subject: [PATCH 2/9] fix compile --- selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 678670f3457236..297f3f391aab81 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -87,7 +87,8 @@ def gen_long_model(): x_obstacle = SX.sym('x_obstacle') a_min = SX.sym('a_min') a_max = SX.sym('a_max') - model.p = vertcat(a_min, a_max, x_obstacle) + prev_a = SX.sym('prev_a') + model.p = vertcat(a_min, a_max, x_obstacle, prev_a) # dynamics model f_expl = vertcat(v_ego, a_ego, j_ego) @@ -152,7 +153,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, 0.0]) # We put all constraint cost weights to 0 and only set them at runtime cost_weights = np.zeros(CONSTR_DIM) From ef838e368337fdfa0bf69876e619c2336556f4f9 Mon Sep 17 00:00:00 2001 From: Harald Schafer Date: Mon, 15 Nov 2021 20:23:06 -0800 Subject: [PATCH 3/9] set weights --- selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 297f3f391aab81..e8cde4028b571a 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -228,7 +228,7 @@ 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])) + 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): self.solver.cost_set(i, 'W', W) # Setting the slice without the copy make the array not contiguous, @@ -241,7 +241,7 @@ 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., 1.])) + W = np.asfortranarray(np.diag([0., 10., 1., 10., 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, From 3cfbdd760bda1d28dcef2c8e02496a5a8afb78c9 Mon Sep 17 00:00:00 2001 From: Harald Schafer Date: Tue, 16 Nov 2021 10:50:54 -0800 Subject: [PATCH 4/9] try this cost --- selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index e8cde4028b571a..9021ae3263411f 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -33,8 +33,8 @@ X_EGO_COST = 0. V_EGO_COST = 0. A_EGO_COST = 0. -J_EGO_COST = 10. -A_CHANGE_COST = 5. +J_EGO_COST = 5. +A_CHANGE_COST = .5 DANGER_ZONE_COST = 100. CRASH_DISTANCE = .5 LIMIT_COST = 1e6 @@ -136,7 +136,7 @@ def gen_long_mpc_solver(): x_ego, v_ego, a_ego, - a_ego - prev_a, + 20*(a_ego - prev_a), j_ego] ocp.model.cost_y_expr = vertcat(*costs) ocp.model.cost_y_expr_e = vertcat(*costs[:-1]) @@ -326,7 +326,7 @@ def update(self, carstate, radarstate, v_cruise): 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.prev_a + self.params[:,3] = np.copy(self.prev_a) self.run() if (np.any(lead_xv_0[:,0] - self.x_sol[:,0] < CRASH_DISTANCE) and From 70fbe5f5b206a6e9ab197c23eb14985abce67fe0 Mon Sep 17 00:00:00 2001 From: Harald Schafer Date: Tue, 16 Nov 2021 14:17:23 -0800 Subject: [PATCH 5/9] horizon problem --- selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 9021ae3263411f..33a1f2cdc8502b 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -230,6 +230,7 @@ def set_weights(self): 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])) 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]) self.solver.cost_set(i, 'W', W) # Setting the slice without the copy make the array not contiguous, # causing issues with the C interface. @@ -366,7 +367,7 @@ def run(self): self.a_solution = self.x_sol[:,2] self.j_solution = self.u_sol[:,0] - self.prev_a = self.a_solution + self.prev_a = interp(T_IDXS + 0.05, T_IDXS, self.a_solution) t = sec_since_boot() if self.solution_status != 0: From 475d17e79092dbe6b3fce9e52c0a571b59654ba8 Mon Sep 17 00:00:00 2001 From: Harald Schafer Date: Tue, 16 Nov 2021 14:43:43 -0800 Subject: [PATCH 6/9] looks pretty good --- selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 33a1f2cdc8502b..577651f8d47262 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -33,7 +33,7 @@ X_EGO_COST = 0. V_EGO_COST = 0. A_EGO_COST = 0. -J_EGO_COST = 5. +J_EGO_COST = 5.0 A_CHANGE_COST = .5 DANGER_ZONE_COST = 100. CRASH_DISTANCE = .5 From 6540e8c5a765975fd292b1efdef97b2d6391fa9c Mon Sep 17 00:00:00 2001 From: Harald Schafer Date: Tue, 16 Nov 2021 15:22:00 -0800 Subject: [PATCH 7/9] update refs --- selfdrive/test/process_replay/ref_commit | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 0c2cdc6a551400..adfbdac52f26b1 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -8b929f005a01a4207e218510512ae72689dd2565 \ No newline at end of file +475d17e79092dbe6b3fce9e52c0a571b59654ba8 \ No newline at end of file From 89aa3d2be771e850ad7f8e676ef2c806c64bb6ea Mon Sep 17 00:00:00 2001 From: Harald Schafer Date: Tue, 16 Nov 2021 15:48:34 -0800 Subject: [PATCH 8/9] update refs --- selfdrive/test/process_replay/ref_commit | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index adfbdac52f26b1..5027b0c1798917 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -475d17e79092dbe6b3fce9e52c0a571b59654ba8 \ No newline at end of file +6540e8c5a765975fd292b1efdef97b2d6391fa9c \ No newline at end of file From 59193eec9c2dd3f78faea76bf901386b5aa77b07 Mon Sep 17 00:00:00 2001 From: Harald Schafer Date: Tue, 16 Nov 2021 17:29:42 -0800 Subject: [PATCH 9/9] smoother plan changes --- selfdrive/test/longitudinal_maneuvers/test_longitudinal.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py index 4267f3c9e134d7..35b9f32e68e86a 100755 --- a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py +++ b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py @@ -62,7 +62,7 @@ speed_lead_values=[20., 20., 0.], prob_lead_values=[0., 1., 1.], cruise_values=[20., 20., 20.], - breakpoints=[2., 2.01, 8.51], + breakpoints=[2., 2.01, 8.8], ), Maneuver( "approach stopped car at 20m/s",