From 9f817e5d8ab01b28eb0eac5f0e594f6c76ded775 Mon Sep 17 00:00:00 2001 From: Joost Wooning Date: Wed, 10 Nov 2021 16:44:08 +0100 Subject: [PATCH] try without slices --- .../controls/lib/lateral_mpc_lib/lat_mpc.py | 16 ++++++--- .../lib/longitudinal_mpc_lib/long_mpc.py | 35 +++++++++++-------- 2 files changed, 31 insertions(+), 20 deletions(-) diff --git a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py index 24c1189ad6..f42c4c5ab9 100755 --- a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py +++ b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py @@ -9,6 +9,7 @@ from selfdrive.controls.lib.drive_helpers import T_IDXS if __name__ == '__main__': # generating code from pyextra.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver else: + # from pyextra.acados_template import AcadosOcpSolverFast from selfdrive.controls.lib.lateral_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverFast # pylint: disable=no-name-in-module, import-error LAT_MPC_DIR = os.path.dirname(os.path.abspath(__file__)) @@ -120,7 +121,8 @@ class LateralMpc(): self.x_sol = np.zeros((N+1, X_DIM)) self.u_sol = np.zeros((N, 1)) self.yref = np.zeros((N+1, 3)) - self.solver.cost_set_slice(0, N, "yref", self.yref[:N]) + for i in range(N): + self.solver.cost_set(i, "yref", self.yref[i]) self.solver.cost_set(N, "yref", self.yref[N][:2]) W = np.eye(3) self.Ws = np.tile(W[None], reps=(N,1,1)) @@ -138,7 +140,8 @@ class LateralMpc(): self.Ws[:,0,0] = path_weight self.Ws[:,1,1] = heading_weight self.Ws[:,2,2] = steer_rate_weight - self.solver.cost_set_slice(0, N, 'W', self.Ws, api='old') + for i in range(N): + self.solver.cost_set(i, 'W', self.Ws[i], api='new') #TODO hacky weights to keep behavior the same self.solver.cost_set(N, 'W', (3/20.)*self.Ws[0,:2,:2]) @@ -148,12 +151,15 @@ class LateralMpc(): self.solver.constraints_set(0, "ubx", x0_cp) self.yref[:,0] = y_pts self.yref[:,1] = heading_pts*(v_ego+5.0) - self.solver.cost_set_slice(0, N, "yref", self.yref[:N]) + for i in range(N): + self.solver.cost_set(i, "yref", self.yref[i]) self.solver.cost_set(N, "yref", self.yref[N][:2]) self.solution_status = self.solver.solve() - self.solver.fill_in_slice(0, N+1, 'x', self.x_sol) - self.solver.fill_in_slice(0, N, 'u', self.u_sol) + for i in range(N+1): + self.x_sol[i] = self.solver.get(i, 'x') + for i in range(N): + self.u_sol[i] = self.solver.get(i, 'u') self.cost = self.solver.get_cost() diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index e909245332..b73c38c4e6 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -11,6 +11,7 @@ from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU if __name__ == '__main__': # generating code from pyextra.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver else: + # from pyextra.acados_template import AcadosOcpSolver as AcadosOcpSolverFast from selfdrive.controls.lib.longitudinal_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverFast # pylint: disable=no-name-in-module, import-error from casadi import SX, vertcat @@ -199,8 +200,9 @@ class LongitudinalMpc(): self.a_solution = [0.0 for i in range(N+1)] self.j_solution = [0.0 for i in range(N)] self.yref = np.zeros((N+1, COST_DIM)) - self.solver.cost_set_slice(0, N, "yref", self.yref[:N]) - self.solver.set(N, "yref", self.yref[N][:COST_E_DIM]) + for i in range(N): + self.solver.cost_set(i, "yref", self.yref[i]) + 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)) @@ -221,29 +223,29 @@ class LongitudinalMpc(): def set_weights_for_lead_policy(self): W = np.diag([X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, J_EGO_COST]) - Ws = np.tile(W[None], reps=(N,1,1)) - self.solver.cost_set_slice(0, N, 'W', Ws, api='old') + for i in range(N): + self.solver.cost_set(i, 'W', W, api='new') # 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 Zl = np.array([LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST]) - Zls = np.tile(Zl[None], reps=(N+1,1,1)) - self.solver.cost_set_slice(0, N+1, 'Zl', Zls, api='old') + for i in range(N): + self.solver.cost_set(i, 'Zl', Zl, api='new') def set_weights_for_xva_policy(self): W = np.diag([0., 10., 1., 10., 1.]) - Ws = np.tile(W[None], reps=(N,1,1)) - self.solver.cost_set_slice(0, N, 'W', Ws, api='old') + for i in range(N): + self.solver.cost_set(i, 'W', W, api='new') # 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 Zl = np.array([LIMIT_COST, LIMIT_COST, LIMIT_COST, 0.0]) - Zls = np.tile(Zl[None], reps=(N+1,1,1)) - self.solver.cost_set_slice(0, N+1, 'Zl', Zls, api='old') + for i in range(N): + self.solver.cost_set(i, 'Zl', Zl, api='new') def set_cur_state(self, v, a): if abs(self.x0[1] - v) > 1.: @@ -330,8 +332,9 @@ class LongitudinalMpc(): 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]) + for i in range(N): + self.solver.cost_set(i, "yref", self.yref[i], api='new') + self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM]) self.accel_limit_arr[:,0] = -10. self.accel_limit_arr[:,1] = 10. x_obstacle = 1e5*np.ones((N+1)) @@ -342,12 +345,14 @@ class LongitudinalMpc(): def run(self): for i in range(N+1): - self.solver.set_param(i, self.params[i]) + self.solver.set(i, 'p', self.params[i]) self.solver.constraints_set(0, "lbx", self.x0) self.solver.constraints_set(0, "ubx", self.x0) self.solution_status = self.solver.solve() - self.solver.fill_in_slice(0, N+1, 'x', self.x_sol) - self.solver.fill_in_slice(0, N, 'u', self.u_sol) + for i in range(N+1): + self.x_sol[i] = self.solver.get(i, 'x') + for i in range(N): + self.u_sol[i] = self.solver.get(i, 'u') self.v_solution = self.x_sol[:,1] self.a_solution = self.x_sol[:,2]