mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-09-30 03:13:48 +08:00
Code Issues Packages Projects Releases Wiki Activity

long mpc: state name before subscript (#37101)

Browse Source
This commit is contained in:
felsager
2026-02-06 14:26:20 -08:00
committed by GitHub
parent 593c3a0c8e
commit 12597856da
1 changed files with 7 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+7 -7
View File
@@ -108,10 +108,10 @@ def gen_long_model():
a_min = SX.sym('a_min')
a_max = SX.sym('a_max')
x_obstacle = SX.sym('x_obstacle')
prev_a = SX.sym('prev_a')
a_prev = SX.sym('a_prev')
lead_t_follow = SX.sym('lead_t_follow')
lead_danger_factor = SX.sym('lead_danger_factor')
model.p = vertcat(a_min, a_max, x_obstacle, prev_a, lead_t_follow, lead_danger_factor)
model.p = vertcat(a_min, a_max, x_obstacle, a_prev, lead_t_follow, lead_danger_factor)
# dynamics model
f_expl = vertcat(v_ego, a_ego, j_ego)
@@ -143,7 +143,7 @@ def gen_long_ocp():
a_min, a_max = ocp.model.p[0], ocp.model.p[1]
x_obstacle = ocp.model.p[2]
prev_a = ocp.model.p[3]
a_prev = ocp.model.p[3]
lead_t_follow = ocp.model.p[4]
lead_danger_factor = ocp.model.p[5]
@@ -160,7 +160,7 @@ def gen_long_ocp():
x_ego,
v_ego,
a_ego,
a_ego - prev_a,
a_ego - a_prev,
j_ego]
ocp.model.cost_y_expr = vertcat(*costs)
ocp.model.cost_y_expr_e = vertcat(*costs[:-1])
@@ -228,7 +228,7 @@ class LongitudinalMpc:
self.v_solution = np.zeros(N+1)
self.a_solution = np.zeros(N+1)
self.j_solution = np.zeros(N)
self.prev_a = np.array(self.a_solution)
self.a_prev = np.array(self.a_solution)
self.yref = np.zeros((N+1, COST_DIM))
for i in range(N):
@@ -346,7 +346,7 @@ class LongitudinalMpc:
self.params[:,0] = ACCEL_MIN
self.params[:,1] = ACCEL_MAX
self.params[:,2] = np.min(x_obstacles, axis=1)
self.params[:,3] = np.copy(self.prev_a)
self.params[:,3] = np.copy(self.a_prev)
self.params[:,4] = t_follow
self.params[:,5] = LEAD_DANGER_FACTOR
@@ -378,7 +378,7 @@ class LongitudinalMpc:
self.a_solution = self.x_sol[:,2]
self.j_solution = self.u_sol[:,0]
self.prev_a = np.interp(T_IDXS + self.dt, T_IDXS, self.a_solution)
self.a_prev = np.interp(T_IDXS + self.dt, T_IDXS, self.a_solution)
t = time.monotonic()
if self.solution_status != 0: