sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

formatting

Browse Source
pull/23296/head
Shane Smiskol 3 years ago
parent b80b08e6e9
commit 481c390f40
1 changed files with 53 additions and 52 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 105
      selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

105
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
Unescape View File

@ -24,7 +24,7 @@ SOURCES = ['lead0', 'lead1', 'cruise']
X_DIM = 3
U_DIM = 1
PARAM_DIM= 4
PARAM_DIM = 4
COST_E_DIM = 5
COST_DIM = COST_E_DIM + 1
CONSTR_DIM = 4
@ -39,12 +39,11 @@ DANGER_ZONE_COST = 100.
CRASH_DISTANCE = .5
LIMIT_COST = 1e6
# Less timestamps doesn't hurt performance and leads 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+1) for idx in range(N+1)]
T_IDXS_LST = [index_function(idx, max_val=MAX_T, max_idx=N + 1) for idx in range(N + 1)]
T_IDXS = np.array(T_IDXS_LST)
T_DIFFS = np.diff(T_IDXS, prepend=[0.])
@ -53,11 +52,14 @@ T_FOLLOW = 1.45
COMFORT_BRAKE = 2.5
STOP_DISTANCE = 6.0
def get_stopped_equivalence_factor(v_lead):
return (v_lead**2) / (2 * COMFORT_BRAKE)
return (v_lead ** 2) / (2 * COMFORT_BRAKE)
def get_safe_obstacle_distance(v_ego):
return (v_ego**2) / (2 * COMFORT_BRAKE) + T_FOLLOW * v_ego + STOP_DISTANCE
return (v_ego ** 2) / (2 * COMFORT_BRAKE) + T_FOLLOW * v_ego + STOP_DISTANCE
def desired_follow_distance(v_ego, v_lead):
return get_safe_obstacle_distance(v_ego) - get_stopped_equivalence_factor(v_lead)
@ -123,8 +125,8 @@ def gen_long_mpc_solver():
x_obstacle = ocp.model.p[2]
prev_a = ocp.model.p[3]
ocp.cost.yref = np.zeros((COST_DIM, ))
ocp.cost.yref_e = np.zeros((COST_E_DIM, ))
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)
@ -136,7 +138,7 @@ def gen_long_mpc_solver():
x_ego,
v_ego,
a_ego,
20*(a_ego - prev_a),
20 * (a_ego - prev_a),
j_ego]
ocp.model.cost_y_expr = vertcat(*costs)
ocp.model.cost_y_expr_e = vertcat(*costs[:-1])
@ -144,10 +146,10 @@ def gen_long_mpc_solver():
# Constraints on speed, acceleration and desired distance to
# the obstacle, which is treated as a slack constraint so it
# behaves like an assymetrical cost.
constraints = vertcat((v_ego),
constraints = vertcat(v_ego,
(a_ego - a_min),
(a_max - a_ego),
((x_obstacle - x_ego) - (3/4) * (desired_dist_comfort)) / (v_ego + 10.))
((x_obstacle - x_ego) - (3 / 4) * (desired_dist_comfort)) / (v_ego + 10.))
ocp.model.con_h_expr = constraints
ocp.model.con_h_expr_e = vertcat(np.zeros(CONSTR_DIM))
@ -164,8 +166,8 @@ def gen_long_mpc_solver():
ocp.constraints.lh = np.zeros(CONSTR_DIM)
ocp.constraints.lh_e = np.zeros(CONSTR_DIM)
ocp.constraints.uh = 1e4*np.ones(CONSTR_DIM)
ocp.constraints.uh_e = 1e4*np.ones(CONSTR_DIM)
ocp.constraints.uh = 1e4 * np.ones(CONSTR_DIM)
ocp.constraints.uh_e = 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
@ -176,7 +178,7 @@ def gen_long_mpc_solver():
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
ocp.solver_options.integrator_type = 'ERK'
ocp.solver_options.nlp_solver_type = 'SQP_RTI'
ocp.solver_options.qp_solver_cond_N = N//4
ocp.solver_options.qp_solver_cond_N = N // 4
# 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.
@ -190,29 +192,29 @@ def gen_long_mpc_solver():
return ocp
class LongitudinalMpc():
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.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.v_solution = [0.0 for i in range(N + 1)]
self.a_solution = [0.0 for i in range(N + 1)]
self.prev_a = np.array(self.a_solution)
self.j_solution = [0.0 for i in range(N)]
self.yref = np.zeros((N+1, COST_DIM))
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.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
@ -230,7 +232,7 @@ class LongitudinalMpc():
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, A_CHANGE_COST, J_EGO_COST]))
for i in range(N):
W[4,4] = A_CHANGE_COST * np.interp(T_IDXS[i], [0.0, 1.0, 2.0], [1.0, 1.0, 0.0])
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.
@ -258,14 +260,14 @@ class LongitudinalMpc():
if abs(self.x0[1] - v) > 2.:
self.x0[1] = v
self.x0[2] = a
for i in range(0, N+1):
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.)
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))
@ -287,7 +289,7 @@ class LongitudinalMpc():
# 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)
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.)
@ -307,69 +309,68 @@ class LongitudinalMpc():
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
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])
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_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[:, 2] = np.min(x_obstacles, axis=1)
if prev_accel_constraint:
self.params[:,3] = np.copy(self.prev_a)
self.params[:, 3] = np.copy(self.prev_a)
else:
self.params[:,3] = a_ego
self.params[:, 3] = a_ego
self.run()
if (np.any(lead_xv_0[:,0] - self.x_sol[:,0] < CRASH_DISTANCE) and
radarstate.leadOne.modelProb > 0.9):
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.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.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],
self.prev_a[:,None]], axis=1)
x_obstacle[:, None],
self.prev_a[:, None]], axis=1)
self.run()
def run(self):
for i in range(N+1):
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):
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.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)

Write Preview
Loading…
Cancel
Save