|
|
@ -203,7 +203,7 @@ class LongitudinalMpc(): |
|
|
|
self.solver = AcadosOcpSolverFast('long', N, EXPORT_DIR) |
|
|
|
self.solver = AcadosOcpSolverFast('long', N, EXPORT_DIR) |
|
|
|
self.v_solution = [0.0 for i in range(N+1)] |
|
|
|
self.v_solution = [0.0 for i in range(N+1)] |
|
|
|
self.a_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.prev_a = np.array(self.a_solution) |
|
|
|
self.j_solution = [0.0 for i in range(N)] |
|
|
|
self.j_solution = [0.0 for i in range(N)] |
|
|
|
self.yref = np.zeros((N+1, COST_DIM)) |
|
|
|
self.yref = np.zeros((N+1, COST_DIM)) |
|
|
|
for i in range(N): |
|
|
|
for i in range(N): |
|
|
@ -371,7 +371,7 @@ class LongitudinalMpc(): |
|
|
|
self.a_solution = self.x_sol[:,2] |
|
|
|
self.a_solution = self.x_sol[:,2] |
|
|
|
self.j_solution = self.u_sol[:,0] |
|
|
|
self.j_solution = self.u_sol[:,0] |
|
|
|
|
|
|
|
|
|
|
|
self.prev_a = interp(T_IDXS + 0.05, T_IDXS, self.a_solution) |
|
|
|
self.prev_a = np.interp(T_IDXS + 0.05, T_IDXS, self.a_solution) |
|
|
|
|
|
|
|
|
|
|
|
t = sec_since_boot() |
|
|
|
t = sec_since_boot() |
|
|
|
if self.solution_status != 0: |
|
|
|
if self.solution_status != 0: |
|
|
|