      self.solver.constraints_set(0, "ubx", x0_cp)
      self.yref[:,0] = y_pts
      v_ego = p_cp[0]
      # rotation_radius = p_cp[1]
      self.yref[:,1] = heading_pts * (v_ego+5.0)
      self.yref[:,2] = yaw_rate_pts * (v_ego+5.0)
      for i in range(N):
        self.solver.cost_set(i, "yref", self.yref[i])
        self.solver.set(i, "p", p_cp)
      self.solver.set(N, "p", p_cp)
      self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM])
 
</s> remove     # clip speed for curv calculation at 1m/s, to prevent low speed extremes
    clipped_speed = max(1.0, sm['carState'].vEgo)
    lateralPlan.curvatures = (self.lat_mpc.x_sol[0:CONTROL_N, 3]/clipped_speed).tolist()
    lateralPlan.curvatureRates = [float(x/clipped_speed) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0]
 </s> add     lateralPlan.curvatures = (self.lat_mpc.x_sol[0:CONTROL_N, 3]/self.v_ego).tolist()
    lateralPlan.curvatureRates = [float(x/self.v_ego) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0] </s> remove                                   ((v_ego + 5.0) * psi_ego),
                                  ((v_ego + 5.0) * psi_rate_ego))
 </s> add                                   ((v_ego + SPEED_OFFSET) * psi_ego),
                                  ((v_ego + SPEED_OFFSET) * psi_rate_ego))
    self.yref[:,1] = heading_pts * (v_ego + SPEED_OFFSET)
    self.yref[:,2] = yaw_rate_pts * (v_ego + SPEED_OFFSET)
      self.solver.constraints_set(0, "ubx", x0_cp)
      self.yref[:,0] = y_pts
      v_ego = p_cp[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)
    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)
      self.solver.set(N, "p", p_cp)
      self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM])
keep keep keep keep replace replace keep keep keep keep keep
 Increase low speed jerk cost (#26008)

* Increase low speed jerk cost

* Update planner weight

* Update ref_commit

* Update lateral_planner.py

* cleanup and refactor

* Update ref_commit 
selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py