mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-02-18 17:43:54 +08:00
Code Issues Packages Projects Releases Wiki Activity

acados long merged (#22224)

Browse Source 
* rebased

* cleaner, seems to drive better?

* more stable

* wrong import

* new way of thinking

* reports look nice

* start move back

* works at leas

* good timestamps

* step by step

* somewhat work

* tests pass

* ALL CARS STOPPED

* should work

* fake a cruise obstacle

* cleaner costs

* pretty good except cruise braking

* works pretty well now!

* cleanup

* add source

* add source

* that is needed for unit tests

* nan recovery

* little cleaner

* stop wasting arrays

* unreasonable without unfair init

* this isnt needed without the exponential

* that works too

* unused

* uses less

* new ref

* long enough

* e2e long api

* DONT PUT IN A VIEW INTO ACADOS

* new ref for outside weights

* remove debug prints
old-commit-hash: fe983a7b8c
This commit is contained in:
HaraldSchafer
2021-09-27 19:48:28 -07:00
committed by GitHub
parent 654695809f
commit 2b470f4e38
13 changed files with 300 additions and 447 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
SConstruct
View File

@@ -414,7 +414,6 @@ SConscript(['selfdrive/modeld/SConscript'])
SConscript(['selfdrive/controls/lib/cluster/SConscript'])
SConscript(['selfdrive/controls/lib/lateral_mpc_lib/SConscript'])
SConscript(['selfdrive/controls/lib/lead_mpc_lib/SConscript'])
SConscript(['selfdrive/controls/lib/longitudinal_mpc_lib/SConscript'])
SConscript(['selfdrive/boardd/SConscript'])

2
release/files_common
View File

@@ -240,10 +240,8 @@ selfdrive/controls/lib/fcw.py
selfdrive/controls/lib/cluster/*
selfdrive/controls/lib/lateral_mpc_lib/.gitignore
selfdrive/controls/lib/lead_mpc_lib/.gitignore
selfdrive/controls/lib/longitudinal_mpc_lib/.gitignore
selfdrive/controls/lib/lateral_mpc_lib/*
selfdrive/controls/lib/lead_mpc_lib/*
selfdrive/controls/lib/longitudinal_mpc_lib/*
selfdrive/hardware/__init__.py

2
selfdrive/controls/lib/lead_mpc_lib/.gitignore vendored
View File

@@ -1,2 +0,0 @@
acados_ocp_lead.json
c_generated_code/

58
selfdrive/controls/lib/lead_mpc_lib/SConscript
View File

@@ -1,58 +0,0 @@
Import('env', 'arch')
gen = "c_generated_code"
casadi_model = [
f'{gen}/lead_model/lead_expl_ode_fun.c',
f'{gen}/lead_model/lead_expl_vde_forw.c',
]
casadi_cost_y = [
f'{gen}/lead_cost/lead_cost_y_fun.c',
f'{gen}/lead_cost/lead_cost_y_fun_jac_ut_xt.c',
f'{gen}/lead_cost/lead_cost_y_hess.c',
]
casadi_cost_e = [
f'{gen}/lead_cost/lead_cost_y_e_fun.c',
f'{gen}/lead_cost/lead_cost_y_e_fun_jac_ut_xt.c',
f'{gen}/lead_cost/lead_cost_y_e_hess.c',
]
casadi_cost_0 = [
f'{gen}/lead_cost/lead_cost_y_0_fun.c',
f'{gen}/lead_cost/lead_cost_y_0_fun_jac_ut_xt.c',
f'{gen}/lead_cost/lead_cost_y_0_hess.c',
]
build_files = [f'{gen}/acados_solver_lead.c'] + casadi_model + casadi_cost_y + casadi_cost_e + casadi_cost_0
# extra generated files used to trigger a rebuild
generated_files = [
f'{gen}/Makefile',
f'{gen}/main_lead.c',
f'{gen}/acados_solver_lead.h',
f'{gen}/lead_model/lead_expl_vde_adj.c',
f'{gen}/lead_model/lead_model.h',
f'{gen}/lead_cost/lead_cost_y_fun.h',
f'{gen}/lead_cost/lead_cost_y_e_fun.h',
f'{gen}/lead_cost/lead_cost_y_0_fun.h',
] + build_files
lenv = env.Clone()
lenv.Clean(generated_files, Dir(gen))
lenv.Command(generated_files,
["lead_mpc.py"],
f"cd {Dir('.').abspath} && python lead_mpc.py")
lenv["CFLAGS"].append("-DACADOS_WITH_QPOASES")
lenv["CXXFLAGS"].append("-DACADOS_WITH_QPOASES")
lenv["CCFLAGS"].append("-Wno-unused")
lenv["LINKFLAGS"].append("-Wl,--disable-new-dtags")
lenv.SharedLibrary(f"{gen}/acados_ocp_solver_lead",
build_files,
LIBS=['m', 'acados', 'hpipm', 'blasfeo', 'qpOASES_e'])

0
selfdrive/controls/lib/lead_mpc_lib/__init__.py
View File

270
selfdrive/controls/lib/lead_mpc_lib/lead_mpc.py
View File

@@ -1,270 +0,0 @@
#!/usr/bin/env python3
import os
import math
import numpy as np
from common.realtime import sec_since_boot
from common.numpy_fast import clip
from selfdrive.swaglog import cloudlog
from selfdrive.modeld.constants import T_IDXS
from selfdrive.controls.lib.drive_helpers import MPC_COST_LONG, CONTROL_N
from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU
from pyextra.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
from casadi import SX, vertcat, sqrt, exp
LEAD_MPC_DIR = os.path.dirname(os.path.abspath(__file__))
EXPORT_DIR = os.path.join(LEAD_MPC_DIR, "c_generated_code")
JSON_FILE = "acados_ocp_lead.json"
MPC_T = list(np.arange(0,1.,.2)) + list(np.arange(1.,10.6,.6))
N = len(MPC_T) - 1
def desired_follow_distance(v_ego, v_lead):
TR = 1.8
G = 9.81
return (v_ego * TR - (v_lead - v_ego) * TR + v_ego * v_ego / (2 * G) - v_lead * v_lead / (2 * G)) + 4.0
def gen_lead_model():
model = AcadosModel()
model.name = 'lead'
# set up states & controls
x_ego = SX.sym('x_ego')
v_ego = SX.sym('v_ego')
a_ego = SX.sym('a_ego')
model.x = vertcat(x_ego, v_ego, a_ego)
# controls
j_ego = SX.sym('j_ego')
model.u = vertcat(j_ego)
# xdot
x_ego_dot = SX.sym('x_ego_dot')
v_ego_dot = SX.sym('v_ego_dot')
a_ego_dot = SX.sym('a_ego_dot')
model.xdot = vertcat(x_ego_dot, v_ego_dot, a_ego_dot)
# live parameters
x_lead = SX.sym('x_lead')
v_lead = SX.sym('v_lead')
model.p = vertcat(x_lead, v_lead)
# dynamics model
f_expl = vertcat(v_ego, a_ego, j_ego)
model.f_impl_expr = model.xdot - f_expl
model.f_expl_expr = f_expl
return model
def gen_lead_mpc_solver():
ocp = AcadosOcp()
ocp.model = gen_lead_model()
Tf = np.array(MPC_T)[-1]
# set dimensions
ocp.dims.N = N
# set cost module
ocp.cost.cost_type = 'NONLINEAR_LS'
ocp.cost.cost_type_e = 'NONLINEAR_LS'
QR = np.diag([0.0, 0.0, 0.0, 0.0])
Q = np.diag([0.0, 0.0, 0.0])
ocp.cost.W = QR
ocp.cost.W_e = Q
x_ego, v_ego, a_ego = ocp.model.x[0], ocp.model.x[1], ocp.model.x[2]
j_ego = ocp.model.u[0]
ocp.cost.yref = np.zeros((4, ))
ocp.cost.yref_e = np.zeros((3, ))
x_lead, v_lead = ocp.model.p[0], ocp.model.p[1]
desired_dist = desired_follow_distance(v_ego, v_lead)
dist_err = (desired_dist - (x_lead - x_ego))/(sqrt(v_ego + 0.5) + 0.1)
# TODO hacky weights to keep behavior the same
ocp.model.cost_y_expr = vertcat(exp(.3 * dist_err) - 1.,
((x_lead - x_ego) - (desired_dist)) / (0.05 * v_ego + 0.5),
a_ego * (.1 * v_ego + 1.0),
j_ego * (.1 * v_ego + 1.0))
ocp.model.cost_y_expr_e = vertcat(exp(.3 * dist_err) - 1.,
((x_lead - x_ego) - (desired_dist)) / (0.05 * v_ego + 0.5),
a_ego * (.1 * v_ego + 1.0))
ocp.parameter_values = np.array([0., .0])
# set constraints
ocp.constraints.constr_type = 'BGH'
ocp.constraints.idxbx = np.array([1,])
ocp.constraints.lbx = np.array([0,])
ocp.constraints.ubx = np.array([100.,])
x0 = np.array([0.0, 0.0, 0.0])
ocp.constraints.x0 = x0
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
ocp.solver_options.integrator_type = 'ERK'
ocp.solver_options.nlp_solver_type = 'SQP_RTI'
#ocp.solver_options.nlp_solver_tol_stat = 1e-3
#ocp.solver_options.tol = 1e-3
ocp.solver_options.qp_solver_iter_max = 10
#ocp.solver_options.qp_tol = 1e-3
# set prediction horizon
ocp.solver_options.tf = Tf
ocp.solver_options.shooting_nodes = np.array(MPC_T)
ocp.code_export_directory = EXPORT_DIR
return ocp
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, -2 * (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 = clip(lead.aLeadK, -5.0, 5.0)
# 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()
if __name__ == "__main__":
ocp = gen_lead_mpc_solver()
AcadosOcpSolver.generate(ocp, json_file=JSON_FILE, build=False)

12
selfdrive/controls/lib/longitudinal_mpc_lib/SConscript
View File

@@ -25,7 +25,15 @@ casadi_cost_0 = [
f'{gen}/long_cost/long_cost_y_0_hess.c',
]
build_files = [f'{gen}/acados_solver_long.c'] + casadi_model + casadi_cost_y + casadi_cost_e + casadi_cost_0
casadi_constraints = [
f'{gen}/long_constraints/long_constr_h_fun.c',
f'{gen}/long_constraints/long_constr_h_fun_jac_uxt_zt.c',
f'{gen}/long_constraints/long_constr_h_e_fun.c',
f'{gen}/long_constraints/long_constr_h_e_fun_jac_uxt_zt.c',
]
build_files = [f'{gen}/acados_solver_long.c'] + casadi_model + casadi_cost_y + casadi_cost_e + \
casadi_cost_0 + casadi_constraints
# extra generated files used to trigger a rebuild
generated_files = [
@@ -37,6 +45,8 @@ generated_files = [
f'{gen}/long_model/long_expl_vde_adj.c',
f'{gen}/long_model/long_model.h',
f'{gen}/long_constraints/long_h_constraint.h',
f'{gen}/long_constraints/long_h_e_constraint.h',
f'{gen}/long_cost/long_cost_y_fun.h',
f'{gen}/long_cost/long_cost_y_e_fun.h',
f'{gen}/long_cost/long_cost_y_0_fun.h',

326
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
View File

@@ -1,23 +1,52 @@
#!/usr/bin/env python3
import os
import math
import numpy as np
from common.numpy_fast import clip
from common.realtime import sec_since_boot
from common.numpy_fast import clip
from selfdrive.swaglog import cloudlog
from selfdrive.modeld.constants import T_IDXS as T_IDXS_LST
from selfdrive.controls.lib.drive_helpers import LON_MPC_N as N
from selfdrive.modeld.constants import T_IDXS
from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU
from pyextra.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
from casadi import SX, vertcat
LONG_MPC_DIR = os.path.dirname(os.path.abspath(__file__))
EXPORT_DIR = os.path.join(LONG_MPC_DIR, "c_generated_code")
JSON_FILE = "acados_ocp_long.json"
SOURCES = ['lead0', 'lead1', 'cruise']
X_DIM = 3
U_DIM = 1
COST_E_DIM = 3
COST_DIM = COST_E_DIM + 1
MIN_ACCEL = -3.5
X_EGO_COST = 80.
X_EGO_E2E_COST = 100.
A_EGO_COST = .1
J_EGO_COST = .2
DANGER_ZONE_COST = 1e3
CRASH_DISTANCE = 1.5
LIMIT_COST = 1e6
T_IDXS = np.array(T_IDXS_LST)
T_REACT = 1.8
MAX_BRAKE = 9.81
def get_stopped_equivalence_factor(v_lead):
return T_REACT * v_lead + (v_lead*v_lead) / (2 * MAX_BRAKE)
def get_safe_obstacle_distance(v_ego):
return 2 * T_REACT * v_ego + (v_ego*v_ego) / (2 * MAX_BRAKE) + 4.0
def desired_follow_distance(v_ego, v_lead):
return get_safe_obstacle_distance(v_ego) - get_stopped_equivalence_factor(v_lead)
def gen_long_model():
model = AcadosModel()
@@ -39,6 +68,12 @@ def gen_long_model():
a_ego_dot = SX.sym('a_ego_dot')
model.xdot = vertcat(x_ego_dot, v_ego_dot, a_ego_dot)
# live parameters
x_obstacle = SX.sym('x_obstacle')
a_min = SX.sym('a_min')
a_max = SX.sym('a_max')
model.p = vertcat(a_min, a_max, x_obstacle)
# dynamics model
f_expl = vertcat(v_ego, a_ego, j_ego)
model.f_impl_expr = model.xdot - f_expl
@@ -50,7 +85,7 @@ def gen_long_mpc_solver():
ocp = AcadosOcp()
ocp.model = gen_long_model()
Tf = np.array(T_IDXS)[N]
Tf = T_IDXS[-1]
# set dimensions
ocp.dims.N = N
@@ -59,8 +94,8 @@ def gen_long_mpc_solver():
ocp.cost.cost_type = 'NONLINEAR_LS'
ocp.cost.cost_type_e = 'NONLINEAR_LS'
QR = np.diag([0.0, 0.0, 0.0, 0.0])
Q = np.diag([0.0, 0.0, 0.0])
QR = np.zeros((COST_DIM, COST_DIM))
Q = np.zeros((COST_E_DIM, COST_E_DIM))
ocp.cost.W = QR
ocp.cost.W_e = Q
@@ -68,110 +103,245 @@ def gen_long_mpc_solver():
x_ego, v_ego, a_ego = ocp.model.x[0], ocp.model.x[1], ocp.model.x[2]
j_ego = ocp.model.u[0]
ocp.cost.yref = np.zeros((4, ))
ocp.cost.yref_e = np.zeros((3, ))
# TODO hacky weights to keep behavior the same
ocp.model.cost_y_expr = vertcat(x_ego, v_ego, a_ego, j_ego)
ocp.model.cost_y_expr_e = vertcat(x_ego, v_ego, a_ego)
a_min, a_max = ocp.model.p[0], ocp.model.p[1]
x_obstacle = ocp.model.p[2]
# set constraints
ocp.constraints.constr_type = 'BGH'
ocp.constraints.idxbx = np.array([0, 1,2])
ocp.constraints.lbx = np.array([0., 0, -1.2])
ocp.constraints.ubx = np.array([10000, 100., 1.2])
ocp.constraints.Jsbx = np.eye(3)
x0 = np.array([0.0, 0.0, 0.0])
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)
desired_dist_danger = (7/8) * get_safe_obstacle_distance(v_ego)
costs = [((x_obstacle - x_ego) - (desired_dist_comfort)) / (v_ego + 10.),
x_ego,
a_ego * (v_ego + 10.),
j_ego * (v_ego + 10.)]
ocp.model.cost_y_expr = vertcat(*costs)
ocp.model.cost_y_expr_e = vertcat(*costs[:-1])
constraints = vertcat((v_ego),
(a_ego - a_min),
(a_max - a_ego),
((x_obstacle - x_ego) - (desired_dist_danger)) / (v_ego + 10.),
0.0)
ocp.model.con_h_expr = constraints
ocp.model.con_h_expr_e = constraints
x0 = np.zeros(X_DIM)
ocp.constraints.x0 = x0
ocp.parameter_values = np.array([-1.2, 1.2, 0.0])
l2_penalty = 1.0
# These constraints put hard limits on speed and
# acceleration as well as giving an assymetrical
# cost on approaching a lead. We only use lower
# bounds with an L2 cost.
l1_penalty = 0.0
weights = np.array([0.0, 1e4, 1e4])
ocp.cost.Zl = l2_penalty * weights
ocp.cost.Zu = l2_penalty * weights
l2_penalty = 1.0
weights = np.array([1e6, 1e6, 1e6, 0.0, 0.])
ocp.cost.zl = l1_penalty * weights
ocp.cost.zu = l1_penalty * weights
ocp.cost.Zl = l2_penalty * weights
ocp.cost.Zu = 0.0 * weights
ocp.cost.zu = 0.0 * weights
ocp.constraints.lh = np.array([0.0, 0.0, 0.0, 0.0, 0.0])
ocp.constraints.lh_e = np.array([0.0, 0.0, 0.0, 0.0, 0.0])
ocp.constraints.uh = np.array([1e3, 1e3, 1e3, 1e4, 1e4])
ocp.constraints.uh_e = np.array([1e3, 1e3, 1e3, 1e6, 1e6])
ocp.constraints.idxsh = np.array([0,1,2,3,4])
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
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_iter_max = 2
ocp.solver_options.qp_solver_iter_max = 3
# set prediction horizon
ocp.solver_options.tf = Tf
ocp.solver_options.shooting_nodes = np.array(T_IDXS)[:N+1]
ocp.solver_options.shooting_nodes = T_IDXS
ocp.code_export_directory = EXPORT_DIR
return ocp
class LongitudinalMpc():
def __init__(self):
self.solver = AcadosOcpSolver('long', N, EXPORT_DIR)
self.x_sol = np.zeros((N+1, 3))
self.u_sol = np.zeros((N, 1))
self.set_weights()
self.v_solution = [0.0 for i in range(len(T_IDXS))]
self.a_solution = [0.0 for i in range(len(T_IDXS))]
self.j_solution = [0.0 for i in range(len(T_IDXS)-1)]
self.yref = np.zeros((N+1, 4))
self.solver.cost_set_slice(0, N, "yref", self.yref[:N])
self.solver.cost_set(N, "yref", self.yref[N][:3])
self.T_IDXS = np.array(T_IDXS[:N+1])
self.min_a = -1.2
self.max_a = 1.2
self.mins = np.tile(np.array([0.0, 0.0, self.min_a])[None], reps=(N-1,1))
self.maxs = np.tile(np.array([0.0, 100.0, self.max_a])[None], reps=(N-1,1))
self.x0 = np.zeros(3)
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.last_cloudlog_t = 0
self.status = True
self.solution_status = 0
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', self.x0)
self.solver.set(i, 'x', np.zeros(X_DIM))
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(X_DIM)
self.set_weights()
def set_weights(self):
W = np.diag([0.0, 1.0, 0.0, 50.0])
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')
#TODO hacky weights to keep behavior the same
self.solver.cost_set(N, 'W', (3/20.)*W[:3,:3])
self.solver.cost_set(N, 'W', (3./5.)*np.copy(W[:COST_E_DIM, :COST_E_DIM]))
def set_accel_limits(self, min_a, max_a):
self.min_a = min_a
self.max_a = max_a
self.mins[:,2] = self.min_a
self.maxs[:,2] = self.max_a
self.solver.constraints_set_slice(1, N, "lbx", self.mins, api='old')
self.solver.constraints_set_slice(1, N, "ubx", self.maxs, api='old')
Zl = np.array([LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST, 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_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')
self.solver.cost_set(N, 'W', np.copy(W[:COST_E_DIM, :COST_E_DIM]))
Zl = np.array([LIMIT_COST, LIMIT_COST, LIMIT_COST, 0.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):
self.x0[1] = v
self.x0[2] = 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_0, a_lead_tau):
lead_xv = np.zeros((N+1,2))
lead_xv[0, 0], lead_xv[0, 1] = x_lead, v_lead
for i in range(1, N+1):
dt = T_IDXS[i] - T_IDXS[i-1]
a_lead = a_lead_0 * math.exp(-a_lead_tau * (T_IDXS[i]**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
lead_xv[i, 0], lead_xv[i, 1] = x_lead, v_lead
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 = max(0.0, lead.vLead)
a_lead = clip(lead.aLeadK, -10.0, 5.0)
# MPC will not converge if immidiate 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)
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
lead_xv = 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.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 can keep running in the same mode
x_lead = 50.0
v_lead = v_ego + 10.0
a_lead = 0.0
self.a_lead_tau = _LEAD_ACCEL_TAU
lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, self.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.crashing = False
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)
self.accel_limit_arr[:,0] = np.interp(float(self.status), [0.0, 1.0], [self.cruise_min_a, MIN_ACCEL])
self.accel_limit_arr[:,1] = self.cruise_max_a
# To consider 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
# TODO find cleaner way to write hacky fake cruise obstacle
cruise_lower_bound = v_ego - (1.0 + ((v_ego + 15)/60) * T_IDXS)
cruise_upper_bound = v_ego + (.7 + .7*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])]
x_obstacle = np.min(x_obstacles, axis=1)
self.params = np.concatenate([self.accel_limit_arr,
x_obstacle[:,None]], axis=1)
self.run()
self.crashing = self.crashing or np.sum(lead_xv_0[:,0] - self.x_sol[:,0] < CRASH_DISTANCE) > 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)
def update(self, carstate, model, v_cruise):
v_cruise_clipped = clip(v_cruise, self.x0[1] - 10., self.x0[1] + 10.0)
position = v_cruise_clipped * self.T_IDXS
speed = v_cruise_clipped * np.ones(N+1)
accel = np.zeros(N+1)
self.update_with_xva(position, speed, accel)
def update_with_xva(self, position, speed, accel):
self.yref[:,0] = position
self.yref[:,1] = speed
self.yref[:,2] = accel
self.solver.cost_set_slice(0, N, "yref", self.yref[:N])
self.solver.cost_set(N, "yref", self.yref[N][:3])
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 = list(self.x_sol[:,1])
self.a_solution = list(self.x_sol[:,2])
@@ -181,7 +351,9 @@ class LongitudinalMpc():
if self.solution_status != 0:
if t > self.last_cloudlog_t + 5.0:
self.last_cloudlog_t = t
cloudlog.warning(f'Longitudinal model mpc reset, solution status: {self.solution_status}')
cloudlog.warning("Long mpc reset, solution_status: %s" % (
self.solution_status))
self.prev_lead_status = False
self.reset()

32
selfdrive/controls/lib/longitudinal_planner.py
View File

@@ -11,7 +11,6 @@ from selfdrive.modeld.constants import T_IDXS
from selfdrive.config import Conversions as CV
from selfdrive.controls.lib.fcw import FCWChecker
from selfdrive.controls.lib.longcontrol import LongCtrlState
from selfdrive.controls.lib.lead_mpc_lib.lead_mpc import LeadMpc
from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc
from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N
from selfdrive.swaglog import cloudlog
@@ -47,17 +46,13 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
class Planner():
def __init__(self, CP, init_v=0.0, init_a=0.0):
self.CP = CP
self.mpcs = {}
self.mpcs['lead0'] = LeadMpc(0)
self.mpcs['lead1'] = LeadMpc(1)
self.mpcs['cruise'] = LongitudinalMpc()
self.mpc = LongitudinalMpc()
self.fcw = False
self.fcw_checker = FCWChecker()
self.v_desired = init_v
self.a_desired = init_a
self.longitudinalPlanSource = 'cruise'
self.alpha = np.exp(-DT_MDL/2.0)
self.lead_0 = log.ModelDataV2.LeadDataV3.new_message()
self.lead_1 = log.ModelDataV2.LeadDataV3.new_message()
@@ -100,23 +95,18 @@ class Planner():
# clip limits, cannot init MPC outside of bounds
accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05)
accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05)
self.mpcs['cruise'].set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
next_a = np.inf
for key in self.mpcs:
self.mpcs[key].set_cur_state(self.v_desired, self.a_desired)
self.mpcs[key].update(sm['carState'], sm['radarState'], v_cruise)
if self.mpcs[key].status and self.mpcs[key].a_solution[5] < next_a: # picks slowest solution from accel in ~0.2 seconds
self.longitudinalPlanSource = key
self.v_desired_trajectory = self.mpcs[key].v_solution[:CONTROL_N]
self.a_desired_trajectory = self.mpcs[key].a_solution[:CONTROL_N]
self.j_desired_trajectory = self.mpcs[key].j_solution[:CONTROL_N]
next_a = self.mpcs[key].a_solution[5]
self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
self.mpc.set_cur_state(self.v_desired, self.a_desired)
self.mpc.update(sm['carState'], sm['radarState'], v_cruise)
self.v_desired_trajectory = self.mpc.v_solution[:CONTROL_N]
self.a_desired_trajectory = self.mpc.a_solution[:CONTROL_N]
self.j_desired_trajectory = self.mpc.j_solution[:CONTROL_N]
# determine fcw
if self.mpcs['lead0'].new_lead:
if self.mpc.new_lead:
self.fcw_checker.reset_lead(cur_time)
blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker
self.fcw = self.fcw_checker.update(self.mpcs['lead0'].x_sol[:,2], cur_time,
self.fcw = self.fcw_checker.update(self.mpc.x_sol[:,2], cur_time,
sm['controlsState'].active,
v_ego, sm['carState'].aEgo,
self.lead_1.dRel, self.lead_1.vLead, self.lead_1.aLeadK,
@@ -143,8 +133,8 @@ class Planner():
longitudinalPlan.accels = [float(x) for x in self.a_desired_trajectory]
longitudinalPlan.jerks = [float(x) for x in self.j_desired_trajectory]
longitudinalPlan.hasLead = self.mpcs['lead0'].status
longitudinalPlan.longitudinalPlanSource = self.longitudinalPlanSource
longitudinalPlan.hasLead = self.mpc.prev_lead_status
longitudinalPlan.longitudinalPlanSource = self.mpc.source
longitudinalPlan.fcw = self.fcw
pm.send('longitudinalPlan', plan_send)

6
selfdrive/controls/tests/test_following_distance.py
View File

@@ -2,10 +2,10 @@
import unittest
import numpy as np
from selfdrive.controls.lib.lead_mpc_lib.lead_mpc import desired_follow_distance
from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import desired_follow_distance
from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
def run_following_distance_simulation(v_lead, t_end=150.0):
def run_following_distance_simulation(v_lead, t_end=100.0):
man = Maneuver(
'',
duration=t_end,
@@ -29,7 +29,7 @@ class TestFollowingDistance(unittest.TestCase):
simulation_steady_state = run_following_distance_simulation(v_lead)
correct_steady_state = desired_follow_distance(v_lead, v_lead)
self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=.2)
self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=(correct_steady_state*.1 + .3))
if __name__ == "__main__":

32
selfdrive/test/longitudinal_maneuvers/test_longitudinal.py
View File

@@ -9,9 +9,9 @@ from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
# TODO: make new FCW tests
maneuvers = [
Maneuver(
'approach stopped car at 30m/s',
'approach stopped car at 20m/s',
duration=20.,
initial_speed=30.,
initial_speed=25.,
lead_relevancy=True,
initial_distance_lead=120.,
speed_lead_values=[30., 0.],
@@ -22,7 +22,7 @@ maneuvers = [
duration=20.,
initial_speed=20.,
lead_relevancy=True,
initial_distance_lead=60.,
initial_distance_lead=90.,
speed_lead_values=[20., 0.],
breakpoints=[0., 1.],
),
@@ -54,22 +54,26 @@ maneuvers = [
breakpoints=[0., 15., 21.66],
),
Maneuver(
'steady state following a car at 20m/s, then lead decel to 0mph at 5m/s^2',
'steady state following a car at 20m/s, then lead decel to 0mph at 4m/s^2',
duration=40.,
initial_speed=20.,
lead_relevancy=True,
initial_distance_lead=35.,
speed_lead_values=[20., 20., 0.],
breakpoints=[0., 15., 19.],
prob_lead_values=[0., 1., 1.],
cruise_values=[20., 20., 20.],
breakpoints=[2., 2.01, 7.01],
),
Maneuver(
"approach stopped car at 20m/s",
duration=30.,
initial_speed=20.,
lead_relevancy=True,
initial_distance_lead=50.,
speed_lead_values=[0., 0.],
breakpoints=[1., 11.],
initial_distance_lead=120.,
speed_lead_values=[0.0, 0., 0.],
prob_lead_values=[0.0, 0., 1.],
cruise_values=[20., 20., 20.],
breakpoints=[0.0, 2., 2.01],
),
Maneuver(
"approach slower cut-in car at 20m/s",
@@ -92,6 +96,16 @@ maneuvers = [
breakpoints=[1., 11.],
only_radar=True,
),
Maneuver(
"NaN recovery",
duration=30.,
initial_speed=15.,
lead_relevancy=True,
initial_distance_lead=60.,
speed_lead_values=[0., 0., 0.0],
breakpoints=[1., 1.01, 11.],
cruise_values=[float("nan"), 15., 15.],
),
]
@@ -118,7 +132,7 @@ def run_maneuver_worker(k):
man = maneuvers[k]
print(man.title)
valid, _ = man.evaluate()
self.assertTrue(valid)
self.assertTrue(valid, msg=man.title)
return run

2
selfdrive/test/process_replay/ref_commit
View File

@@ -1 +1 @@
8c733450bb28bcdb11d6b9991c8784e1f720f7b2
2282e3f208438237fe61d7bf636d6ad6b507c571

4
selfdrive/test/test_onroad.py
View File

@@ -23,7 +23,7 @@ PROCS = {
"selfdrive.controls.controlsd": 50.0,
"./loggerd": 45.0,
"./locationd": 9.1,
"selfdrive.controls.plannerd": 33.0,
"selfdrive.controls.plannerd": 27.0,
"./_ui": 15.0,
"selfdrive.locationd.paramsd": 9.1,
"./camerad": 7.07,
@@ -55,7 +55,7 @@ if TICI:
"selfdrive.controls.controlsd": 28.0,
"./camerad": 31.0,
"./_ui": 21.0,
"selfdrive.controls.plannerd": 15.0,
"selfdrive.controls.plannerd": 14.0,
"selfdrive.locationd.paramsd": 5.0,
"./_dmonitoringmodeld": 10.0,
"selfdrive.thermald.thermald": 1.5,