openpilot v0.9.6 release

date: 2024-02-21T23:02:42
master commit: 0b4d08fab8e35a264bc7383e878538f8083c33e5

selfdrive/controls/lib/lateral_mpc_lib/.gitignore

@@ -0,0 +1,2 @@
+acados_ocp_lat.json
+c_generated_code/

selfdrive/controls/lib/lateral_mpc_lib/SConscript

@@ -0,0 +1,89 @@
+Import('env', 'envCython', 'arch')
+
+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_dir = '#third_party/acados'
+acados_templates_dir = '#third_party/acados/acados_template/c_templates_tera'
+
+source_list = ['lat_mpc.py',
+  '#selfdrive/modeld/constants.py',
+  f'{acados_dir}/include/acados_c/ocp_nlp_interface.h',
+  f'{acados_templates_dir}/acados_solver.in.c',
+]
+
+lenv = env.Clone()
+lenv.Clean(generated_files, Dir(gen))
+
+generated_lat = lenv.Command(generated_files,
+                             source_list,
+                             f"cd {Dir('.').abspath} && python3 lat_mpc.py")
+
+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")
+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 = File("#third_party/acados/acados_template/acados_ocp_solver_pyx.pyx")
+acados_ocp_solver_common = File("#third_party/acados/acados_template/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["LINKFLAGS"] += [lib_solver[0].get_labspath()]
+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])
+lenv2.Depends(lib_cython, lib_solver)

selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py

@@ -0,0 +1,199 @@
+#!/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 openpilot.third_party.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)