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

Acados long fast (#22233)

Browse Source 
* acados long

* new ref

* SPPEEEEEDDD

* less iterations

* this shouldn't be so high

* reset only essentials

* minimal reset for long mpc

* more cpu usage plannerd

* Use lead mpc even when going to crash

* reset to current state

* Use open loop speed for lead mpc

* 1 iteration is too little for cruise mpc

* add whitespace

* update refs
old-commit-hash: 66c275b711
This commit is contained in:
HaraldSchafer
2021-09-15 19:00:12 -07:00
committed by GitHub
parent 9ffbc81b07
commit 2d4b64ed04
14 changed files with 564 additions and 103 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
phonelibs/acados/build.sh
View File

@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:56eac2b5de0c9b1eccc31f072f315a17f3e116fd31acfb8720026ff551bc4f79
oid sha256:3adaf7cd3c6bc6bb6e7c8bc35c9107ce618467c46e54e051785e786241281846
size 2053

4
pyextra/acados_template/acados_ocp_solver.py
View File

@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:7cf452d4fc7959bc2b2dac312550b9ef41640c9762515c6a1b904aefbd304e23
size 61689
oid sha256:b406ef368856f2e70e02a56b42361d7c5937383529654489318e5831a1d23926
size 60258

3
selfdrive/controls/lib/fcw.py
View File

@@ -44,8 +44,7 @@ class FCWChecker():
ttc = min(2 * x_lead / (math.sqrt(delta) + v_rel), max_ttc)
return ttc
def update(self, mpc_solution, cur_time, active, v_ego, a_ego, x_lead, v_lead, a_lead, y_lead, vlat_lead, fcw_lead, blinkers):
mpc_solution_a = list(mpc_solution[0].a_ego)
def update(self, mpc_solution_a, cur_time, active, v_ego, a_ego, x_lead, v_lead, a_lead, y_lead, vlat_lead, fcw_lead, blinkers):
self.last_min_a = min(mpc_solution_a)
self.v_lead_max = max(self.v_lead_max, v_lead)

17
selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py
View File

@@ -11,7 +11,7 @@ from pyextra.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
LAT_MPC_DIR = os.path.dirname(os.path.abspath(__file__))
EXPORT_DIR = os.path.join(LAT_MPC_DIR, "c_generated_code")
JSON_FILE = "acados_ocp_lat.json"
X_DIM = 6
def gen_lat_model():
model = AcadosModel()
@@ -56,7 +56,6 @@ def gen_lat_mpc_solver():
ocp = AcadosOcp()
ocp.model = gen_lat_model()
N = 16
Tf = np.array(T_IDXS)[N]
# set dimensions
@@ -109,13 +108,13 @@ def gen_lat_mpc_solver():
class LateralMpc():
def __init__(self, x0=np.zeros(6)):
def __init__(self, x0=np.zeros(X_DIM)):
self.solver = AcadosOcpSolver('lat', N, EXPORT_DIR)
self.reset(x0)
def reset(self, x0=np.zeros(6)):
self.x_sol = np.zeros((N+1, 4))
self.u_sol = np.zeros((N))
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])
@@ -124,7 +123,7 @@ class LateralMpc():
# Somehow needed for stable init
for i in range(N+1):
self.solver.set(i, 'x', np.zeros(6))
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()
@@ -149,8 +148,8 @@ class LateralMpc():
self.solver.cost_set(N, "yref", self.yref[N][:2])
self.solution_status = self.solver.solve()
self.x_sol = self.solver.get_slice(0, N+1, 'x')
self.u_sol = self.solver.get_slice(0, N, 'u')
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()

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

@@ -1,2 +1,2 @@
generator
lib_qp/
acados_ocp_lead.json
c_generated_code/

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

