class LongitudinalMpc:
def __init__(self, e2e=False):
self.e2e = e2e
self.reset()
self.source = SOURCES[2]
def reset(self):
self.solver = AcadosOcpSolverFast('long', N)
self.v_solution = np.zeros(N + 1)
self.a_solution = np.zeros(N + 1)
self.prev_a = np.array(self.a_solution)
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.x_sol = np.zeros((N + 1, X_DIM))
self.u_sol = np.zeros((N, 1))
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.set_weights()
def set_weights(self, prev_accel_constraint=True):
if self.e2e:
self.set_weights_for_xva_policy()
self.params[:, 0] = -10.
self.params[:, 1] = 10.
self.params[:, 2] = 1e5
else:
self.set_weights_for_lead_policy(prev_accel_constraint)
def set_weights_for_lead_policy(self, prev_accel_constraint=True):
a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0
W = np.asfortranarray(
np.diag([
X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST,
a_change_cost, J_EGO_COST
]))
for i in range(N):
# reduce the cost on (a-a_prev) later in the horizon.
W[4, 4] = a_change_cost * np.interp(T_IDXS[i], [0.0, 1.0, 2.0],
[1.0, 1.0, 0.0])
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([LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST])
for i in range(N):
self.solver.cost_set(i, 'Zl', Zl)
def set_weights_for_xva_policy(self):
W = np.asfortranarray(np.diag([0., 10., 1., 10., 0.0, 1.]))
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([LIMIT_COST, LIMIT_COST, LIMIT_COST, 0.0])
for i in range(N):
self.solver.cost_set(i, 'Zl', Zl)
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(0, 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(self, lead):
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 = (
(v_ego + v_lead) / 2) * (v_ego - v_lead) / (-MIN_ACCEL * 2)
x_lead = clip(x_lead, min_x_lead, 1e8)
v_lead = clip(v_lead, 0.0, 1e8)
a_lead = clip(a_lead, -10., 5.)
lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau)
return lead_xv
def set_accel_limits(self, min_a, max_a):
self.cruise_min_a = min_a
self.cruise_max_a = max_a
def update(self, carstate, radarstate, v_cruise):
v_ego = self.x0[1]
self.status = radarstate.leadOne.status or radarstate.leadTwo.status
lead_xv_0 = self.process_lead(radarstate.leadOne)
lead_xv_1 = self.process_lead(radarstate.leadTwo)
# set accel limits in params
self.params[:, 0] = interp(float(self.status), [0.0, 1.0],
[self.cruise_min_a, MIN_ACCEL])
self.params[:, 1] = self.cruise_max_a
# 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])
# Fake an obstacle for cruise, this ensures smooth acceleration to set speed
# when the leads are no factor.
v_lower = v_ego + (T_IDXS * self.cruise_min_a * 1.05)
v_upper = v_ego + (T_IDXS * self.cruise_max_a * 1.05)
v_cruise_clipped = np.clip(v_cruise * np.ones(N + 1), v_lower, v_upper)
cruise_obstacle = np.cumsum(
T_DIFFS *
v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped)
x_obstacles = np.column_stack(
[lead_0_obstacle, lead_1_obstacle, cruise_obstacle])
self.source = SOURCES[np.argmin(x_obstacles[0])]
self.params[:, 2] = np.min(x_obstacles, axis=1)
self.params[:, 3] = np.copy(self.prev_a)
self.run()
if (np.any(lead_xv_0[:, 0] - self.x_sol[:, 0] < CRASH_DISTANCE)
and radarstate.leadOne.modelProb > 0.9):
self.crash_cnt += 1
else:
self.crash_cnt = 0
def update_with_xva(self, x, v, a):
self.yref[:, 1] = x
self.yref[:, 2] = v
self.yref[:, 3] = a
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[:, 3] = np.copy(self.prev_a)
self.run()
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])
# qp_iter = self.solver.get_stats('statistics')[-1][-1] # SQP_RTI specific
# print(f"long_mpc timings: tot {self.solve_time:.2e}, qp {self.time_qp_solution:.2e}, lin {self.time_linearization:.2e}, integrator {self.time_integrator:.2e}, qp_iter {qp_iter}")
# res = self.solver.get_residuals()
# print(f"long_mpc residuals: {res[0]:.2e}, {res[1]:.2e}, {res[2]:.2e}, {res[3]:.2e}")
# self.solver.print_statistics()
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]
self.prev_a = np.interp(T_IDXS + 0.05, T_IDXS, self.a_solution)
t = sec_since_boot()
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()
class LongitudinalMpc():
def __init__(self, e2e=False):
self.e2e = e2e
self.reset()
self.accel_limit_arr = np.zeros((N + 1, 2))
self.accel_limit_arr[:, 0] = -1.2
self.accel_limit_arr[:, 1] = 1.2
self.source = SOURCES[2]
def reset(self):
self.solver = AcadosOcpSolverFast('long', N, EXPORT_DIR)
self.v_solution = [0.0 for i in range(N + 1)]
self.a_solution = [0.0 for i in range(N + 1)]
self.j_solution = [0.0 for i in range(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.x_sol = np.zeros((N + 1, X_DIM))
self.u_sol = np.zeros((N, 1))
self.params = np.zeros((N + 1, 3))
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
self.x0 = np.zeros(X_DIM)
self.set_weights()
def set_weights(self):
if self.e2e:
self.set_weights_for_xva_policy()
else:
self.set_weights_for_lead_policy()
def set_weights_for_lead_policy(self):
W = np.asfortranarray(
np.diag([
X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST,
J_EGO_COST
]))
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([LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST])
for i in range(N):
self.solver.cost_set(i, 'Zl', Zl)
def set_weights_for_xva_policy(self):
W = np.asfortranarray(np.diag([0., 10., 1., 10., 1.]))
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([LIMIT_COST, LIMIT_COST, LIMIT_COST, 0.0])
for i in range(N):
self.solver.cost_set(i, 'Zl', Zl)
def set_cur_state(self, v, a):
if abs(self.x0[1] - v) > 1.:
self.x0[1] = v
self.x0[2] = a
for i in range(0, N + 1):
self.solver.set(i, 'x', self.x0)
else:
self.x0[1] = v
self.x0[2] = a
def extrapolate_lead(self, 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(self, lead):
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 = (
(v_ego + v_lead) / 2) * (v_ego - v_lead) / (-MIN_ACCEL * 2)
x_lead = clip(x_lead, min_x_lead, 1e8)
v_lead = clip(v_lead, 0.0, 1e8)
a_lead = clip(a_lead, -10., 5.)
lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau)
return lead_xv
def set_accel_limits(self, min_a, max_a):
self.cruise_min_a = min_a
self.cruise_max_a = max_a
def update(self, carstate, radarstate, v_cruise):
v_ego = self.x0[1]
self.status = radarstate.leadOne.status or radarstate.leadTwo.status
lead_xv_0 = self.process_lead(radarstate.leadOne)
lead_xv_1 = self.process_lead(radarstate.leadTwo)
# set accel limits in params
self.params[:, 0] = interp(float(self.status), [0.0, 1.0],
[self.cruise_min_a, MIN_ACCEL])
self.params[:, 1] = self.cruise_max_a
# 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])
# Fake an obstacle for cruise, this ensures smooth acceleration to set speed
# when the leads are no factor.
cruise_lower_bound = v_ego + (3 / 4) * self.cruise_min_a * T_IDXS
cruise_upper_bound = v_ego + (3 / 4) * self.cruise_max_a * T_IDXS
v_cruise_clipped = np.clip(v_cruise * np.ones(N + 1),
cruise_lower_bound, cruise_upper_bound)
cruise_obstacle = T_IDXS * v_cruise_clipped + get_safe_obstacle_distance(
v_cruise_clipped)
x_obstacles = np.column_stack(
[lead_0_obstacle, lead_1_obstacle, cruise_obstacle])
self.source = SOURCES[np.argmin(x_obstacles[0])]
self.params[:, 2] = np.min(x_obstacles, axis=1)
self.run()
if (np.any(lead_xv_0[:, 0] - self.x_sol[:, 0] < CRASH_DISTANCE)
and radarstate.leadOne.modelProb > 0.9):
self.crash_cnt += 1
else:
self.crash_cnt = 0
def update_with_xva(self, x, v, a):
self.yref[:, 1] = x
self.yref[:, 2] = v
self.yref[:, 3] = a
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.accel_limit_arr[:, 0] = -10.
self.accel_limit_arr[:, 1] = 10.
x_obstacle = 1e5 * np.ones((N + 1))
self.params = np.concatenate(
[self.accel_limit_arr, x_obstacle[:, None]], axis=1)
self.run()
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()
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 = sec_since_boot()
if self.solution_status != 0:
if t > self.last_cloudlog_t + 5.0:
self.last_cloudlog_t = t
cloudlog.warning("Long mpc reset, solution_status: %s" %
(self.solution_status))
self.reset()
class LongitudinalMpc:
def __init__(self, e2e=False):
self.e2e = e2e
self.desired_TF = T_FOLLOW
self.reset()
self.source = SOURCES[2]
def reset(self):
self.solver = AcadosOcpSolverFast('long', N, EXPORT_DIR)
self.v_solution = np.zeros(N+1)
self.a_solution = np.zeros(N+1)
self.prev_a = np.array(self.a_solution)
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.x_sol = np.zeros((N+1, X_DIM))
self.u_sol = np.zeros((N,1))
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
self.solve_time = 0.0
self.x0 = np.zeros(X_DIM)
self.set_weights()
def set_weights(self, prev_accel_constraint=True, v_lead0=0, v_lead1=0):
if self.e2e:
self.set_weights_for_xva_policy()
self.params[:,0] = -10.
self.params[:,1] = 10.
self.params[:,2] = 1e5
else:
self.set_weights_for_lead_policy(prev_accel_constraint)
def get_cost_multipliers(self, v_lead0, v_lead1):
v_ego = self.x0[1]
v_ego_bps = [0, 10]
TFs = [1.0, 1.25, T_FOLLOW]
# KRKeegan adjustments to costs for different TFs
# these were calculated using the test_longitudial.py deceleration tests
a_change_tf = interp(self.desired_TF, TFs, [.1, .8, 1.])
j_ego_tf = interp(self.desired_TF, TFs, [.6, .8, 1.])
d_zone_tf = interp(self.desired_TF, TFs, [1.6, 1.3, 1.])
# KRKeegan adjustments to improve sluggish acceleration
# do not apply to deceleration
j_ego_v_ego = 1
a_change_v_ego = 1
if (v_lead0 - v_ego >= 0) and (v_lead1 - v_ego >= 0):
j_ego_v_ego = interp(v_ego, v_ego_bps, [.05, 1.])
a_change_v_ego = interp(v_ego, v_ego_bps, [.05, 1.])
# Select the appropriate min/max of the options
j_ego = min(j_ego_tf, j_ego_v_ego)
a_change = min(a_change_tf, a_change_v_ego)
return (a_change, j_ego, d_zone_tf)
def set_weights_for_lead_policy(self, prev_accel_constraint=True, v_lead0=0, v_lead1=0):
a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0
cost_mulitpliers = self.get_cost_multipliers(v_lead0, v_lead1)
W = np.asfortranarray(np.diag([X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST,
A_EGO_COST, a_change_cost * cost_mulitpliers[0],
J_EGO_COST * cost_mulitpliers[1]]))
for i in range(N):
W[4,4] = A_CHANGE_COST * cost_mulitpliers[0] * np.interp(T_IDXS[i], [0.0, 1.0, 2.0], [1.0, 1.0, 0.0])
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([LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST * cost_mulitpliers[2]])
for i in range(N):
self.solver.cost_set(i, 'Zl', Zl)
def set_weights_for_xva_policy(self):
W = np.asfortranarray(np.diag([0., 10., 1., 10., 0.0, 1.]))
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([LIMIT_COST, LIMIT_COST, LIMIT_COST, 0.0])
for i in range(N):
self.solver.cost_set(i, 'Zl', Zl)
def set_cur_state(self, v, a):
if abs(self.x0[1] - v) > 2.:
self.x0[1] = v
self.x0[2] = a
for i in range(0, N+1):
self.solver.set(i, 'x', self.x0)
else:
self.x0[1] = v
self.x0[2] = a
@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(self, lead):
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 = ((v_ego + v_lead)/2) * (v_ego - v_lead) / (-MIN_ACCEL * 2)
x_lead = clip(x_lead, min_x_lead, 1e8)
v_lead = clip(v_lead, 0.0, 1e8)
a_lead = clip(a_lead, -10., 5.)
lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau)
return lead_xv
def set_accel_limits(self, min_a, max_a):
self.cruise_min_a = min_a
self.cruise_max_a = max_a
def update_TF(self, carstate):
if carstate.distanceLines == 1: # Traffic
# At slow speeds more time, decrease time up to 60mph
# in mph ~= 5 10 15 20 25 30 35 40 45 50 55 60 65 70 75 80 85 90
x_vel = [0, 2.25, 4.5, 6.75, 9, 11.25, 13.5, 15.75, 18, 20.25, 22.5, 24.75, 27, 29.25, 31.5, 33.75, 36, 38.25, 40.5]
y_dist = [1.25, 1.24, 1.23, 1.22, 1.21, 1.20, 1.18, 1.16, 1.13, 1.11, 1.09, 1.07, 1.05, 1.05, 1.05, 1.05, 1.05, 1.05, 1.05]
self.desired_TF = np.interp(carstate.vEgo, x_vel, y_dist)
elif carstate.distanceLines == 2: # Relaxed
self.desired_TF = 1.25
else:
self.desired_TF = T_FOLLOW
def update(self, carstate, radarstate, v_cruise):
self.update_TF(carstate)
v_ego = self.x0[1]
self.status = radarstate.leadOne.status or radarstate.leadTwo.status
lead_xv_0 = self.process_lead(radarstate.leadOne)
lead_xv_1 = self.process_lead(radarstate.leadTwo)
# Use the processed leads which always have a velocity
self.set_weights(lead_xv_0[0,1], lead_xv_1[0,1])
# set accel limits in params
self.params[:,0] = interp(float(self.status), [0.0, 1.0], [self.cruise_min_a, MIN_ACCEL])
self.params[:,1] = self.cruise_max_a
# 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], self.x_sol[:,1], self.desired_TF)
lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1], self.x_sol[:,1], self.desired_TF)
# Fake an obstacle for cruise, this ensures smooth acceleration to set speed
# when the leads are no factor.
v_lower = v_ego + (T_IDXS * self.cruise_min_a * 1.05)
v_upper = v_ego + (T_IDXS * self.cruise_max_a * 1.05)
v_cruise_clipped = np.clip(v_cruise * np.ones(N+1),
v_lower,
v_upper)
cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, self.desired_TF)
x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle])
self.source = SOURCES[np.argmin(x_obstacles[0])]
self.params[:,2] = np.min(x_obstacles, axis=1)
self.params[:,3] = np.copy(self.prev_a)
self.params[:,4] = self.desired_TF
self.run()
if (np.any(lead_xv_0[:,0] - self.x_sol[:,0] < CRASH_DISTANCE) and
radarstate.leadOne.modelProb > 0.9):
self.crash_cnt += 1
else:
self.crash_cnt = 0
def update_with_xva(self, x, v, a):
self.yref[:,1] = x
self.yref[:,2] = v
self.yref[:,3] = a
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[:,3] = np.copy(self.prev_a)
self.run()
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)
t = sec_since_boot()
self.solution_status = self.solver.solve()
self.solve_time = sec_since_boot() - 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.v_solution = self.x_sol[:,1]
self.a_solution = self.x_sol[:,2]
self.j_solution = self.u_sol[:,0]
self.prev_a = np.interp(T_IDXS + 0.05, T_IDXS, self.a_solution)
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()