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

Support e2e long in longitudinal planner (#25636)

Browse Source 
* refactor

* Add planer modes to support offline, acc, and blended

* add acceleration

* Fix index

* Update model ref

* Read in model outputs

* Add model msg

* Add e2e logic

* Add source
pull/25639/head
HaraldSchafer 3 years ago committed by GitHub
parent cbe1c89698
commit e1b7a37a1f
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 103 additions and 47 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. 117
      selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
  2. 22
      selfdrive/controls/lib/longitudinal_planner.py
  3. 5
      selfdrive/modeld/models/driving.cc
  4. 4
      selfdrive/test/longitudinal_maneuvers/plant.py
  5. 2
      selfdrive/test/process_replay/model_replay_ref_commit

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

@ -3,7 +3,7 @@ import os
import numpy as np
from common.realtime import sec_since_boot
from common.numpy_fast import clip, interp
from common.numpy_fast import clip
from system.swaglog import cloudlog
from selfdrive.modeld.constants import index_function
from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU
@ -20,7 +20,7 @@ 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")
SOURCES = ['lead0', 'lead1', 'cruise']
SOURCES = ['lead0', 'lead1', 'cruise', 'e2e']
X_DIM = 3
U_DIM = 1
@ -50,6 +50,7 @@ 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)
T_DIFFS = np.diff(T_IDXS, prepend=[0.])
MIN_ACCEL = -3.5
MAX_ACCEL = 2.0
T_FOLLOW = 1.45
COMFORT_BRAKE = 2.5
STOP_DISTANCE = 6.0
@ -190,8 +191,8 @@ def gen_long_ocp():
class LongitudinalMpc:
def __init__(self, e2e=False):
self.e2e = e2e
def __init__(self, mode='acc'):
self.mode = mode
self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
self.reset()
self.source = SOURCES[2]
@ -225,49 +226,42 @@ class LongitudinalMpc:
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]))
def set_cost_weights(self, cost_weights, constraint_cost_weights):
W = np.asfortranarray(np.diag(cost_weights))
for i in range(N):
# TODO don't hardcode A_CHANGE_COST idx
# 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])
W[4,4] = cost_weights[4] * 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])
Zl = np.array(constraint_cost_weights)
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., 0.2, 0.25, 1., 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_weights(self, prev_accel_constraint=True):
if self.mode == 'acc':
a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0
cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, a_change_cost, J_EGO_COST]
constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST]
elif self.mode == 'blended':
cost_weights = [0., 1.0, 0.0, 0.0, 0.0, 1.0]
constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, 0.0]
elif self.mode == 'e2e':
cost_weights = [0., 0.2, 0.25, 1., 0.0, .1]
constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, 0.0]
else:
raise NotImplementedError(f'Planner mode {self.mode} not recognized')
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
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)
@ -306,31 +300,62 @@ class LongitudinalMpc:
self.cruise_min_a = min_a
self.cruise_max_a = max_a
def update(self, carstate, radarstate, v_cruise):
def update(self, carstate, radarstate, v_cruise, x, v, a, j):
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)
# Update in ACC mode or ACC/e2e blend
if self.mode == 'acc':
self.params[:,0] = MIN_ACCEL if self.status else self.cruise_min_a
self.params[:,1] = self.cruise_max_a
# 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])]
# These are not used in ACC mode
x[:], v[:], a[:], j[:] = 0.0, 0.0, 0.0, 0.0
elif self.mode == 'blended':
self.params[:,0] = MIN_ACCEL
self.params[:,1] = MAX_ACCEL
x_obstacles = np.column_stack([lead_0_obstacle,
lead_1_obstacle])
cruise_target = T_IDXS * v_cruise + x[0]
xforward = ((v[1:] + v[:-1]) / 2) * (T_IDXS[1:] - T_IDXS[:-1])
x = np.cumsum(np.insert(xforward, 0, x[0]))
x_and_cruise = np.column_stack([x, cruise_target])
x = np.min(x_and_cruise, axis=1)
self.source = 'e2e'
else:
raise NotImplementedError(f'Planner mode {self.mode} not recognized')
self.yref[:,1] = x
self.yref[:,2] = v
self.yref[:,3] = a
self.yref[:,5] = j
for i in range(N):
self.solver.set(i, "yref", self.yref[i])
self.solver.set(N, "yref", self.yref[N][:COST_E_DIM])
x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle])
self.source = SOURCES[np.argmin(x_obstacles[0])]
@ -345,6 +370,10 @@ class LongitudinalMpc:
self.crash_cnt = 0
def update_with_xva(self, x, v, a):
self.params[:,0] = -10.
self.params[:,1] = 10.
self.params[:,2] = 1e5
# v, and a are in local frame, but x is wrt the x[0] position
# In >90degree turns, x goes to 0 (and may even be -ve)
# So, we use integral(v) + x[0] to obtain the forward-distance

22
selfdrive/controls/lib/longitudinal_planner.py
Unescape View File

