class LateralMpc():
def __init__(self, x0=np.zeros(X_DIM)):
self.solver = AcadosOcpSolver('lat', N, EXPORT_DIR)
self.reset(x0)
def reset(self, 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, 3))
self.solver.cost_set_slice(0, N, "yref", self.yref[:N])
self.solver.cost_set(N, "yref", self.yref[N][:2])
W = np.eye(3)
self.Ws = np.tile(W[None], reps=(N, 1, 1))
# Somehow needed for stable init
for i in range(N + 1):
self.solver.set(i, 'x', np.zeros(X_DIM))
self.solver.constraints_set(0, "lbx", x0)
self.solver.constraints_set(0, "ubx", x0)
self.solver.solve()
self.solution_status = 0
self.cost = 0
def set_weights(self, path_weight, heading_weight, steer_rate_weight):
self.Ws[:, 0, 0] = path_weight
self.Ws[:, 1, 1] = heading_weight
self.Ws[:, 2, 2] = steer_rate_weight
self.solver.cost_set_slice(0, N, 'W', self.Ws, api='old')
#TODO hacky weights to keep behavior the same
self.solver.cost_set(N, 'W', (3 / 20.) * self.Ws[0, :2, :2])
def run(self, x0, v_ego, car_rotation_radius, y_pts, heading_pts):
x0_cp = np.copy(x0)
self.solver.constraints_set(0, "lbx", x0_cp)
self.solver.constraints_set(0, "ubx", x0_cp)
self.yref[:, 0] = y_pts
self.yref[:, 1] = heading_pts * (v_ego + 5.0)
self.solver.cost_set_slice(0, N, "yref", self.yref[:N])
self.solver.cost_set(N, "yref", self.yref[N][:2])
self.solution_status = self.solver.solve()
self.solver.fill_in_slice(0, N + 1, 'x', self.x_sol)
self.solver.fill_in_slice(0, N, 'u', self.u_sol)
self.cost = self.solver.get_cost()
class LeadMpc():
def __init__(self, lead_id):
self.lead_id = lead_id
self.solver = AcadosOcpSolver('lead', N, EXPORT_DIR)
self.v_solution = [0.0 for i in range(N)]
self.a_solution = [0.0 for i in range(N)]
self.j_solution = [0.0 for i in range(N - 1)]
yref = np.zeros((N + 1, 4))
self.solver.cost_set_slice(0, N, "yref", yref[:N])
self.solver.set(N, "yref", yref[N][:3])
self.x_sol = np.zeros((N + 1, 3))
self.u_sol = np.zeros((N, 1))
self.lead_xv = np.zeros((N + 1, 2))
self.reset()
self.set_weights()
def reset(self):
for i in range(N + 1):
self.solver.set(i, 'x', np.zeros(3))
self.last_cloudlog_t = 0
self.status = False
self.new_lead = False
self.prev_lead_status = False
self.crashing = False
self.prev_lead_x = 10
self.solution_status = 0
self.x0 = np.zeros(3)
def set_weights(self):
W = np.diag([
MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE,
MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK
])
Ws = np.tile(W[None], reps=(N, 1, 1))
self.solver.cost_set_slice(0, N, 'W', Ws, api='old')
#TODO hacky weights to keep behavior the same
self.solver.cost_set(N, 'W', (3. / 5.) * W[:3, :3])
def set_cur_state(self, v, a):
self.x0[1] = v
self.x0[2] = a
def extrapolate_lead(self, x_lead, v_lead, a_lead_0, a_lead_tau):
dt = .2
t = .0
for i in range(N + 1):
if i > 4:
dt = .6
self.lead_xv[i, 0], self.lead_xv[i, 1] = x_lead, v_lead
a_lead = a_lead_0 * math.exp(-a_lead_tau * (t**2) / 2.)
x_lead += v_lead * dt
v_lead += a_lead * dt
if v_lead < 0.0:
a_lead = 0.0
v_lead = 0.0
t += dt
def init_with_sim(self, v_ego, lead_xv, a_lead_0):
a_ego = min(
0.0, -(v_ego - lead_xv[0, 1]) * (v_ego - lead_xv[0, 1]) /
(2.0 * lead_xv[0, 0] + 0.01) + a_lead_0)
dt = .2
t = .0
x_ego = 0.0
for i in range(N + 1):
if i > 4:
dt = .6
v_ego += a_ego * dt
if v_ego <= 0.0:
v_ego = 0.0
a_ego = 0.0
x_ego += v_ego * dt
t += dt
self.solver.set(i, 'x', np.array([x_ego, v_ego, a_ego]))
def update(self, carstate, radarstate, v_cruise):
self.crashing = False
v_ego = self.x0[1]
if self.lead_id == 0:
lead = radarstate.leadOne
else:
lead = radarstate.leadTwo
self.status = lead.status
if lead is not None and lead.status:
x_lead = lead.dRel
v_lead = max(0.0, lead.vLead)
a_lead = lead.aLeadK
# MPC will not converge if immidiate crash is expected
# Clip lead distance to what is still possible to brake for
MIN_ACCEL = -3.5
min_x_lead = (
(v_ego + v_lead) / 2) * (v_ego - v_lead) / (-MIN_ACCEL * 2)
if x_lead < min_x_lead:
x_lead = min_x_lead
self.crashing = True
if (v_lead < 0.1 or -a_lead / 2.0 > v_lead):
v_lead = 0.0
a_lead = 0.0
self.a_lead_tau = lead.aLeadTau
self.new_lead = False
self.extrapolate_lead(x_lead, v_lead, a_lead, self.a_lead_tau)
if not self.prev_lead_status or abs(x_lead -
self.prev_lead_x) > 2.5:
self.init_with_sim(v_ego, self.lead_xv, a_lead)
self.new_lead = True
self.prev_lead_status = True
self.prev_lead_x = x_lead
else:
self.prev_lead_status = False
# Fake a fast lead car, so mpc keeps running
x_lead = 50.0
v_lead = v_ego + 10.0
a_lead = 0.0
self.a_lead_tau = _LEAD_ACCEL_TAU
self.extrapolate_lead(x_lead, v_lead, a_lead, self.a_lead_tau)
self.solver.constraints_set(0, "lbx", self.x0)
self.solver.constraints_set(0, "ubx", self.x0)
for i in range(N + 1):
self.solver.set_param(i, self.lead_xv[i])
self.solution_status = self.solver.solve()
self.solver.fill_in_slice(0, N + 1, 'x', self.x_sol)
self.solver.fill_in_slice(0, N, 'u', self.u_sol)
#self.solver.print_statistics()
self.v_solution = np.interp(T_IDXS[:CONTROL_N], MPC_T,
list(self.x_sol[:, 1]))
self.a_solution = np.interp(T_IDXS[:CONTROL_N], MPC_T,
list(self.x_sol[:, 2]))
self.j_solution = np.interp(T_IDXS[:CONTROL_N], MPC_T[:-1],
list(self.u_sol[:, 0]))
# Reset if goes through lead car
self.crashing = self.crashing or np.sum(
self.lead_xv[:, 0] - self.x_sol[:, 0] < 0) > 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("Lead mpc %d reset, solution_status: %s" %
(self.lead_id, self.solution_status))
self.prev_lead_status = False
self.reset()
class LongitudinalMpc():
def __init__(self, e2e=False, desired_TR=T_REACT):
self.e2e = e2e
self.desired_TR = desired_TR
self.v_ego = 0.
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 = AcadosOcpSolver('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))
self.solver.cost_set_slice(0, N, "yref", self.yref[:N])
self.solver.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, 4))
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):
# WARNING: deceleration tests with these costs:
# 1.0 TR fails at 3 m/s/s test
# 1.1 TR fails at 3+ m/s/s test
# 1.2-1.8 TR succeeds at all tests with no FCW
TRs = [1.2, 1.8, 2.7]
x_ego_obstacle_cost_multiplier = interp(self.desired_TR, TRs,
[3., 1.0, 0.1])
j_ego_cost_multiplier = interp(self.desired_TR, TRs, [0.5, 1.0, 1.0])
d_zone_cost_multiplier = interp(self.desired_TR, TRs, [4., 1.0, 1.0])
W = np.asfortranarray(
np.diag([
X_EGO_OBSTACLE_COST * x_ego_obstacle_cost_multiplier,
X_EGO_COST, V_EGO_COST, A_EGO_COST,
J_EGO_COST * j_ego_cost_multiplier
]))
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 * d_zone_cost_multiplier
])
for i in range(N):
self.solver.cost_set(i, 'Zl', Zl)
def set_weights_for_xva_policy(self):
W = np.diag([0., 10., 1., 10., 1.])
Ws = np.tile(W[None], reps=(N, 1, 1))
self.solver.cost_set_slice(0, N, 'W', Ws, api='old')
# 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])
Zls = np.tile(Zl[None], reps=(N + 1, 1, 1))
self.solver.cost_set_slice(0, N + 1, 'Zl', Zls, api='old')
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 set_desired_TR(self, desired_TR):
self.desired_TR = clip(desired_TR, 1.2, 2.7)
self.set_weights()
def update(self, carstate, radarstate, v_cruise):
self.v_ego = carstate.vEgo
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], self.desired_TR)
lead_1_obstacle = lead_xv_1[:, 0] + get_stopped_equivalence_factor(
lead_xv_1[:, 1], self.desired_TR)
# 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, self.desired_TR)
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] = self.desired_TR
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
self.solver.cost_set_slice(0, N, "yref", self.yref[:N], api='old')
self.solver.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))
desired_TR = T_REACT * np.ones((N + 1))
self.params = np.concatenate(
[self.accel_limit_arr, x_obstacle[:, None], desired_TR[:, None]],
axis=1)
self.run()
def run(self):
for i in range(N + 1):
self.solver.set_param(i, 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.solver.fill_in_slice(0, N + 1, 'x', self.x_sol)
self.solver.fill_in_slice(0, N, 'u', self.u_sol)
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.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]
self.desired_TR = 1.8
def reset(self):
self.solver = AcadosOcpSolver('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))
self.solver.cost_set_slice(0, N, "yref", self.yref[:N])
self.solver.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.diag([X_EGO_COST, 0.0, A_EGO_COST, J_EGO_COST])
Ws = np.tile(W[None], reps=(N, 1, 1))
self.solver.cost_set_slice(0, N, 'W', Ws, api='old')
# 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])
Zls = np.tile(Zl[None], reps=(N + 1, 1, 1))
self.solver.cost_set_slice(0, N + 1, 'Zl', Zls, api='old')
def set_weights_for_xva_policy(self):
W = np.diag([0.0, X_EGO_E2E_COST, 0., J_EGO_COST])
Ws = np.tile(W[None], reps=(N, 1, 1))
self.solver.cost_set_slice(0, N, 'W', Ws, api='old')
# 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])
Zls = np.tile(Zl[None], reps=(N + 1, 1, 1))
self.solver.cost_set_slice(0, N + 1, 'Zl', Zls, api='old')
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)
# TODO: experiment with setting accel to 0 so lead's accel is factored in (only distance is considered now)
lead_xv_ideal = self.extrapolate_lead(
get_stopped_equivalence_factor(v_lead, self.desired_TR), v_lead,
a_lead, a_lead_tau)
return lead_xv, lead_xv_ideal
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, lead_xv_ideal_0 = self.process_lead(radarstate.leadOne)
lead_xv_1, lead_xv_ideal_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.
# This may all be wrong, just some tests to enable a hacky adjustable following distance
# Calculates difference in ideal distance vs actual distance to a TR diff in seconds
# lead_react_diff_0 = (lead_xv_ideal_0[:,0] - lead_xv_0[:,0]) / lead_xv_0[:,1]
# lead_react_diff_1 = (lead_xv_ideal_1[:,0] - lead_xv_1[:,0]) / lead_xv_1[:,1]
# TODO: some tuning to be had here
# basically the lower the desired TR the more we want to stick near it
# so we come up with a cost to multiply difference in actual TR vs. desired TR by
# and thus the less we change the stopped factor below
# react_diff_cost = np.interp(self.desired_TR, [0.9, 1.8, 2.7], [2, 1, 0.5])
# react_diff_cost = 1.
# print(lead_react_diff_0[0])
# lead_react_diff_0 = lead_react_diff_0[0]
# lead_react_diff_1 = lead_react_diff_1[0]
# react_diff_mult_0 = np.interp(abs(lead_react_diff_0) * react_diff_cost, [0, 0.9], [1, 0.1])
# react_diff_mult_1 = np.interp(abs(lead_react_diff_1) * react_diff_cost, [0, 0.9], [1, 0.1])
# t_react_compensation_0 = T_REACT + (T_REACT - self.desired_TR)
# t_react_compensation_1 = T_REACT + (T_REACT - self.desired_TR)
# lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1], t_react_compensation_0)
# lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1], t_react_compensation_1)
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.params[:, 3] = get_safe_obstacle_distance(v_ego, self.desired_TR)
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.solver.cost_set_slice(0, N, "yref", self.yref[:N], api='old')
self.solver.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_param(i, 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.solver.fill_in_slice(0, N + 1, 'x', self.x_sol)
self.solver.fill_in_slice(0, N, 'u', self.u_sol)
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()