@@ -1,48 +1,58 @@
Import('env', 'arch')
gen = "c_generated_code"
cpp_path = [
"#phonelibs/acado/include",
"#phonelibs/acado/include/acado",
"#phonelibs/qpoases/INCLUDE",
"#phonelibs/qpoases/INCLUDE/EXTRAS",
"#phonelibs/qpoases/SRC/",
"#phonelibs/qpoases",
"lib_mpc_export",
casadi_model = [
f'{gen}/lead_model/lead_expl_ode_fun.c',
f'{gen}/lead_model/lead_expl_vde_forw.c',
]
generated_c = [
'lib_mpc_export/acado_auxiliary_functions.c',
'lib_mpc_export/acado_qpoases_interface.cpp',
'lib_mpc_export/acado_integrator.c',
'lib_mpc_export/acado_solver.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',
]
generated_h = [
'lib_mpc_export/acado_common.h',
'lib_mpc_export/acado_auxiliary_functions.h',
'lib_mpc_export/acado_qpoases_interface.hpp',
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',
]
interface_dir = Dir('lib_mpc_export')
build_files = [f'{gen}/acados_solver_lead.c'] + casadi_model + casadi_cost_y + casadi_cost_e + casadi_cost_0
SConscript(['#phonelibs/qpoases/SConscript'], variant_dir='lib_qp', exports=['interface_dir'])
# extra generated files used to trigger a rebuild
generated_files = [
f'{gen}/Makefile',
if GetOption('mpc_generate'):
generator_cpp = File('generator.cpp')
f'{gen}/main_lead.c',
f'{gen}/acados_solver_lead.h',
acado_libs = [File(f"#phonelibs/acado/{arch}/lib/libacado_toolkit.a"),
File(f"#phonelibs/acado/{arch}/lib/libacado_casadi.a"),
File(f"#phonelibs/acado/{arch}/lib/libacado_csparse.a")]
f'{gen}/lead_model/lead_expl_vde_adj.c',
generator = env.Program('generator', generator_cpp, LIBS=acado_libs, CPPPATH=cpp_path,
CCFLAGS=env['CCFLAGS'] + ["-Wno-deprecated", "-Wno-overloaded-shift-op-parentheses"])
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
cmd = f"cd {Dir('.').get_abspath()} && {generator[0].get_abspath()}"
env.Command(generated_c + generated_h, generator, cmd)
lenv = env.Clone()
lenv.Clean(generated_files, Dir(gen))
lenv.Command(generated_files,
["lead_mpc.py"],
f"cd {Dir('.').abspath} && python lead_mpc.py")
mpc_files = ["longitudinal_mpc.c"] + generated_c
env.SharedLibrary('mpc0', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path)
env.SharedLibrary('mpc1', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path)
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'])

265
selfdrive/controls/lib/lead_mpc_lib/lead_mpc.py Normal file
View File

@@ -0,0 +1,265 @@
#!/usr/bin/env python3
import os
import math
import numpy as np
from common.realtime import sec_since_boot
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 RW(v_ego, v_l):
TR = 1.8
G = 9.81
return (v_ego * TR - (v_l - v_ego) * TR + v_ego * v_ego / (2 * G) - v_l * v_l / (2 * G))
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]
G = 9.81
TR = 1.8
desired_dist = (v_ego * TR
- (v_lead - v_ego) * TR
+ v_ego*v_ego/(2*G)
- v_lead * v_lead / (2*G))
dist_err = (desired_dist + 4.0 - (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 + 4.0)) / (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 + 4.0)) / (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, -(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):
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
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 = 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)

4
selfdrive/controls/lib/longitudinal_mpc_lib/.gitignore vendored
View File

@@ -1,2 +1,2 @@
generator
lib_qp/
acados_ocp_long.json
c_generated_code/

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

