IQ.Pilot Release Commit @ f2a861c

iqpilot/selfdrive/controls/lib/longitudinal_mpc_lib/.gitignore

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

iqpilot/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript

@@ -0,0 +1,105 @@
+Import('env', 'envCython', 'arch', 'msgq_python', 'common_python', 'np_version')
+
+gen = "c_generated_code"
+
+casadi_model = [
+  f'{gen}/long_model/long_expl_ode_fun.c',
+  f'{gen}/long_model/long_expl_vde_forw.c',
+]
+
+casadi_cost_y = [
+  f'{gen}/long_cost/long_cost_y_fun.c',
+  f'{gen}/long_cost/long_cost_y_fun_jac_ut_xt.c',
+  f'{gen}/long_cost/long_cost_y_hess.c',
+]
+
+casadi_cost_e = [
+  f'{gen}/long_cost/long_cost_y_e_fun.c',
+  f'{gen}/long_cost/long_cost_y_e_fun_jac_ut_xt.c',
+  f'{gen}/long_cost/long_cost_y_e_hess.c',
+]
+
+casadi_cost_0 = [
+  f'{gen}/long_cost/long_cost_y_0_fun.c',
+  f'{gen}/long_cost/long_cost_y_0_fun_jac_ut_xt.c',
+  f'{gen}/long_cost/long_cost_y_0_hess.c',
+]
+
+casadi_constraints = [
+  f'{gen}/long_constraints/long_constr_h_fun.c',
+  f'{gen}/long_constraints/long_constr_h_fun_jac_uxt_zt.c',
+]
+
+build_files = [f'{gen}/acados_solver_long.c'] + casadi_model + casadi_cost_y + casadi_cost_e +  \
+              casadi_cost_0 + casadi_constraints
+
+# extra generated files used to trigger a rebuild
+generated_files = [
+  f'{gen}/Makefile',
+
+  f'{gen}/main_long.c',
+  f'{gen}/main_sim_long.c',
+  f'{gen}/acados_solver_long.h',
+  f'{gen}/acados_sim_solver_long.h',
+  f'{gen}/acados_sim_solver_long.c',
+  f'{gen}/acados_solver.pxd',
+
+  f'{gen}/long_model/long_expl_vde_adj.c',
+
+  f'{gen}/long_model/long_model.h',
+  f'{gen}/long_constraints/long_constraints.h',
+  f'{gen}/long_cost/long_cost.h',
+] + build_files
+
+acados_dir = '#iqpilot/third_party/acados'
+acados_templates_dir = '#iqpilot/third_party/acados/acados_template/c_templates_tera'
+
+source_list = ['long_mpc.py',
+  '#iqpilot/selfdrive/iqmodeld/config.py',
+  f'{acados_dir}/include/acados_c/ocp_nlp_interface.h',
+  f'{acados_templates_dir}/acados_solver.in.c',
+]
+
+lenv = env.Clone()
+acados_rel_path = Dir(gen).rel_path(Dir(f"#iqpilot/third_party/acados/{arch}/lib"))
+lenv["RPATH"] += [lenv.Literal(f'\\$$ORIGIN/{acados_rel_path}')]
+lenv.Clean(generated_files, Dir(gen))
+_mpc_dir = Dir('.').abspath
+generated_long = lenv.Command(generated_files,
+                              source_list,
+                              lenv.PrettyAction(f"cd {_mpc_dir} && python3 long_mpc.py", 'GEN',
+                                                logfile=f"{_mpc_dir}/gen.log", capture_stderr=True))
+lenv.Depends(generated_long, [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_long.dylib")
+  lenv["LINKFLAGS"].append(f"-Wl,-rpath,@loader_path/{acados_rel_path}")
+lib_solver = lenv.SharedLibrary(f"{gen}/acados_ocp_solver_long",
+                                build_files,
+                                LIBS=['m', 'acados', 'hpipm', 'blasfeo', 'qpOASES_e'])
+
+# generate cython stuff
+acados_ocp_solver_pyx = File("#iqpilot/third_party/acados/acados_template/acados_ocp_solver_pyx.pyx")
+acados_ocp_solver_common = File("#iqpilot/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["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],
+  lenv2.PrettyAction(
+    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()}', 'CYTHON'))
+lib_cython = lenv2.Program(f'{gen}/acados_ocp_solver_pyx.so', [libacados_ocp_solver_c], LIBS=['acados_ocp_solver_long'])
+lenv2.Depends(lib_cython, lib_solver)
+lenv2.Depends(libacados_ocp_solver_c, np_version)

iqpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -0,0 +1,433 @@
+#!/usr/bin/env python3
+import os
+import time
+import numpy as np
+from iqpilot.cereal import log
+from iqdbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
+from iqpilot.common.realtime import DT_MDL
+from iqpilot.common.swaglog import cloudlog
+# WARNING: imports outside of constants will not trigger a rebuild
+from iqpilot.selfdrive.iqmodeld.config import index_function, ModelConstants
+from iqpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU  # legacy lead extrapolation (newLeadMpc=False)
+from iqpilot.common.params import Params, UnknownKeyName
+
+LEAD_T_IDXS_MODEL = np.array(ModelConstants.LEAD_T_IDXS)  # [0, 2, 4, 6, 8, 10]s
+
+if __name__ == '__main__':  # generating code
+  from iqpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
+else:
+  from iqpilot.selfdrive.controls.lib.longitudinal_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython
+
+from casadi import SX, vertcat
+
+MODEL_NAME = 'long'
+LONG_MPC_DIR = os.path.dirname(os.path.abspath(__file__))
+EXPORT_DIR = os.path.join(LONG_MPC_DIR, "c_generated_code")
+JSON_FILE = os.path.join(LONG_MPC_DIR, "acados_ocp_long.json")
+
+LongitudinalPlanSource = log.LongitudinalPlan.LongitudinalPlanSource
+MPC_SOURCES = (LongitudinalPlanSource.lead0, LongitudinalPlanSource.lead1)
+
+X_DIM = 3
+U_DIM = 1
+PARAM_DIM = 5
+COST_E_DIM = 4
+COST_DIM = COST_E_DIM + 1
+CONSTR_DIM = 4
+
+X_EGO_OBSTACLE_COST = 3.
+X_EGO_COST = 0.
+V_EGO_COST = 0.
+A_EGO_COST = 0.
+J_EGO_COST = 20.
+DANGER_ZONE_COST = 100.
+CRASH_DISTANCE = .25
+LEAD_DANGER_FACTOR = 0.75
+LIMIT_COST = 1e6
+ACADOS_SOLVER_TYPE = 'SQP_RTI'
+
+# Fewer timestamps don't hurt performance and lead to
+# much better convergence of the MPC with low iterations
+N = 12
+MAX_T = 10.0
+T_IDXS_LST = [index_function(idx, max_val=MAX_T, max_idx=N) for idx in range(N+1)]
+
+T_IDXS = np.array(T_IDXS_LST)
+FCW_IDXS = T_IDXS < 5.0
+T_DIFFS = np.diff(T_IDXS, prepend=[0.])
+COMFORT_BRAKE = 2.5
+STOP_DISTANCE = 3.0
+MIN_X_LEAD_FACTOR = 0.5
+LEAD_PULLAWAY_VREL = 0.5
+LEAD_PULLAWAY_ABRAKE = -0.5
+
+def get_jerk_factor(personality=log.LongitudinalPersonality.standard):
+  if personality==log.LongitudinalPersonality.relaxed:
+    return 1.0
+  elif personality==log.LongitudinalPersonality.standard:
+    return 1.0
+  elif personality==log.LongitudinalPersonality.aggressive:
+    return 0.5
+  else:
+    raise NotImplementedError("Longitudinal personality not supported")
+
+
+def get_T_FOLLOW(personality=log.LongitudinalPersonality.standard):
+  if personality==log.LongitudinalPersonality.relaxed:
+    return 1.75
+  elif personality==log.LongitudinalPersonality.standard:
+    return 1.45
+  elif personality==log.LongitudinalPersonality.aggressive:
+    return 1.25
+  else:
+    raise NotImplementedError("Longitudinal personality not supported")
+
+def get_stopped_equivalence_factor(v_lead):
+  return (v_lead**2) / (2 * COMFORT_BRAKE)
+
+def get_safe_obstacle_distance(v_ego, t_follow):
+  return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + STOP_DISTANCE
+
+def gen_long_model():
+  model = AcadosModel()
+  model.name = MODEL_NAME
+
+  # states
+  x_ego, v_ego, a_ego = SX.sym('x_ego'), SX.sym('v_ego'), SX.sym('a_ego')
+  model.x = vertcat(x_ego, v_ego, a_ego)
+
+  # controls
+  j_ego = SX.sym('j_ego')
+  model.u = vertcat(j_ego)
+
+  # xdot
+  x_ego_dot = SX.sym('x_ego_dot')
+  v_ego_dot = SX.sym('v_ego_dot')
+  a_ego_dot = SX.sym('a_ego_dot')
+  model.xdot = vertcat(x_ego_dot, v_ego_dot, a_ego_dot)
+
+  # live parameters
+  a_min = SX.sym('a_min')
+  a_max = SX.sym('a_max')
+  x_obstacle = SX.sym('x_obstacle')
+  lead_t_follow = SX.sym('lead_t_follow')
+  lead_danger_factor = SX.sym('lead_danger_factor')
+  model.p = vertcat(a_min, a_max, x_obstacle, lead_t_follow, lead_danger_factor)
+
+  # dynamics model
+  f_expl = vertcat(v_ego, a_ego, j_ego)
+  model.f_impl_expr = model.xdot - f_expl
+  model.f_expl_expr = f_expl
+  return model
+
+def gen_long_ocp():
+  ocp = AcadosOcp()
+  ocp.model = gen_long_model()
+
+  Tf = T_IDXS[-1]
+
+  # set dimensions
+  ocp.dims.N = N
+
+  # set cost module
+  ocp.cost.cost_type = 'NONLINEAR_LS'
+  ocp.cost.cost_type_e = 'NONLINEAR_LS'
+
+  QR = np.zeros((COST_DIM, COST_DIM))
+  Q = np.zeros((COST_E_DIM, COST_E_DIM))
+
+  ocp.cost.W = QR
+  ocp.cost.W_e = Q
+
+  x_ego, v_ego, a_ego = ocp.model.x[0], ocp.model.x[1], ocp.model.x[2]
+  j_ego = ocp.model.u[0]
+
+  a_min, a_max = ocp.model.p[0], ocp.model.p[1]
+  x_obstacle = ocp.model.p[2]
+  lead_t_follow = ocp.model.p[3]
+  lead_danger_factor = ocp.model.p[4]
+
+  ocp.cost.yref = np.zeros((COST_DIM, ))
+  ocp.cost.yref_e = np.zeros((COST_E_DIM, ))
+
+  desired_dist_comfort = get_safe_obstacle_distance(v_ego, lead_t_follow)
+
+  # The main cost in normal operation is how close you are to the "desired" distance
+  # from an obstacle at every timestep. This obstacle can be a lead car
+  # or other object. In e2e mode we can use x_position targets as a cost
+  # instead.
+  costs = [((x_obstacle - x_ego) - (desired_dist_comfort)) / (v_ego + 10.),
+           x_ego,
+           v_ego,
+           a_ego,
+           j_ego]
+  ocp.model.cost_y_expr = vertcat(*costs)
+  ocp.model.cost_y_expr_e = vertcat(*costs[:-1])
+
+  # Constraints on speed, acceleration and desired distance to
+  # the obstacle, which is treated as a slack constraint so it
+  # behaves like an asymmetrical cost.
+  constraints = vertcat(v_ego,
+                        (a_ego - a_min),
+                        (a_max - a_ego),
+                        ((x_obstacle - x_ego) - lead_danger_factor * (desired_dist_comfort)) / (v_ego + 10.))
+  ocp.model.con_h_expr = constraints
+
+  x0 = np.zeros(X_DIM)
+  ocp.constraints.x0 = x0
+  ocp.parameter_values = np.array([-1.2, 1.2, 0.0, get_T_FOLLOW(), LEAD_DANGER_FACTOR])
+
+
+  # We put all constraint cost weights to 0 and only set them at runtime
+  cost_weights = np.zeros(CONSTR_DIM)
+  ocp.cost.zl = cost_weights
+  ocp.cost.Zl = cost_weights
+  ocp.cost.Zu = cost_weights
+  ocp.cost.zu = cost_weights
+
+  ocp.constraints.lh = np.zeros(CONSTR_DIM)
+  ocp.constraints.uh = 1e4*np.ones(CONSTR_DIM)
+  ocp.constraints.idxsh = np.arange(CONSTR_DIM)
+
+  # The HPIPM solver can give decent solutions even when it is stopped early
+  # Which is critical for our purpose where compute time is strictly bounded
+  # We use HPIPM in the SPEED_ABS mode, which ensures fastest runtime. This
+  # does not cause issues since the problem is well bounded.
+  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_cond_N = 1
+
+  # More iterations take too much time and less lead to inaccurate convergence in
+  # some situations. Ideally we would run just 1 iteration to ensure fixed runtime.
+  ocp.solver_options.qp_solver_iter_max = 10
+  ocp.solver_options.qp_tol = 1e-3
+
+  # set prediction horizon
+  ocp.solver_options.tf = Tf
+  ocp.solver_options.shooting_nodes = T_IDXS
+
+  ocp.code_export_directory = EXPORT_DIR
+  return ocp
+
+
+class LongitudinalMpc:
+  def __init__(self, dt=DT_MDL):
+    self.dt = dt
+    self._params = Params()
+    self.new_lead_mpc = self._read_new_lead_mpc()
+    self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
+    self.reset()
+    self.source = LongitudinalPlanSource.cruise
+
+  def _read_new_lead_mpc(self) -> bool:
+    try:
+      return self._params.get_bool("newLeadMpc")
+    except UnknownKeyName:
+      return True
+
+  def reset(self):
+    self.solver.reset()
+
+    self.x_sol = np.zeros((N+1, X_DIM))
+    self.u_sol = np.zeros((N, 1))
+    self.v_solution = np.zeros(N+1)
+    self.a_solution = np.zeros(N+1)
+    self.j_solution = np.zeros(N)
+    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])
+
+    self.params = np.zeros((N+1, PARAM_DIM))
+    for i in range(N+1):
+      self.solver.set(i, 'x', np.zeros(X_DIM))
+
+    self.last_cloudlog_t = 0
+    self.status = False
+    self.crash_cnt = 0.0
+    self.solution_status = 0
+    # timers
+    self.solve_time = 0.0
+    self.time_qp_solution = 0.0
+    self.time_linearization = 0.0
+    self.time_integrator = 0.0
+    self.x0 = np.zeros(X_DIM)
+    self.lead_xv_0 = np.zeros((N+1, 2))
+    self.lead_xv_1 = np.zeros((N+1, 2))
+    self.set_weights()
+
+  def set_cost_weights(self, cost_weights, constraint_cost_weights):
+    W = np.asfortranarray(np.diag(cost_weights))
+    for i in range(N):
+      self.solver.cost_set(i, 'W', W)
+    # 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(constraint_cost_weights)
+    for i in range(N):
+      self.solver.cost_set(i, 'Zl', Zl)
+
+  def set_weights(self, personality=log.LongitudinalPersonality.standard):
+    jerk_factor = get_jerk_factor(personality)
+    cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * J_EGO_COST]
+    constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST]
+    self.set_cost_weights(cost_weights, constraint_cost_weights)
+
+  def set_cur_state(self, v, a):
+    v_prev = self.x0[1]
+    self.x0[1] = v
+    self.x0[2] = a
+    if abs(v_prev - v) > 2.:  # probably only helps if v < v_prev
+      for i in range(N+1):
+        self.solver.set(i, 'x', self.x0)
+
+  @staticmethod
+  def extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau):
+    a_lead_traj = a_lead * np.exp(-a_lead_tau * (T_IDXS**2)/2.)
+    v_lead_traj = np.clip(v_lead + np.cumsum(T_DIFFS * a_lead_traj), 0.0, 1e8)
+    x_lead_traj = x_lead + np.cumsum(T_DIFFS * v_lead_traj)
+    lead_xv = np.column_stack((x_lead_traj, v_lead_traj))
+    return lead_xv
+
+  def process_lead_legacy(self, lead):
+    # behavior before PR #37824 (newLeadMpc=False): one immediate radar lead prediction
+    # extrapolated forward with acceleration decaying to 0
+    v_ego = self.x0[1]
+    if lead is not None and lead.status:
+      x_lead = lead.dRel
+      v_lead = lead.vLead
+      a_lead = lead.aLeadK
+      a_lead_tau = lead.aLeadTau
+    else:
+      # Fake a fast lead car, so mpc can keep running in the same mode
+      x_lead = 50.0
+      v_lead = v_ego + 10.0
+      a_lead = 0.0
+      a_lead_tau = _LEAD_ACCEL_TAU
+
+    # MPC will not converge if immediate crash is expected
+    # Clip lead distance to what is still possible to brake for
+    min_x_lead = MIN_X_LEAD_FACTOR * (v_ego + v_lead) * (v_ego - v_lead) / (-ACCEL_MIN * 2)
+    x_lead = np.clip(x_lead, min_x_lead, 1e8)
+    v_lead = np.clip(v_lead, 0.0, 1e8)
+    a_lead = np.clip(a_lead, -10., 5.)
+    lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau)
+    return lead_xv
+
+  def process_lead(self, model_lead, radar_lead):
+    v_ego = self.x0[1]
+    try:
+      x_model = np.asarray(model_lead.x, dtype=np.float64)
+      v_model = np.asarray(model_lead.v, dtype=np.float64)
+      valid_model_lead = (float(model_lead.prob) > 0.5 and radar_lead.status and float(radar_lead.modelProb) > 0.5 and
+                          x_model.shape == LEAD_T_IDXS_MODEL.shape and v_model.shape == LEAD_T_IDXS_MODEL.shape and
+                          np.all(np.isfinite(x_model)) and np.all(np.isfinite(v_model)))
+    except (AttributeError, TypeError, ValueError):
+      valid_model_lead = False
+
+    if not valid_model_lead:
+      return self.process_lead_legacy(radar_lead)
+
+    x_lead_traj = float(radar_lead.dRel) + (x_model - x_model[0])
+    v_lead_traj = float(radar_lead.vLead) + (v_model - v_model[0])
+
+    # MPC won't converge on immediate crashes; lift h=0 to the minimum braking distance.
+    v_lead_0 = v_lead_traj[0]
+    min_x_lead = MIN_X_LEAD_FACTOR * (v_ego + v_lead_0) * (v_ego - v_lead_0) / (-ACCEL_MIN * 2)
+    x_lead_traj[0] = max(x_lead_traj[0], min_x_lead)
+    v_lead_traj = np.clip(v_lead_traj, 0.0, 1e8)
+
+    x_lead_mpc = np.maximum.accumulate(np.interp(T_IDXS, LEAD_T_IDXS_MODEL, x_lead_traj))
+    v_lead_mpc = np.interp(T_IDXS, LEAD_T_IDXS_MODEL, v_lead_traj)
+    if radar_lead.status and radar_lead.vRel > LEAD_PULLAWAY_VREL and radar_lead.aLeadK > LEAD_PULLAWAY_ABRAKE:
+      # ty spysyweeb for lead pull away fix phantom launch braking so you don't ram the lead in edge cases.
+      radar_velocity_floor = np.full_like(T_IDXS, float(radar_lead.vLead))
+      radar_distance_floor = float(radar_lead.dRel) + float(radar_lead.vLead) * T_IDXS
+      v_lead_mpc = np.maximum(v_lead_mpc, radar_velocity_floor)
+      x_lead_mpc = np.maximum(x_lead_mpc, radar_distance_floor)
+    return np.column_stack((x_lead_mpc, v_lead_mpc))
+
+  def update(self, modelV2, radarstate, personality=log.LongitudinalPersonality.standard):
+    self.new_lead_mpc = self._read_new_lead_mpc()
+    t_follow = get_T_FOLLOW(personality)
+    model_leads = modelV2.leadsV3
+
+    if self.new_lead_mpc:
+      self.status = radarstate.leadOne.status or radarstate.leadTwo.status
+      model_lead_0 = model_leads[0] if len(model_leads) > 0 else None
+      model_lead_1 = model_leads[1] if len(model_leads) > 1 else None
+      lead_xv_0 = self.process_lead(model_lead_0, radarstate.leadOne)
+      lead_xv_1 = self.process_lead(model_lead_1, radarstate.leadTwo)
+    else:
+      # pre-PR behavior: radar lead extrapolated with accel decay
+      self.status = radarstate.leadOne.status or radarstate.leadTwo.status
+      lead_xv_0 = self.process_lead_legacy(radarstate.leadOne)
+      lead_xv_1 = self.process_lead_legacy(radarstate.leadTwo)
+    self.lead_xv_0 = lead_xv_0
+    self.lead_xv_1 = lead_xv_1
+
+    # To estimate a safe distance from a moving lead, we calculate how much stopping
+    # distance that lead needs as a minimum. We can add that to the current distance
+    # and then treat that as a stopped car/obstacle at this new distance.
+    lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1])
+    lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1])
+
+    x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle])
+    self.source = MPC_SOURCES[np.argmin(x_obstacles[0])]
+
+    self.yref[:,:] = 0.0
+    for i in range(N):
+      self.solver.set(i, "yref", self.yref[i])
+    self.solver.set(N, "yref", self.yref[N][:COST_E_DIM])
+
+    self.params[:,0] = ACCEL_MIN
+    self.params[:,1] = ACCEL_MAX
+    self.params[:,2] = np.min(x_obstacles, axis=1)
+    self.params[:,3] = t_follow
+    self.params[:,4] = LEAD_DANGER_FACTOR
+
+    self.run()
+    lead_crash_prob = radarstate.leadOne.modelProb
+    if (np.any(lead_xv_0[FCW_IDXS,0] - self.x_sol[FCW_IDXS,0] < CRASH_DISTANCE) and
+            lead_crash_prob > 0.9):
+      self.crash_cnt += 1
+    else:
+      self.crash_cnt = 0
+
+  def run(self):
+    for i in range(N+1):
+      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.solve_time = float(self.solver.get_stats('time_tot')[0])
+    self.time_qp_solution = float(self.solver.get_stats('time_qp')[0])
+    self.time_linearization = float(self.solver.get_stats('time_lin')[0])
+    self.time_integrator = float(self.solver.get_stats('time_sim')[0])
+
+    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]
+    self.j_solution = self.u_sol[:,0]
+
+    t = time.monotonic()
+    if self.solution_status != 0:
+      if t > self.last_cloudlog_t + 5.0:
+        self.last_cloudlog_t = t
+        cloudlog.warning(f"Long mpc reset, solution_status: {self.solution_status}")
+      self.reset()
+
+
+if __name__ == "__main__":
+  ocp = gen_long_ocp()
+  AcadosOcpSolver.generate(ocp, json_file=JSON_FILE)