mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-07-17 06:22:06 +08:00
Remove lateral MPC library (#38281)
This commit is contained in:
Adeeb Shihadeh
@@ -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',
|
||||
|
||||
@@ -1,2 +0,0 @@
|
||||
acados_ocp_lat.json
|
||||
c_generated_code/
|
||||
@@ -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)
|
||||
@@ -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)
|
||||
@@ -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)
|
||||
Reference in New Issue
Block a user