@@ -1,47 +1,58 @@
Import('env', 'arch')
cpp_path = [
"#",
"#selfdrive",
"#phonelibs/acado/include",
"#phonelibs/acado/include/acado",
"#phonelibs/qpoases/INCLUDE",
"#phonelibs/qpoases/INCLUDE/EXTRAS",
"#phonelibs/qpoases/SRC/",
"#phonelibs/qpoases",
"lib_mpc_export",
gen = "c_generated_code"
casadi_model = [
f'{gen}/long_model/long_expl_ode_fun.c',
f'{gen}/long_model/long_expl_vde_forw.c',
]
generated_c = [
'lib_mpc_export/acado_auxiliary_functions.c',
'lib_mpc_export/acado_qpoases_interface.cpp',
'lib_mpc_export/acado_integrator.c',
'lib_mpc_export/acado_solver.c',
casadi_cost_y = [
f'{gen}/long_cost/long_cost_y_fun.c',
f'{gen}/long_cost/long_cost_y_fun_jac_ut_xt.c',
f'{gen}/long_cost/long_cost_y_hess.c',
]
generated_h = [
'lib_mpc_export/acado_common.h',
'lib_mpc_export/acado_auxiliary_functions.h',
'lib_mpc_export/acado_qpoases_interface.hpp',
casadi_cost_e = [
f'{gen}/long_cost/long_cost_y_e_fun.c',
f'{gen}/long_cost/long_cost_y_e_fun_jac_ut_xt.c',
f'{gen}/long_cost/long_cost_y_e_hess.c',
]
interface_dir = Dir('lib_mpc_export')
casadi_cost_0 = [
f'{gen}/long_cost/long_cost_y_0_fun.c',
f'{gen}/long_cost/long_cost_y_0_fun_jac_ut_xt.c',
f'{gen}/long_cost/long_cost_y_0_hess.c',
]
SConscript(['#phonelibs/qpoases/SConscript'], variant_dir='lib_qp', exports=['interface_dir'])
build_files = [f'{gen}/acados_solver_long.c'] + casadi_model + casadi_cost_y + casadi_cost_e + casadi_cost_0
if GetOption('mpc_generate'):
generator_cpp = File('generator.cpp')
# extra generated files used to trigger a rebuild
generated_files = [
f'{gen}/Makefile',
acado_libs = [File(f"#phonelibs/acado/{arch}/lib/libacado_toolkit.a"),
File(f"#phonelibs/acado/{arch}/lib/libacado_casadi.a"),
File(f"#phonelibs/acado/{arch}/lib/libacado_csparse.a")]
f'{gen}/main_long.c',
f'{gen}/acados_solver_long.h',
generator = env.Program('generator', generator_cpp, LIBS=acado_libs, CPPPATH=cpp_path,
CCFLAGS=env['CCFLAGS'] + ["-Wno-deprecated", "-Wno-overloaded-shift-op-parentheses"])
f'{gen}/long_model/long_expl_vde_adj.c',
cmd = f"cd {Dir('.').get_abspath()} && {generator[0].get_abspath()}"
env.Command(generated_c + generated_h, generator, cmd)
f'{gen}/long_model/long_model.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',
] + build_files
lenv = env.Clone()
lenv.Clean(generated_files, Dir(gen))
mpc_files = ["longitudinal_mpc.c"] + generated_c
env.SharedLibrary('mpc', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path)
lenv.Command(generated_files,
["long_mpc.py"],
f"cd {Dir('.').abspath} && python long_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_long",
build_files,
LIBS=['m', 'acados', 'hpipm', 'blasfeo', 'qpOASES_e'])

183
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py Normal file
View File

@@ -0,0 +1,183 @@
#!/usr/bin/env python3
import os
import numpy as np
from common.numpy_fast import clip
from common.realtime import sec_since_boot
from selfdrive.swaglog import cloudlog
from selfdrive.controls.lib.drive_helpers import LON_MPC_N as N
from selfdrive.modeld.constants import T_IDXS
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"
def gen_long_model():
model = AcadosModel()
model.name = 'long'
# 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)
# 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_long_mpc_solver():
ocp = AcadosOcp()
ocp.model = gen_long_model()
Tf = np.array(T_IDXS)[N]
# 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, ))
# 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)
# 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.constraints.x0 = x0
l2_penalty = 1.0
l1_penalty = 0.0
weights = np.array([0.0, 1e4, 1e4])
ocp.cost.Zl = l2_penalty * weights
ocp.cost.Zu = l2_penalty * weights
ocp.cost.zl = l1_penalty * weights
ocp.cost.zu = l1_penalty * weights
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
# set prediction horizon
ocp.solver_options.tf = Tf
ocp.solver_options.shooting_nodes = np.array(T_IDXS)[:N+1]
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.ones((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)
self.reset()
def reset(self):
self.last_cloudlog_t = 0
self.status = True
self.solution_status = 0
for i in range(N+1):
self.solver.set(i, 'x', self.x0)
def set_weights(self):
W = np.diag([0.0, 1.0, 0.0, 50.0])
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])
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')
def set_cur_state(self, v, a):
self.x0[1] = v
self.x0[2] = a
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)
self.yref[:,0] = v_cruise_clipped * self.T_IDXS # position
self.yref[:,1] = v_cruise_clipped * np.ones(N+1) # speed
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])
self.j_solution = list(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(f'Longitudinal model mpc reset, solution status: {self.solution_status}')
self.reset()
if __name__ == "__main__":
ocp = gen_long_mpc_solver()
AcadosOcpSolver.generate(ocp, json_file=JSON_FILE, build=False)

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

@@ -11,8 +11,8 @@ 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 import LeadMpc
from selfdrive.controls.lib.long_mpc import LongitudinalMpc
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
@@ -64,6 +64,7 @@ class Planner():
self.v_desired_trajectory = np.zeros(CONTROL_N)
self.a_desired_trajectory = np.zeros(CONTROL_N)
self.j_desired_trajectory = np.zeros(CONTROL_N)
def update(self, sm, CP):
@@ -97,10 +98,9 @@ class Planner():
accel_limits_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL)
accel_limits_turns[0] = min(accel_limits_turns[0], accel_limits_turns[1])
# clip limits, cannot init MPC outside of bounds
accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired)
accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired)
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)
@@ -116,7 +116,7 @@ class Planner():
if self.mpcs['lead0'].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'].mpc_solution, cur_time,
self.fcw = self.fcw_checker.update(self.mpcs['lead0'].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,

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

@@ -5,13 +5,7 @@ import numpy as np
from cereal import log
import cereal.messaging as messaging
from selfdrive.config import Conversions as CV
from selfdrive.controls.lib.lead_mpc import LeadMpc
def RW(v_ego, v_l):
TR = 1.8
G = 9.81
return (v_ego * TR - (v_l - v_ego) * TR + v_ego * v_ego / (2 * G) - v_l * v_l / (2 * G))
from selfdrive.controls.lib.lead_mpc_lib.lead_mpc import RW, LeadMpc
class FakePubMaster():
@@ -36,8 +30,8 @@ def run_following_distance_simulation(v_lead, t_end=200.0):
# Setup CarState
CS = messaging.new_message('carState')
CS.carState.vEgo = v_ego
CS.carState.aEgo = a_ego
CS.carState.vEgo = float(v_ego)
CS.carState.aEgo = float(a_ego)
# Setup model packet
radarstate = messaging.new_message('radarState')
@@ -57,7 +51,7 @@ def run_following_distance_simulation(v_lead, t_end=200.0):
mpc.update(CS.carState, radarstate.radarState, 0)
# Choose slowest of two solutions
v_ego, a_ego = mpc.mpc_solution.v_ego[5], mpc.mpc_solution.a_ego[5]
v_ego, a_ego = float(mpc.v_solution[5]), float(mpc.a_solution[5])
# Update state
x_lead += v_lead * dt
@@ -76,7 +70,7 @@ class TestFollowingDistance(unittest.TestCase):
simulation_steady_state = run_following_distance_simulation(v_lead)
correct_steady_state = RW(v_lead, v_lead) + 4.0
self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=.1)
self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=.2)
if __name__ == "__main__":

2
selfdrive/test/process_replay/ref_commit
View File

@@ -1 +1 @@
c967d67902bd36e82c8b9e70c5538475b448ac49
c66c5414f85cd98ad2dd7a83989e05990851da74

8
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": 26.0,
"selfdrive.controls.plannerd": 33.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": 12.0,
"selfdrive.controls.plannerd": 15.0,
"selfdrive.locationd.paramsd": 5.0,
"./_dmonitoringmodeld": 10.0,
"selfdrive.thermald.thermald": 1.5,
@@ -198,8 +198,8 @@ class TestOnroad(unittest.TestCase):
ts = [getattr(getattr(m, s), "modelExecutionTime") for m in self.lr if m.which() == s]
self.assertLess(min(ts), instant_max, f"high '{s}' execution time: {min(ts)}")
self.assertLess(np.mean(ts), avg_max, f"high avg '{s}' execution time: {np.mean(ts)}")
result += f"'{s}' execution time: {min(ts)}'"
result += f"'{s}' avg execution time: {np.mean(ts)}"
result += f"'{s}' execution time: {min(ts)}\n"
result += f"'{s}' avg execution time: {np.mean(ts)}\n"
print(result)
def test_timings(self):