diff --git a/SConstruct b/SConstruct index 031984bf4c..b5e2d04c22 100644 --- a/SConstruct +++ b/SConstruct @@ -247,7 +247,6 @@ if arch == "larch64": # Build selfdrive SConscript([ 'openpilot/selfdrive/pandad/SConscript', - 'openpilot/selfdrive/controls/lib/lateral_mpc_lib/SConscript', 'openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript', 'openpilot/selfdrive/locationd/SConscript', 'openpilot/selfdrive/modeld/SConscript', diff --git a/openpilot/selfdrive/controls/lib/lateral_mpc_lib/.gitignore b/openpilot/selfdrive/controls/lib/lateral_mpc_lib/.gitignore deleted file mode 100644 index aff2eb082b..0000000000 --- a/openpilot/selfdrive/controls/lib/lateral_mpc_lib/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -acados_ocp_lat.json -c_generated_code/ diff --git a/openpilot/selfdrive/controls/lib/lateral_mpc_lib/SConscript b/openpilot/selfdrive/controls/lib/lateral_mpc_lib/SConscript deleted file mode 100644 index 575e5c1af5..0000000000 --- a/openpilot/selfdrive/controls/lib/lateral_mpc_lib/SConscript +++ /dev/null @@ -1,104 +0,0 @@ -Import('env', 'envCython', 'arch', 'msgq_python', 'common_python', 'np_version', 'acados') - -gen = "c_generated_code" - -casadi_model = [ - f'{gen}/lat_model/lat_expl_ode_fun.c', - f'{gen}/lat_model/lat_expl_vde_forw.c', -] - -casadi_cost_y = [ - f'{gen}/lat_cost/lat_cost_y_fun.c', - f'{gen}/lat_cost/lat_cost_y_fun_jac_ut_xt.c', - f'{gen}/lat_cost/lat_cost_y_hess.c', -] - -casadi_cost_e = [ - f'{gen}/lat_cost/lat_cost_y_e_fun.c', - f'{gen}/lat_cost/lat_cost_y_e_fun_jac_ut_xt.c', - f'{gen}/lat_cost/lat_cost_y_e_hess.c', -] - -casadi_cost_0 = [ - f'{gen}/lat_cost/lat_cost_y_0_fun.c', - f'{gen}/lat_cost/lat_cost_y_0_fun_jac_ut_xt.c', - f'{gen}/lat_cost/lat_cost_y_0_hess.c', -] - -build_files = [f'{gen}/acados_solver_lat.c'] + casadi_model + casadi_cost_y + casadi_cost_e + casadi_cost_0 - -# extra generated files used to trigger a rebuild -generated_files = [ - f'{gen}/Makefile', - - f'{gen}/main_lat.c', - f'{gen}/main_sim_lat.c', - f'{gen}/acados_solver_lat.h', - f'{gen}/acados_sim_solver_lat.h', - f'{gen}/acados_sim_solver_lat.c', - f'{gen}/acados_solver.pxd', - - f'{gen}/lat_model/lat_expl_vde_adj.c', - - f'{gen}/lat_model/lat_model.h', - f'{gen}/lat_constraints/lat_constraints.h', - f'{gen}/lat_cost/lat_cost.h', -] + build_files - -acados_include_dir = Dir(acados.INCLUDE_DIR) -acados_template_dir = Dir(acados.TEMPLATE_DIR) - -source_list = ['lat_mpc.py', - '#openpilot/selfdrive/modeld/constants.py', - acados_include_dir.File('acados_c/ocp_nlp_interface.h'), - acados_template_dir.File('c_templates_tera/acados_solver.in.c'), -] - -lenv = env.Clone() -copied_acados_libs = [] -if arch != "Darwin": - for lib in ["libacados.so", "libblasfeo.so", "libhpipm.so", "libqpOASES_e.so.3.1"]: - copied_acados_libs += lenv.Command(f"{gen}/{lib}", Dir(acados.LIB_DIR).File(lib), [Mkdir(Dir(gen)), Copy("$TARGET", "$SOURCE")]) - lenv["RPATH"] += [lenv.Literal('\\$$ORIGIN')] -else: - acados_rel_path = Dir(gen).rel_path(Dir(acados.LIB_DIR)) - lenv["RPATH"] += [lenv.Literal(f'\\$$ORIGIN/{acados_rel_path}')] -lenv.Clean(generated_files, Dir(gen)) - -generated_lat = lenv.Command(generated_files, - source_list, - f"cd {Dir('.').abspath} && python3 lat_mpc.py") -lenv.Depends(generated_lat, [msgq_python, common_python]) - -lenv["CFLAGS"].append("-DACADOS_WITH_QPOASES") -lenv["CXXFLAGS"].append("-DACADOS_WITH_QPOASES") -lenv["CCFLAGS"].append("-Wno-unused") -if arch != "Darwin": - lenv["LINKFLAGS"].append("-Wl,--disable-new-dtags") -else: - lenv["LINKFLAGS"].append("-Wl,-install_name,@loader_path/libacados_ocp_solver_lat.dylib") - lenv["LINKFLAGS"].append(f"-Wl,-rpath,@loader_path/{acados_rel_path}") -lib_solver = lenv.SharedLibrary(f"{gen}/acados_ocp_solver_lat", - build_files, - LIBS=['m', 'acados', 'hpipm', 'blasfeo', 'qpOASES_e']) - -# generate cython stuff -acados_ocp_solver_pyx = acados_template_dir.File('acados_ocp_solver_pyx.pyx') -acados_ocp_solver_common = acados_template_dir.File('acados_solver_common.pxd') -libacados_ocp_solver_pxd = File(f'{gen}/acados_solver.pxd') -libacados_ocp_solver_c = File(f'{gen}/acados_ocp_solver_pyx.c') - -lenv2 = envCython.Clone() -lenv2["LIBPATH"] += [lib_solver[0].dir.abspath] -lenv2["RPATH"] += [lenv2.Literal('\\$$ORIGIN')] -lenv2.Command(libacados_ocp_solver_c, - [acados_ocp_solver_pyx, acados_ocp_solver_common, libacados_ocp_solver_pxd], - f'cython' + \ - f' -o {libacados_ocp_solver_c.get_labspath()}' + \ - f' -I {libacados_ocp_solver_pxd.get_dir().get_labspath()}' + \ - f' -I {acados_ocp_solver_common.get_dir().get_labspath()}' + \ - f' {acados_ocp_solver_pyx.get_labspath()}') -lib_cython = lenv2.Program(f'{gen}/acados_ocp_solver_pyx.so', [libacados_ocp_solver_c], LIBS=['acados_ocp_solver_lat']) -lenv2.Depends(lib_cython, lib_solver) -lenv2.Depends(lib_cython, copied_acados_libs) -lenv2.Depends(libacados_ocp_solver_c, np_version) diff --git a/openpilot/selfdrive/controls/lib/lateral_mpc_lib/__init__.py b/openpilot/selfdrive/controls/lib/lateral_mpc_lib/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/openpilot/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py b/openpilot/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py deleted file mode 100755 index 3b597b2e42..0000000000 --- a/openpilot/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py +++ /dev/null @@ -1,199 +0,0 @@ -#!/usr/bin/env python3 -import os -import time -import numpy as np - -from casadi import SX, vertcat, sin, cos -# WARNING: imports outside of constants will not trigger a rebuild -from openpilot.selfdrive.modeld.constants import ModelConstants - -if __name__ == '__main__': # generating code - from acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver -else: - from openpilot.selfdrive.controls.lib.lateral_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython - -LAT_MPC_DIR = os.path.dirname(os.path.abspath(__file__)) -EXPORT_DIR = os.path.join(LAT_MPC_DIR, "c_generated_code") -JSON_FILE = os.path.join(LAT_MPC_DIR, "acados_ocp_lat.json") -X_DIM = 4 -P_DIM = 2 -COST_E_DIM = 3 -COST_DIM = COST_E_DIM + 2 -SPEED_OFFSET = 10.0 -MODEL_NAME = 'lat' -ACADOS_SOLVER_TYPE = 'SQP_RTI' -N = 32 - -def gen_lat_model(): - model = AcadosModel() - model.name = MODEL_NAME - - # set up states & controls - x_ego = SX.sym('x_ego') - y_ego = SX.sym('y_ego') - psi_ego = SX.sym('psi_ego') - psi_rate_ego = SX.sym('psi_rate_ego') - model.x = vertcat(x_ego, y_ego, psi_ego, psi_rate_ego) - - # parameters - v_ego = SX.sym('v_ego') - rotation_radius = SX.sym('rotation_radius') - model.p = vertcat(v_ego, rotation_radius) - - # controls - psi_accel_ego = SX.sym('psi_accel_ego') - model.u = vertcat(psi_accel_ego) - - # xdot - x_ego_dot = SX.sym('x_ego_dot') - y_ego_dot = SX.sym('y_ego_dot') - psi_ego_dot = SX.sym('psi_ego_dot') - psi_rate_ego_dot = SX.sym('psi_rate_ego_dot') - - model.xdot = vertcat(x_ego_dot, y_ego_dot, psi_ego_dot, psi_rate_ego_dot) - - # dynamics model - f_expl = vertcat(v_ego * cos(psi_ego) - rotation_radius * sin(psi_ego) * psi_rate_ego, - v_ego * sin(psi_ego) + rotation_radius * cos(psi_ego) * psi_rate_ego, - psi_rate_ego, - psi_accel_ego) - model.f_impl_expr = model.xdot - f_expl - model.f_expl_expr = f_expl - return model - - -def gen_lat_ocp(): - ocp = AcadosOcp() - ocp.model = gen_lat_model() - - Tf = np.array(ModelConstants.T_IDXS)[N] - - # set dimensions - ocp.dims.N = N - - # set cost module - ocp.cost.cost_type = 'NONLINEAR_LS' - ocp.cost.cost_type_e = 'NONLINEAR_LS' - - Q = np.diag(np.zeros(COST_E_DIM)) - QR = np.diag(np.zeros(COST_DIM)) - - ocp.cost.W = QR - ocp.cost.W_e = Q - - y_ego, psi_ego, psi_rate_ego = ocp.model.x[1], ocp.model.x[2], ocp.model.x[3] - psi_rate_ego_dot = ocp.model.u[0] - v_ego = ocp.model.p[0] - - ocp.parameter_values = np.zeros((P_DIM, )) - - ocp.cost.yref = np.zeros((COST_DIM, )) - ocp.cost.yref_e = np.zeros((COST_E_DIM, )) - # Add offset to smooth out low speed control - # TODO unclear if this right solution long term - v_ego_offset = v_ego + SPEED_OFFSET - # TODO there are two costs on psi_rate_ego_dot, one - # is correlated to jerk the other to steering wheel movement - # the steering wheel movement cost is added to prevent excessive - # wheel movements - ocp.model.cost_y_expr = vertcat(y_ego, - v_ego_offset * psi_ego, - v_ego_offset * psi_rate_ego, - v_ego_offset * psi_rate_ego_dot, - psi_rate_ego_dot / (v_ego + 0.1)) - ocp.model.cost_y_expr_e = vertcat(y_ego, - v_ego_offset * psi_ego, - v_ego_offset * psi_rate_ego) - - # set constraints - ocp.constraints.constr_type = 'BGH' - ocp.constraints.idxbx = np.array([2,3]) - ocp.constraints.ubx = np.array([np.radians(90), np.radians(50)]) - ocp.constraints.lbx = np.array([-np.radians(90), -np.radians(50)]) - x0 = np.zeros((X_DIM,)) - ocp.constraints.x0 = x0 - - ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' - ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' - ocp.solver_options.integrator_type = 'ERK' - ocp.solver_options.nlp_solver_type = ACADOS_SOLVER_TYPE - ocp.solver_options.qp_solver_iter_max = 1 - ocp.solver_options.qp_solver_cond_N = 1 - - # set prediction horizon - ocp.solver_options.tf = Tf - ocp.solver_options.shooting_nodes = np.array(ModelConstants.T_IDXS)[:N+1] - - ocp.code_export_directory = EXPORT_DIR - return ocp - - -class LateralMpc: - def __init__(self, x0=None): - if x0 is None: - x0 = np.zeros(X_DIM) - self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N) - self.reset(x0) - - def reset(self, x0=None): - if x0 is None: - x0 = np.zeros(X_DIM) - self.x_sol = np.zeros((N+1, X_DIM)) - self.u_sol = np.zeros((N, 1)) - self.yref = np.zeros((N+1, COST_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]) - - # Somehow needed for stable init - for i in range(N+1): - self.solver.set(i, 'x', np.zeros(X_DIM)) - self.solver.set(i, 'p', np.zeros(P_DIM)) - self.solver.constraints_set(0, "lbx", x0) - self.solver.constraints_set(0, "ubx", x0) - self.solver.solve() - self.solution_status = 0 - self.solve_time = 0.0 - self.cost = 0 - - def set_weights(self, path_weight, heading_weight, - lat_accel_weight, lat_jerk_weight, - steering_rate_weight): - W = np.asfortranarray(np.diag([path_weight, heading_weight, - lat_accel_weight, lat_jerk_weight, - steering_rate_weight])) - for i in range(N): - self.solver.cost_set(i, 'W', W) - self.solver.cost_set(N, 'W', W[:COST_E_DIM,:COST_E_DIM]) - - def run(self, x0, p, y_pts, heading_pts, yaw_rate_pts): - x0_cp = np.copy(x0) - p_cp = np.copy(p) - self.solver.constraints_set(0, "lbx", x0_cp) - self.solver.constraints_set(0, "ubx", x0_cp) - self.yref[:,0] = y_pts - v_ego = p_cp[0, 0] - # rotation_radius = p_cp[1] - self.yref[:,1] = heading_pts * (v_ego + SPEED_OFFSET) - self.yref[:,2] = yaw_rate_pts * (v_ego + SPEED_OFFSET) - for i in range(N): - self.solver.cost_set(i, "yref", self.yref[i]) - self.solver.set(i, "p", p_cp[i]) - self.solver.set(N, "p", p_cp[N]) - self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM]) - - t = time.monotonic() - self.solution_status = self.solver.solve() - self.solve_time = time.monotonic() - t - - 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() - - -if __name__ == "__main__": - ocp = gen_lat_ocp() - AcadosOcpSolver.generate(ocp, json_file=JSON_FILE) - # AcadosOcpSolver.build(ocp.code_export_directory, with_cython=True) diff --git a/openpilot/selfdrive/controls/tests/test_lateral_mpc.py b/openpilot/selfdrive/controls/tests/test_lateral_mpc.py deleted file mode 100644 index 3aa0fd1bce..0000000000 --- a/openpilot/selfdrive/controls/tests/test_lateral_mpc.py +++ /dev/null @@ -1,85 +0,0 @@ -import pytest -import numpy as np -from openpilot.selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc -from openpilot.selfdrive.controls.lib.drive_helpers import CAR_ROTATION_RADIUS -from openpilot.selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import N as LAT_MPC_N - - -def run_mpc(lat_mpc=None, v_ref=30., x_init=0., y_init=0., psi_init=0., curvature_init=0., - lane_width=3.6, poly_shift=0.): - - if lat_mpc is None: - lat_mpc = LateralMpc() - lat_mpc.set_weights(1., .1, 0.0, .05, 800) - - y_pts = poly_shift * np.ones(LAT_MPC_N + 1) - heading_pts = np.zeros(LAT_MPC_N + 1) - curv_rate_pts = np.zeros(LAT_MPC_N + 1) - - x0 = np.array([x_init, y_init, psi_init, curvature_init]) - p = np.column_stack([v_ref * np.ones(LAT_MPC_N + 1), - CAR_ROTATION_RADIUS * np.ones(LAT_MPC_N + 1)]) - - # converge in no more than 10 iterations - for _ in range(10): - lat_mpc.run(x0, p, - y_pts, heading_pts, curv_rate_pts) - return lat_mpc.x_sol - - -class TestLateralMpc: - - def _assert_null(self, sol, curvature=1e-6): - for i in range(len(sol)): - assert sol[0,i,1] == pytest.approx(0, abs=curvature) - assert sol[0,i,2] == pytest.approx(0, abs=curvature) - assert sol[0,i,3] == pytest.approx(0, abs=curvature) - - def _assert_simmetry(self, sol, curvature=1e-6): - for i in range(len(sol)): - assert sol[0,i,1] == pytest.approx(-sol[1,i,1], abs=curvature) - assert sol[0,i,2] == pytest.approx(-sol[1,i,2], abs=curvature) - assert sol[0,i,3] == pytest.approx(-sol[1,i,3], abs=curvature) - assert sol[0,i,0] == pytest.approx(sol[1,i,0], abs=curvature) - - def test_straight(self): - sol = run_mpc() - self._assert_null(np.array([sol])) - - def test_y_symmetry(self): - sol = [] - for y_init in [-0.5, 0.5]: - sol.append(run_mpc(y_init=y_init)) - self._assert_simmetry(np.array(sol)) - - def test_poly_symmetry(self): - sol = [] - for poly_shift in [-1., 1.]: - sol.append(run_mpc(poly_shift=poly_shift)) - self._assert_simmetry(np.array(sol)) - - def test_curvature_symmetry(self): - sol = [] - for curvature_init in [-0.1, 0.1]: - sol.append(run_mpc(curvature_init=curvature_init)) - self._assert_simmetry(np.array(sol)) - - def test_psi_symmetry(self): - sol = [] - for psi_init in [-0.1, 0.1]: - sol.append(run_mpc(psi_init=psi_init)) - self._assert_simmetry(np.array(sol)) - - def test_no_overshoot(self): - y_init = 1. - sol = run_mpc(y_init=y_init) - for y in list(sol[:,1]): - assert y_init >= abs(y) - - def test_switch_convergence(self): - lat_mpc = LateralMpc() - sol = run_mpc(lat_mpc=lat_mpc, poly_shift=3.0, v_ref=7.0) - right_psi_deg = np.degrees(sol[:,2]) - sol = run_mpc(lat_mpc=lat_mpc, poly_shift=-3.0, v_ref=7.0) - left_psi_deg = np.degrees(sol[:,2]) - np.testing.assert_almost_equal(right_psi_deg, -left_psi_deg, decimal=3)