From 6868cfc287ccda2b34c2327a39431da24578dfb1 Mon Sep 17 00:00:00 2001 From: Harald Schafer Date: Thu, 7 Oct 2021 14:00:47 -0700 Subject: [PATCH 01/15] e2e long models --- models/supercombo.dlc | 4 ++-- models/supercombo.onnx | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/models/supercombo.dlc b/models/supercombo.dlc index 2ebf4fa828c01b..ca6230ef82b976 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:a481bf939e9d115922b97d23b85889e35456492b8a5199a319b347cb0292f56e +size 56685051 diff --git a/models/supercombo.onnx b/models/supercombo.onnx index 17d233dad7bc5c..3461cf2c440e0e 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:ed5e09684ee9c436b9df4820bf0a9ccb6b9aad0bdb9a2a4adc525a2f24088479 +size 56707084 From c05ad6357f979b184df9e3fa4350154d73ec8940 Mon Sep 17 00:00:00 2001 From: Harald Schafer Date: Thu, 7 Oct 2021 14:10:05 -0700 Subject: [PATCH 02/15] try e2e --- .../controls/lib/longitudinal_planner.py | 24 ++++++++++++------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 41bae4c47518b8..4ebcc4e908f0d0 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -44,7 +44,7 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): class Planner: def __init__(self, CP, init_v=0.0, init_a=0.0): self.CP = CP - self.mpc = LongitudinalMpc() + self.mpc = LongitudinalMpc(e2e=True) self.fcw = False @@ -87,17 +87,25 @@ 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: + x = np.array(sm['modelV2'].position.x) + v = np.array(sm['modelV2'].velocity.x) + a = 0.0*np.array(sm['modelV2'].position.x) + else: + x = np.zeros(33) + v = np.zeros(33) + a = np.zeros(33) + self.mpc.update_with_xva(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 +126,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) From 9cdf3de09528f91389b65f88c1657b9473fbab3a Mon Sep 17 00:00:00 2001 From: Harald Schafer Date: Fri, 8 Oct 2021 13:50:28 -0700 Subject: [PATCH 03/15] respect lead and cruise as limits --- .../lib/longitudinal_mpc_lib/long_mpc.py | 15 ++++++++++---- .../controls/lib/longitudinal_planner.py | 20 ++++++++++--------- 2 files changed, 22 insertions(+), 13 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 3a8234706fc34a..490ca6a72308b6 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -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 @@ -229,8 +230,9 @@ 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])) + W = np.diag([0., 10., 1., 10., .1, 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. @@ -301,13 +303,18 @@ def set_accel_limits(self, min_a, max_a): def update(self, carstate, radarstate, v_cruise, prev_accel_constraint=False): v_ego = self.x0[1] a_ego = self.x0[2] + self.yref[:,1] = x + self.yref[:,2] = v + self.yref[:,3] = a + self.solver.cost_set_slice(0, N, "yref", self.yref[:N], api='old') + self.solver.set(N, "yref", self.yref[N][:COST_E_DIM]) 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 @@ -323,7 +330,7 @@ def update(self, carstate, radarstate, v_cruise, prev_accel_constraint=False): 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) + cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + (3/4) * 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])] diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 4ebcc4e908f0d0..6a372ca827483d 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -44,7 +44,7 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): class Planner: def __init__(self, CP, init_v=0.0, init_a=0.0): self.CP = CP - self.mpc = LongitudinalMpc(e2e=True) + self.mpc = LongitudinalMpc() self.fcw = False @@ -89,15 +89,17 @@ def update(self, sm): accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05) self.mpc.set_accel_limits(-3.5, 2.) self.mpc.set_cur_state(self.v_desired, self.a_desired) - if len(sm['modelV2'].position.x) == 33 and len(sm['modelV2'].velocity.x) == 33: - x = np.array(sm['modelV2'].position.x) - v = np.array(sm['modelV2'].velocity.x) - a = 0.0*np.array(sm['modelV2'].position.x) + 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(33) - v = np.zeros(33) - a = np.zeros(33) - self.mpc.update_with_xva(x, v, a) + 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) From b70617615b07365e7464e14e12de6c471f0a9e49 Mon Sep 17 00:00:00 2001 From: Harald Schafer Date: Mon, 11 Oct 2021 17:18:42 -0700 Subject: [PATCH 04/15] add lead and cruise limits --- .../lib/longitudinal_mpc_lib/long_mpc.py | 31 ++++++++++--------- 1 file changed, 16 insertions(+), 15 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 490ca6a72308b6..8f2d80914b1a77 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 @@ -323,22 +323,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) + (3/4) * 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_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] = 1e8 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 From b56bdc9d3e5ae3c61017c2b6510a1330557b684b Mon Sep 17 00:00:00 2001 From: Harald Schafer Date: Mon, 11 Oct 2021 20:04:24 -0700 Subject: [PATCH 05/15] need to go --- selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 8f2d80914b1a77..9a7c1169709534 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -323,11 +323,11 @@ 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]) - cruise_target = T_IDXS * v_cruise + 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), + 1e4 + lead_0_obstacle - (3/4) * get_safe_obstacle_distance(v), + 1e4 + lead_1_obstacle - (3/4) * get_safe_obstacle_distance(v), cruise_target]) #self.source = SOURCES[np.argmin(x_obstacles[0])] self.params[:,2] = 1e8 From deefba3df6f201cfd927fdd5da86094cfc3b8f83 Mon Sep 17 00:00:00 2001 From: Harald Schafer Date: Mon, 11 Oct 2021 20:21:39 -0700 Subject: [PATCH 06/15] this should work... --- models/supercombo.dlc | 2 +- models/supercombo.onnx | 2 +- selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/models/supercombo.dlc b/models/supercombo.dlc index ca6230ef82b976..cdd40eccbeeaa0 100644 --- a/models/supercombo.dlc +++ b/models/supercombo.dlc @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:a481bf939e9d115922b97d23b85889e35456492b8a5199a319b347cb0292f56e +oid sha256:9c3728be9462b1849236f94bdbadbfe65135bc15f40b35797739fdd4b8a09631 size 56685051 diff --git a/models/supercombo.onnx b/models/supercombo.onnx index 3461cf2c440e0e..63f13f9ca96753 100644 --- a/models/supercombo.onnx +++ b/models/supercombo.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:ed5e09684ee9c436b9df4820bf0a9ccb6b9aad0bdb9a2a4adc525a2f24088479 +oid sha256:dc8e9af9c4fe01d8ebf517a88896d23f37b329bc4fdaa2e45fa85f0e43f5f839 size 56707084 diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 9a7c1169709534..c48a7feeba320d 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -328,9 +328,9 @@ def update(self, carstate, radarstate, v_cruise, prev_accel_constraint=False): x_targets = np.column_stack([x, 1e4 + lead_0_obstacle - (3/4) * get_safe_obstacle_distance(v), 1e4 + lead_1_obstacle - (3/4) * get_safe_obstacle_distance(v), - cruise_target]) + 1e4 + cruise_target]) #self.source = SOURCES[np.argmin(x_obstacles[0])] - self.params[:,2] = 1e8 + self.params[:,2] = 1e4 if prev_accel_constraint: self.params[:, 3] = np.copy(self.prev_a) else: From 88c85cf3f00690715ddeb8547e83df6c44bc74e7 Mon Sep 17 00:00:00 2001 From: Harald Schafer Date: Mon, 11 Oct 2021 20:34:44 -0700 Subject: [PATCH 07/15] add cruise back --- 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 c48a7feeba320d..0e78d098d508da 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -328,7 +328,7 @@ def update(self, carstate, radarstate, v_cruise, prev_accel_constraint=False): x_targets = np.column_stack([x, 1e4 + lead_0_obstacle - (3/4) * get_safe_obstacle_distance(v), 1e4 + lead_1_obstacle - (3/4) * get_safe_obstacle_distance(v), - 1e4 + cruise_target]) + cruise_target]) #self.source = SOURCES[np.argmin(x_obstacles[0])] self.params[:,2] = 1e4 if prev_accel_constraint: From a70327889e54796ebcfeb60e5b083295380a876a Mon Sep 17 00:00:00 2001 From: Harald Schafer Date: Mon, 11 Oct 2021 20:40:11 -0700 Subject: [PATCH 08/15] fixup --- selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 0e78d098d508da..610b2843f91cd4 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -326,11 +326,11 @@ def update(self, carstate, radarstate, v_cruise, prev_accel_constraint=False): cruise_target = T_IDXS * v_cruise + x[0] x_targets = np.column_stack([x, - 1e4 + lead_0_obstacle - (3/4) * get_safe_obstacle_distance(v), - 1e4 + lead_1_obstacle - (3/4) * get_safe_obstacle_distance(v), + 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] = 1e4 + self.params[:,2] = 1e3 if prev_accel_constraint: self.params[:, 3] = np.copy(self.prev_a) else: From cf7ec030ba847b9411b85bb0e5e8a5d9ffeb0f34 Mon Sep 17 00:00:00 2001 From: Harald Schafer Date: Tue, 26 Oct 2021 12:21:11 -0700 Subject: [PATCH 09/15] latest and greatest --- models/supercombo.dlc | 2 +- models/supercombo.onnx | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/models/supercombo.dlc b/models/supercombo.dlc index cdd40eccbeeaa0..14c08c65aaea2f 100644 --- a/models/supercombo.dlc +++ b/models/supercombo.dlc @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:9c3728be9462b1849236f94bdbadbfe65135bc15f40b35797739fdd4b8a09631 +oid sha256:8c67b841b61f78507512746307da3ca14e3a825fe9715e30c1ceab6304fcf6bd size 56685051 diff --git a/models/supercombo.onnx b/models/supercombo.onnx index 63f13f9ca96753..69d9c5fd2f7ff0 100644 --- a/models/supercombo.onnx +++ b/models/supercombo.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:dc8e9af9c4fe01d8ebf517a88896d23f37b329bc4fdaa2e45fa85f0e43f5f839 +oid sha256:67cbb904b13bc0b1472dfa8fa2a57475e21f05a42c249ab4d461cab705026280 size 56707084 From 9b2bef1bb83e0a07c30d02141fcaa9f1c9dfb5b1 Mon Sep 17 00:00:00 2001 From: Harald Schafer Date: Tue, 26 Oct 2021 13:34:18 -0700 Subject: [PATCH 10/15] add accel --- selfdrive/modeld/models/driving.cc | 5 +++++ 1 file changed, 5 insertions(+) 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); } From b013eff1f2d38a0223dd656983ab00ea875667f3 Mon Sep 17 00:00:00 2001 From: Harald Schafer Date: Wed, 27 Oct 2021 17:33:47 -0700 Subject: [PATCH 11/15] chiiiill --- 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 610b2843f91cd4..a48db2c409ecf2 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -244,7 +244,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., 0.0, 1.])) + W = np.diag([0., 0.1, .5, 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, @@ -301,7 +301,7 @@ 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 From 461eec3d9416862d8247d7ae3d39b8a4fc3b90f1 Mon Sep 17 00:00:00 2001 From: Harald Schafer Date: Wed, 27 Oct 2021 19:04:07 -0700 Subject: [PATCH 12/15] chiiiiill round 2 --- selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index a48db2c409ecf2..fdf1f2a8f0ae76 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -229,8 +229,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, A_CHANGE_COST, J_EGO_COST])) - W = np.diag([0., 10., 1., 10., .1, 1.]) + W = np.diag([0., .01, .0, 10., .1, 1.]) for i in range(N): 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) @@ -244,7 +243,7 @@ def set_weights_for_lead_policy(self): self.solver.cost_set(i, 'Zl', Zl) def set_weights_for_xva_policy(self): - W = np.diag([0., 0.1, .5, 1., 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, From 3f29c23d7ecbaeaa9fc8c597d00b4a4437b12bd6 Mon Sep 17 00:00:00 2001 From: Harald Schafer Date: Thu, 28 Oct 2021 10:04:27 -0700 Subject: [PATCH 13/15] more x weights --- 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 fdf1f2a8f0ae76..9d2cc9cb8b3bd2 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -229,7 +229,7 @@ def set_weights(self): self.set_weights_for_lead_policy() def set_weights_for_lead_policy(self): - W = np.diag([0., .01, .0, 10., .1, 1.]) + W = np.diag([0., .03, .0, 10., 0.0, 1.]) for i in range(N): 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) From 874fc336410404f9baac887be048ba747c8429ae Mon Sep 17 00:00:00 2001 From: Harald Schafer Date: Tue, 23 Nov 2021 11:21:56 -0800 Subject: [PATCH 14/15] latest model --- models/supercombo.dlc | 2 +- models/supercombo.onnx | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/models/supercombo.dlc b/models/supercombo.dlc index 14c08c65aaea2f..063dbb2681a756 100644 --- a/models/supercombo.dlc +++ b/models/supercombo.dlc @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:8c67b841b61f78507512746307da3ca14e3a825fe9715e30c1ceab6304fcf6bd +oid sha256:cd3e47f88e7c3b58fa25912c35680b56ba12c4cb4adc1ec33d962688d82a3f28 size 56685051 diff --git a/models/supercombo.onnx b/models/supercombo.onnx index 69d9c5fd2f7ff0..4f067360d2da11 100644 --- a/models/supercombo.onnx +++ b/models/supercombo.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:67cbb904b13bc0b1472dfa8fa2a57475e21f05a42c249ab4d461cab705026280 -size 56707084 +oid sha256:d28a836c0e7c5c594b7648d54b0f244797b05abb120523632a7f3d6922c16b52 +size 57554673 From 868f271c5c94c686e2dc375b693991ca8e0f4907 Mon Sep 17 00:00:00 2001 From: Harald Schafer Date: Tue, 23 Nov 2021 14:07:35 -0800 Subject: [PATCH 15/15] acados has been changed --- selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 6 ------ 1 file changed, 6 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 9d2cc9cb8b3bd2..681ed917598595 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -233,8 +233,6 @@ def set_weights_for_lead_policy(self): for i in range(N): 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 @@ -246,8 +244,6 @@ def set_weights_for_xva_policy(self): 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 @@ -305,8 +301,6 @@ def update(self, carstate, radarstate, v_cruise, prev_accel_constraint=False): self.yref[:,1] = x self.yref[:,2] = v self.yref[:,3] = a - self.solver.cost_set_slice(0, N, "yref", self.yref[:N], api='old') - self.solver.set(N, "yref", self.yref[N][:COST_E_DIM]) self.status = radarstate.leadOne.status or radarstate.leadTwo.status lead_xv_0 = self.process_lead(radarstate.leadOne)