@ -53,12 +53,31 @@ class Planner:
self.a_desired = init_a
self.v_desired_filter = FirstOrderFilter(init_v, 2.0, DT_MDL)
self.t_uniform = np.arange(0.0, T_IDXS_MPC[-1] + 0.5, 0.5)
self.v_desired_trajectory = np.zeros(CONTROL_N)
self.a_desired_trajectory = np.zeros(CONTROL_N)
self.j_desired_trajectory = np.zeros(CONTROL_N)
self.solverExecutionTime = 0.0
def parse_model(self, model_msg):
if (len(model_msg.position.x) == 33 and
len(model_msg.velocity.x) == 33 and
len(model_msg.acceleration.x) == 33):
x = np.interp(T_IDXS_MPC, T_IDXS, model_msg.position.x)
v = np.interp(T_IDXS_MPC, T_IDXS, model_msg.velocity.x)
a = np.interp(T_IDXS_MPC, T_IDXS, model_msg.acceleration.x)
# Uniform interp so gradient is less noisy
a_sparse = np.interp(self.t_uniform, T_IDXS, model_msg.acceleration.x)
j_sparse = np.gradient(a_sparse, self.t_uniform)
j = np.interp(T_IDXS_MPC, self.t_uniform, j_sparse)
else:
x = np.zeros(len(T_IDXS_MPC))
v = np.zeros(len(T_IDXS_MPC))
a = np.zeros(len(T_IDXS_MPC))
j = np.zeros(len(T_IDXS_MPC))
return x, v, a, j
def update(self, sm):
v_ego = sm['carState'].vEgo
@ -95,7 +114,8 @@ class Planner:
self.mpc.set_weights(prev_accel_constraint)
self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
self.mpc.update(sm['carState'], sm['radarState'], v_cruise)
x, v, a, j = self.parse_model(sm['modelV2'])
self.mpc.update(sm['carState'], sm['radarState'], v_cruise, x, v, a, j)
self.v_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.v_solution)
self.a_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.a_solution)

5
selfdrive/modeld/models/driving.cc
Unescape View File

@ -203,6 +203,7 @@ void fill_plan(cereal::ModelDataV2::Builder &framed, const ModelOutputPlanPredic
std::array<float, TRAJECTORY_SIZE> pos_x_std, pos_y_std, pos_z_std;
std::array<float, TRAJECTORY_SIZE> vel_x, vel_y, vel_z;
std::array<float, TRAJECTORY_SIZE> rot_x, rot_y, rot_z;
std::array<float, TRAJECTORY_SIZE> acc_x, acc_y, acc_z;
std::array<float, TRAJECTORY_SIZE> rot_rate_x, rot_rate_y, rot_rate_z;
for(int i=0; i<TRAJECTORY_SIZE; i++) {
@ -215,6 +216,9 @@ void fill_plan(cereal::ModelDataV2::Builder &framed, const ModelOutputPlanPredic
vel_x[i] = plan.mean[i].velocity.x;
vel_y[i] = plan.mean[i].velocity.y;
vel_z[i] = plan.mean[i].velocity.z;
acc_x[i] = plan.mean[i].acceleration.x;
acc_y[i] = plan.mean[i].acceleration.y;
acc_z[i] = plan.mean[i].acceleration.z;
rot_x[i] = plan.mean[i].rotation.x;
rot_y[i] = plan.mean[i].rotation.y;
rot_z[i] = plan.mean[i].rotation.z;
@ -225,6 +229,7 @@ void fill_plan(cereal::ModelDataV2::Builder &framed, const ModelOutputPlanPredic
fill_xyzt(framed.initPosition(), T_IDXS_FLOAT, pos_x, pos_y, pos_z, pos_x_std, pos_y_std, pos_z_std);
fill_xyzt(framed.initVelocity(), T_IDXS_FLOAT, vel_x, vel_y, vel_z);
fill_xyzt(framed.initAcceleration(), T_IDXS_FLOAT, acc_x, acc_y, acc_z);
fill_xyzt(framed.initOrientation(), T_IDXS_FLOAT, rot_x, rot_y, rot_z);
fill_xyzt(framed.initOrientationRate(), T_IDXS_FLOAT, rot_rate_x, rot_rate_y, rot_rate_z);
}

4
selfdrive/test/longitudinal_maneuvers/plant.py
Unescape View File

@ -54,6 +54,7 @@ class Plant():
radar = messaging.new_message('radarState')
control = messaging.new_message('controlsState')
car_state = messaging.new_message('carState')
model = messaging.new_message('modelV2')
a_lead = (v_lead - self.v_lead_prev)/self.ts
self.v_lead_prev = v_lead
@ -95,7 +96,8 @@ class Plant():
# ******** get controlsState messages for plotting ***
sm = {'radarState': radar.radarState,
'carState': car_state.carState,
'controlsState': control.controlsState}
'controlsState': control.controlsState,
'modelV2': model.modelV2}
self.planner.update(sm)
self.speed = self.planner.v_desired_filter.x
self.acceleration = self.planner.a_desired

2
selfdrive/test/process_replay/model_replay_ref_commit
Unescape View File

@ -1 +1 @@
cffb4e720b0379bedd4ff802912d998ace775c37
c40319a454840d8a2196ec1227d27b536ee14375

Write Preview
Loading…
Cancel
Save