Port lateral planning to ACADOS (#22080)
* lateral acados * looks good! * add another solve, needed for init somehow * use copy * init correctly * cleanup sconstruct * Update files_common * update cpu usage * reset when invalid * fix cpu usage * cost_set doesnt leak * new ref * non leaky reset Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com> Co-authored-by: Comma Device <device@comma.ai>pull/22222/head
HaraldSchafer
GitHub
parent
32db9184d4
commit
7081ab4fb7
36
SConstruct
36
SConstruct
|
|
@ -67,10 +67,17 @@ USE_WEBCAM = os.getenv("USE_WEBCAM") is not None
|
|||
|
||||
lenv = {
|
||||
"PATH": os.environ['PATH'],
|
||||
"LD_LIBRARY_PATH": [Dir(f"#phonelibs/acados/{arch}/lib").abspath],
|
||||
"PYTHONPATH": Dir("#").abspath + ":" + Dir("#pyextra/").abspath,
|
||||
|
||||
"ACADOS_SOURCE_DIR": Dir("#phonelibs/acados/acados").abspath,
|
||||
"TERA_PATH": Dir("#").abspath + f"/phonelibs/acados/{arch}/t_renderer",
|
||||
}
|
||||
|
||||
rpath = lenv["LD_LIBRARY_PATH"].copy()
|
||||
|
||||
if arch == "aarch64" or arch == "larch64":
|
||||
lenv["LD_LIBRARY_PATH"] = '/data/data/com.termux/files/usr/lib'
|
||||
lenv["LD_LIBRARY_PATH"] += ['/data/data/com.termux/files/usr/lib']
|
||||
|
||||
if arch == "aarch64":
|
||||
# android
|
||||
|
|
@ -87,6 +94,7 @@ if arch == "aarch64" or arch == "larch64":
|
|||
"/system/vendor/lib64",
|
||||
"/system/comma/usr/lib",
|
||||
"#phonelibs/nanovg",
|
||||
f"#phonelibs/acados/{arch}/lib",
|
||||
]
|
||||
|
||||
if arch == "larch64":
|
||||
|
|
@ -100,8 +108,9 @@ if arch == "aarch64" or arch == "larch64":
|
|||
]
|
||||
cflags = ["-DQCOM2", "-mcpu=cortex-a57"]
|
||||
cxxflags = ["-DQCOM2", "-mcpu=cortex-a57"]
|
||||
rpath = ["/usr/local/lib"]
|
||||
rpath += ["/usr/local/lib"]
|
||||
else:
|
||||
rpath = []
|
||||
libpath += [
|
||||
"#phonelibs/snpe/aarch64",
|
||||
"#phonelibs/libyuv/lib",
|
||||
|
|
@ -109,7 +118,6 @@ if arch == "aarch64" or arch == "larch64":
|
|||
]
|
||||
cflags = ["-DQCOM", "-D_USING_LIBCXX", "-mcpu=cortex-a57"]
|
||||
cxxflags = ["-DQCOM", "-D_USING_LIBCXX", "-mcpu=cortex-a57"]
|
||||
rpath = []
|
||||
else:
|
||||
cflags = []
|
||||
cxxflags = []
|
||||
|
|
@ -134,6 +142,7 @@ else:
|
|||
]
|
||||
else:
|
||||
libpath = [
|
||||
"#phonelibs/acados/x86_64/lib",
|
||||
"#phonelibs/snpe/x86_64-linux-clang",
|
||||
"#phonelibs/libyuv/x64/lib",
|
||||
"#phonelibs/mapbox-gl-native-qt/x86_64",
|
||||
|
|
@ -143,15 +152,12 @@ else:
|
|||
"/usr/local/lib",
|
||||
]
|
||||
|
||||
rpath = [
|
||||
"phonelibs/snpe/x86_64-linux-clang",
|
||||
"cereal",
|
||||
"selfdrive/common"
|
||||
rpath += [
|
||||
Dir("#phonelibs/snpe/x86_64-linux-clang").abspath,
|
||||
Dir("#cereal").abspath,
|
||||
Dir("#selfdrive/common").abspath
|
||||
]
|
||||
|
||||
# allows shared libraries to work globally
|
||||
rpath = [os.path.join(os.getcwd(), x) for x in rpath]
|
||||
|
||||
if GetOption('asan'):
|
||||
ccflags = ["-fsanitize=address", "-fno-omit-frame-pointer"]
|
||||
ldflags = ["-fsanitize=address"]
|
||||
|
|
@ -164,15 +170,12 @@ else:
|
|||
|
||||
# no --as-needed on mac linker
|
||||
if arch != "Darwin":
|
||||
ldflags += ["-Wl,--as-needed"]
|
||||
ldflags += ["-Wl,--as-needed", "-Wl,--no-undefined"]
|
||||
|
||||
# Enable swaglog include in submodules
|
||||
cflags += ["-DSWAGLOG"]
|
||||
cxxflags += ["-DSWAGLOG"]
|
||||
|
||||
# change pythonpath to this
|
||||
lenv["PYTHONPATH"] = Dir("#").path
|
||||
|
||||
env = Environment(
|
||||
ENV=lenv,
|
||||
CCFLAGS=[
|
||||
|
|
@ -191,6 +194,9 @@ env = Environment(
|
|||
|
||||
CPPPATH=cpppath + [
|
||||
"#",
|
||||
"#phonelibs/acados/include",
|
||||
"#phonelibs/acados/include/blasfeo/include",
|
||||
"#phonelibs/acados/include/hpipm/include",
|
||||
"#phonelibs/catch2/include",
|
||||
"#phonelibs/bzip2",
|
||||
"#phonelibs/libyuv/include",
|
||||
|
|
@ -410,7 +416,7 @@ SConscript(['selfdrive/camerad/SConscript'])
|
|||
SConscript(['selfdrive/modeld/SConscript'])
|
||||
|
||||
SConscript(['selfdrive/controls/lib/cluster/SConscript'])
|
||||
SConscript(['selfdrive/controls/lib/lateral_mpc/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'])
|
||||
|
||||
|
|
|
|||
|
|
@ -237,33 +237,16 @@ selfdrive/controls/lib/radar_helpers.py
|
|||
selfdrive/controls/lib/vehicle_model.py
|
||||
selfdrive/controls/lib/fcw.py
|
||||
selfdrive/controls/lib/long_mpc.py
|
||||
selfdrive/controls/lib/lead_mpc.py
|
||||
selfdrive/controls/lib/lead_mpc.py
|
||||
|
||||
selfdrive/controls/lib/cluster/*
|
||||
|
||||
selfdrive/controls/lib/lateral_mpc/lib_mpc_export/*
|
||||
selfdrive/controls/lib/lateral_mpc/.gitignore
|
||||
selfdrive/controls/lib/lateral_mpc/SConscript
|
||||
selfdrive/controls/lib/lateral_mpc/__init__.py
|
||||
selfdrive/controls/lib/lateral_mpc/generator.cpp
|
||||
selfdrive/controls/lib/lateral_mpc/libmpc_py.py
|
||||
selfdrive/controls/lib/lateral_mpc/lateral_mpc.c
|
||||
|
||||
selfdrive/controls/lib/lead_mpc_lib/lib_mpc_export/*
|
||||
selfdrive/controls/lib/lateral_mpc_lib/.gitignore
|
||||
selfdrive/controls/lib/lead_mpc_lib/.gitignore
|
||||
selfdrive/controls/lib/lead_mpc_lib/SConscript
|
||||
selfdrive/controls/lib/lead_mpc_lib/__init__.py
|
||||
selfdrive/controls/lib/lead_mpc_lib/generator.cpp
|
||||
selfdrive/controls/lib/lead_mpc_lib/libmpc_py.py
|
||||
selfdrive/controls/lib/lead_mpc_lib/longitudinal_mpc.c
|
||||
|
||||
selfdrive/controls/lib/longitudinal_mpc_lib/lib_mpc_export/*
|
||||
selfdrive/controls/lib/longitudinal_mpc_lib/.gitignore
|
||||
selfdrive/controls/lib/longitudinal_mpc_lib/SConscript
|
||||
selfdrive/controls/lib/longitudinal_mpc_lib/__init__.py
|
||||
selfdrive/controls/lib/longitudinal_mpc_lib/generator.cpp
|
||||
selfdrive/controls/lib/longitudinal_mpc_lib/libmpc_py.py
|
||||
selfdrive/controls/lib/longitudinal_mpc_lib/longitudinal_mpc.c
|
||||
selfdrive/controls/lib/lateral_mpc_lib/*
|
||||
selfdrive/controls/lib/lead_mpc_lib/*
|
||||
selfdrive/controls/lib/longitudinal_mpc_lib/*
|
||||
|
||||
selfdrive/hardware/__init__.py
|
||||
selfdrive/hardware/base.h
|
||||
|
|
@ -481,6 +464,11 @@ phonelibs/snpe/aarch64**
|
|||
phonelibs/snpe/larch64**
|
||||
phonelibs/snpe/dsp**
|
||||
|
||||
phonelibs/acados/x86_64/**
|
||||
phonelibs/acados/aarch64/**
|
||||
phonelibs/acados/larch64/**
|
||||
phonelibs/acados/include/**
|
||||
|
||||
phonelibs/android_frameworks_native/**
|
||||
phonelibs/android_hardware_libhardware/**
|
||||
phonelibs/android_system_core/**
|
||||
|
|
@ -489,6 +477,7 @@ scripts/update_now.sh
|
|||
scripts/stop_updater.sh
|
||||
|
||||
pyextra/.gitignore
|
||||
pyextra/acados_template/**
|
||||
|
||||
rednose/**
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,2 @@
|
|||
acados_ocp_lat.json
|
||||
c_generated_code/
|
||||
|
|
@ -0,0 +1,58 @@
|
|||
Import('env', 'arch')
|
||||
|
||||
gen = "c_generated_code"
|
||||
|
||||
casadi_model = [
|
||||
f'{gen}/lat_model/lat_expl_ode_fun.c',
|
||||
f'{gen}/lat_model/lat_expl_vde_forw.c',
|
||||
]
|
||||
|
||||
casadi_cost_y = [
|
||||
f'{gen}/lat_cost/lat_cost_y_fun.c',
|
||||
f'{gen}/lat_cost/lat_cost_y_fun_jac_ut_xt.c',
|
||||
f'{gen}/lat_cost/lat_cost_y_hess.c',
|
||||
]
|
||||
|
||||
casadi_cost_e = [
|
||||
f'{gen}/lat_cost/lat_cost_y_e_fun.c',
|
||||
f'{gen}/lat_cost/lat_cost_y_e_fun_jac_ut_xt.c',
|
||||
f'{gen}/lat_cost/lat_cost_y_e_hess.c',
|
||||
]
|
||||
|
||||
casadi_cost_0 = [
|
||||
f'{gen}/lat_cost/lat_cost_y_0_fun.c',
|
||||
f'{gen}/lat_cost/lat_cost_y_0_fun_jac_ut_xt.c',
|
||||
f'{gen}/lat_cost/lat_cost_y_0_hess.c',
|
||||
]
|
||||
|
||||
build_files = [f'{gen}/acados_solver_lat.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_lat.c',
|
||||
f'{gen}/acados_solver_lat.h',
|
||||
|
||||
f'{gen}/lat_model/lat_expl_vde_adj.c',
|
||||
|
||||
f'{gen}/lat_model/lat_model.h',
|
||||
f'{gen}/lat_cost/lat_cost_y_fun.h',
|
||||
f'{gen}/lat_cost/lat_cost_y_e_fun.h',
|
||||
f'{gen}/lat_cost/lat_cost_y_0_fun.h',
|
||||
] + build_files
|
||||
|
||||
lenv = env.Clone()
|
||||
lenv.Clean(generated_files, Dir(gen))
|
||||
|
||||
lenv.Command(generated_files,
|
||||
["lat_mpc.py"],
|
||||
f"cd {Dir('.').abspath} && python lat_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_lat",
|
||||
build_files,
|
||||
LIBS=['m', 'acados', 'hpipm', 'blasfeo', 'qpOASES_e'])
|
||||
|
|
@ -0,0 +1,158 @@
|
|||
#!/usr/bin/env python3
|
||||
import os
|
||||
import numpy as np
|
||||
|
||||
from casadi import SX, vertcat, sin, cos
|
||||
from selfdrive.controls.lib.drive_helpers import LAT_MPC_N as N
|
||||
from selfdrive.controls.lib.drive_helpers import T_IDXS
|
||||
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"
|
||||
|
||||
|
||||
def gen_lat_model():
|
||||
model = AcadosModel()
|
||||
model.name = 'lat'
|
||||
|
||||
# set up states & controls
|
||||
x_ego = SX.sym('x_ego')
|
||||
y_ego = SX.sym('y_ego')
|
||||
psi_ego = SX.sym('psi_ego')
|
||||
curv_ego = SX.sym('curv_ego')
|
||||
v_ego = SX.sym('v_ego')
|
||||
rotation_radius = SX.sym('rotation_radius')
|
||||
model.x = vertcat(x_ego, y_ego, psi_ego, curv_ego, v_ego, rotation_radius)
|
||||
|
||||
# controls
|
||||
curv_rate = SX.sym('curv_rate')
|
||||
model.u = vertcat(curv_rate)
|
||||
|
||||
# xdot
|
||||
x_ego_dot = SX.sym('x_ego_dot')
|
||||
y_ego_dot = SX.sym('y_ego_dot')
|
||||
psi_ego_dot = SX.sym('psi_ego_dot')
|
||||
curv_ego_dot = SX.sym('curv_ego_dot')
|
||||
v_ego_dot = SX.sym('v_ego_dot')
|
||||
rotation_radius_dot = SX.sym('rotation_radius_dot')
|
||||
model.xdot = vertcat(x_ego_dot, y_ego_dot, psi_ego_dot, curv_ego_dot,
|
||||
v_ego_dot, rotation_radius_dot)
|
||||
|
||||
# dynamics model
|
||||
f_expl = vertcat(v_ego * cos(psi_ego) - rotation_radius * sin(psi_ego) * (v_ego * curv_ego),
|
||||
v_ego * sin(psi_ego) + rotation_radius * cos(psi_ego) * (v_ego * curv_ego),
|
||||
v_ego * curv_ego,
|
||||
curv_rate,
|
||||
0.0,
|
||||
0.0)
|
||||
model.f_impl_expr = model.xdot - f_expl
|
||||
model.f_expl_expr = f_expl
|
||||
return model
|
||||
|
||||
|
||||
def gen_lat_mpc_solver():
|
||||
ocp = AcadosOcp()
|
||||
ocp.model = gen_lat_model()
|
||||
|
||||
N = 16
|
||||
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'
|
||||
|
||||
Q = np.diag([0.0, 0.0])
|
||||
QR = np.diag([0.0, 0.0, 0.0])
|
||||
|
||||
ocp.cost.W = QR
|
||||
ocp.cost.W_e = Q
|
||||
|
||||
y_ego, psi_ego = ocp.model.x[1], ocp.model.x[2]
|
||||
curv_rate = ocp.model.u[0]
|
||||
v_ego = ocp.model.x[4]
|
||||
|
||||
|
||||
ocp.cost.yref = np.zeros((3, ))
|
||||
ocp.cost.yref_e = np.zeros((2, ))
|
||||
# TODO hacky weights to keep behavior the same
|
||||
ocp.model.cost_y_expr = vertcat(y_ego,
|
||||
((v_ego +5.0) * psi_ego),
|
||||
((v_ego +5.0) * 4 * curv_rate))
|
||||
ocp.model.cost_y_expr_e = vertcat(y_ego,
|
||||
((v_ego +5.0) * psi_ego))
|
||||
|
||||
# set constraints
|
||||
ocp.constraints.constr_type = 'BGH'
|
||||
ocp.constraints.idxbx = np.array([2,3])
|
||||
ocp.constraints.ubx = np.array([np.radians(90), np.radians(50)])
|
||||
ocp.constraints.lbx = np.array([-np.radians(90), -np.radians(50)])
|
||||
x0 = np.array([0.0, 0.0, 0.0, 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.qp_solver_iter_max = 10
|
||||
|
||||
# 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 LateralMpc():
|
||||
def __init__(self, x0=np.zeros(6)):
|
||||
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))
|
||||
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(6))
|
||||
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 = np.column_stack([y_pts, heading_pts*(v_ego+5.0), np.zeros(N+1)])
|
||||
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.x_sol = self.solver.get_slice(0, N+1, 'x')
|
||||
self.u_sol = self.solver.get_slice(0, N, 'u')
|
||||
self.cost = self.solver.get_cost()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
ocp = gen_lat_mpc_solver()
|
||||
AcadosOcpSolver.generate(ocp, json_file=JSON_FILE, build=False)
|
||||
|
|
@ -3,7 +3,7 @@ import numpy as np
|
|||
from common.realtime import sec_since_boot, DT_MDL
|
||||
from common.numpy_fast import interp
|
||||
from selfdrive.swaglog import cloudlog
|
||||
from selfdrive.controls.lib.lateral_mpc import libmpc_py
|
||||
from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc
|
||||
from selfdrive.controls.lib.drive_helpers import CONTROL_N, MPC_COST_LAT, LAT_MPC_N, CAR_ROTATION_RADIUS
|
||||
from selfdrive.controls.lib.lane_planner import LanePlanner, TRAJECTORY_SIZE
|
||||
from selfdrive.config import Conversions as CV
|
||||
|
|
@ -46,7 +46,6 @@ class LateralPlanner():
|
|||
self.last_cloudlog_t = 0
|
||||
self.steer_rate_cost = CP.steerRateCost
|
||||
|
||||
self.setup_mpc()
|
||||
self.solution_invalid_cnt = 0
|
||||
self.lane_change_state = LaneChangeState.off
|
||||
self.lane_change_direction = LaneChangeDirection.none
|
||||
|
|
@ -62,17 +61,12 @@ class LateralPlanner():
|
|||
self.t_idxs = np.arange(TRAJECTORY_SIZE)
|
||||
self.y_pts = np.zeros(TRAJECTORY_SIZE)
|
||||
|
||||
def setup_mpc(self):
|
||||
self.libmpc = libmpc_py.libmpc
|
||||
self.libmpc.init()
|
||||
|
||||
self.mpc_solution = libmpc_py.ffi.new("log_t *")
|
||||
self.cur_state = libmpc_py.ffi.new("state_t *")
|
||||
self.cur_state[0].x = 0.0
|
||||
self.cur_state[0].y = 0.0
|
||||
self.cur_state[0].psi = 0.0
|
||||
self.cur_state[0].curvature = 0.0
|
||||
self.lat_mpc = LateralMpc()
|
||||
self.reset_mpc()
|
||||
|
||||
def reset_mpc(self, x0=np.zeros(6)):
|
||||
self.x0 = x0
|
||||
self.lat_mpc.reset(x0=self.x0)
|
||||
self.desired_curvature = 0.0
|
||||
self.safe_desired_curvature = 0.0
|
||||
self.desired_curvature_rate = 0.0
|
||||
|
|
@ -171,45 +165,40 @@ class LateralPlanner():
|
|||
self.LP.rll_prob *= self.lane_change_ll_prob
|
||||
if self.use_lanelines:
|
||||
d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz)
|
||||
self.libmpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, CP.steerRateCost)
|
||||
self.lat_mpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, CP.steerRateCost)
|
||||
else:
|
||||
d_path_xyz = self.path_xyz
|
||||
path_cost = np.clip(abs(self.path_xyz[0,1]/self.path_xyz_stds[0,1]), 0.5, 5.0) * MPC_COST_LAT.PATH
|
||||
# Heading cost is useful at low speed, otherwise end of plan can be off-heading
|
||||
heading_cost = interp(v_ego, [5.0, 10.0], [MPC_COST_LAT.HEADING, 0.0])
|
||||
self.libmpc.set_weights(path_cost, heading_cost, CP.steerRateCost)
|
||||
self.lat_mpc.set_weights(path_cost, heading_cost, CP.steerRateCost)
|
||||
y_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:,1])
|
||||
heading_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw)
|
||||
self.y_pts = y_pts
|
||||
|
||||
assert len(y_pts) == LAT_MPC_N + 1
|
||||
assert len(heading_pts) == LAT_MPC_N + 1
|
||||
# for now CAR_ROTATION_RADIUS is disabled
|
||||
# to use it, enable it in the MPC
|
||||
assert abs(CAR_ROTATION_RADIUS) < 1e-3
|
||||
self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
|
||||
float(v_ego),
|
||||
CAR_ROTATION_RADIUS,
|
||||
list(y_pts),
|
||||
list(heading_pts))
|
||||
self.x0[4] = v_ego
|
||||
self.lat_mpc.run(self.x0,
|
||||
v_ego,
|
||||
CAR_ROTATION_RADIUS,
|
||||
y_pts,
|
||||
heading_pts)
|
||||
# init state for next
|
||||
self.cur_state.x = 0.0
|
||||
self.cur_state.y = 0.0
|
||||
self.cur_state.psi = 0.0
|
||||
self.cur_state.curvature = interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1], self.mpc_solution.curvature)
|
||||
self.x0[3] = interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1], self.lat_mpc.x_sol[:,3])
|
||||
|
||||
|
||||
# Check for infeasable MPC solution
|
||||
mpc_nans = any(math.isnan(x) for x in self.mpc_solution.curvature)
|
||||
mpc_nans = any(math.isnan(x) for x in self.lat_mpc.x_sol[:,3])
|
||||
t = sec_since_boot()
|
||||
if mpc_nans:
|
||||
self.libmpc.init()
|
||||
self.cur_state.curvature = measured_curvature
|
||||
|
||||
if mpc_nans or self.lat_mpc.solution_status != 0:
|
||||
self.reset_mpc()
|
||||
self.x0[3] = measured_curvature
|
||||
if t > self.last_cloudlog_t + 5.0:
|
||||
self.last_cloudlog_t = t
|
||||
cloudlog.warning("Lateral mpc - nan: True")
|
||||
|
||||
if self.mpc_solution[0].cost > 20000. or mpc_nans: # TODO: find a better way to detect when MPC did not converge
|
||||
if self.lat_mpc.cost > 20000. or mpc_nans:
|
||||
self.solution_invalid_cnt += 1
|
||||
else:
|
||||
self.solution_invalid_cnt = 0
|
||||
|
|
@ -220,9 +209,9 @@ class LateralPlanner():
|
|||
plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'modelV2'])
|
||||
plan_send.lateralPlan.laneWidth = float(self.LP.lane_width)
|
||||
plan_send.lateralPlan.dPathPoints = [float(x) for x in self.y_pts]
|
||||
plan_send.lateralPlan.psis = [float(x) for x in self.mpc_solution.psi[0:CONTROL_N]]
|
||||
plan_send.lateralPlan.curvatures = [float(x) for x in self.mpc_solution.curvature[0:CONTROL_N]]
|
||||
plan_send.lateralPlan.curvatureRates = [float(x) for x in self.mpc_solution.curvature_rate[0:CONTROL_N-1]] +[0.0]
|
||||
plan_send.lateralPlan.psis = [float(x) for x in self.lat_mpc.x_sol[0:CONTROL_N, 2]]
|
||||
plan_send.lateralPlan.curvatures = [float(x) for x in self.lat_mpc.x_sol[0:CONTROL_N,3]]
|
||||
plan_send.lateralPlan.curvatureRates = [float(x) for x in self.lat_mpc.u_sol[0:CONTROL_N-1]] +[0.0]
|
||||
plan_send.lateralPlan.lProb = float(self.LP.lll_prob)
|
||||
plan_send.lateralPlan.rProb = float(self.LP.rll_prob)
|
||||
plan_send.lateralPlan.dProb = float(self.LP.d_prob)
|
||||
|
|
|
|||
|
|
@ -1,102 +1,75 @@
|
|||
import unittest
|
||||
import numpy as np
|
||||
from selfdrive.controls.lib.lateral_mpc import libmpc_py
|
||||
from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc
|
||||
from selfdrive.controls.lib.drive_helpers import LAT_MPC_N, CAR_ROTATION_RADIUS
|
||||
|
||||
|
||||
def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., curvature_init=0.,
|
||||
lane_width=3.6, poly_shift=0.):
|
||||
|
||||
libmpc = libmpc_py.libmpc
|
||||
libmpc.init()
|
||||
libmpc.set_weights(1., 1., 1.)
|
||||
|
||||
|
||||
mpc_solution = libmpc_py.ffi.new("log_t *")
|
||||
lat_mpc = LateralMpc()
|
||||
lat_mpc.set_weights(1., 1., 1.)
|
||||
|
||||
y_pts = poly_shift * np.ones(LAT_MPC_N + 1)
|
||||
heading_pts = np.zeros(LAT_MPC_N + 1)
|
||||
|
||||
|
||||
cur_state = libmpc_py.ffi.new("state_t *")
|
||||
cur_state.x = x_init
|
||||
cur_state.y = y_init
|
||||
cur_state.psi = psi_init
|
||||
cur_state.curvature = curvature_init
|
||||
x0 = np.array([x_init, y_init, psi_init, curvature_init, v_ref, CAR_ROTATION_RADIUS])
|
||||
|
||||
# converge in no more than 20 iterations
|
||||
for _ in range(20):
|
||||
libmpc.run_mpc(cur_state, mpc_solution, v_ref,
|
||||
CAR_ROTATION_RADIUS,
|
||||
list(y_pts), list(heading_pts))
|
||||
|
||||
return mpc_solution
|
||||
lat_mpc.run(x0, v_ref,
|
||||
CAR_ROTATION_RADIUS,
|
||||
y_pts, heading_pts)
|
||||
return lat_mpc.x_sol
|
||||
|
||||
|
||||
class TestLateralMpc(unittest.TestCase):
|
||||
|
||||
def _assert_null(self, sol, curvature=1e-6):
|
||||
for i in range(len(sol[0].y)):
|
||||
self.assertAlmostEqual(sol[0].y[i], 0., delta=curvature)
|
||||
self.assertAlmostEqual(sol[0].psi[i], 0., delta=curvature)
|
||||
self.assertAlmostEqual(sol[0].curvature[i], 0., delta=curvature)
|
||||
for i in range(len(sol)):
|
||||
self.assertAlmostEqual(sol[0,i,1], 0., delta=curvature)
|
||||
self.assertAlmostEqual(sol[0,i,2], 0., delta=curvature)
|
||||
self.assertAlmostEqual(sol[0,i,3], 0., delta=curvature)
|
||||
|
||||
def _assert_simmetry(self, sol, curvature=1e-6):
|
||||
for i in range(len(sol[0][0].y)):
|
||||
self.assertAlmostEqual(sol[0][0].y[i], -sol[1][0].y[i], delta=curvature)
|
||||
self.assertAlmostEqual(sol[0][0].psi[i], -sol[1][0].psi[i], delta=curvature)
|
||||
self.assertAlmostEqual(sol[0][0].curvature[i], -sol[1][0].curvature[i], delta=curvature)
|
||||
self.assertAlmostEqual(sol[0][0].x[i], sol[1][0].x[i], delta=curvature)
|
||||
|
||||
def _assert_identity(self, sol, ignore_y=False, curvature=1e-6):
|
||||
for i in range(len(sol[0][0].y)):
|
||||
self.assertAlmostEqual(sol[0][0].psi[i], sol[1][0].psi[i], delta=curvature)
|
||||
self.assertAlmostEqual(sol[0][0].curvature[i], sol[1][0].curvature[i], delta=curvature)
|
||||
self.assertAlmostEqual(sol[0][0].x[i], sol[1][0].x[i], delta=curvature)
|
||||
if not ignore_y:
|
||||
self.assertAlmostEqual(sol[0][0].y[i], sol[1][0].y[i], delta=curvature)
|
||||
for i in range(len(sol)):
|
||||
self.assertAlmostEqual(sol[0,i,1], -sol[1,i,1], delta=curvature)
|
||||
self.assertAlmostEqual(sol[0,i,2], -sol[1,i,2], delta=curvature)
|
||||
self.assertAlmostEqual(sol[0,i,3], -sol[1,i,3], delta=curvature)
|
||||
self.assertAlmostEqual(sol[0,i,0], sol[1,i,0], delta=curvature)
|
||||
|
||||
def test_straight(self):
|
||||
sol = run_mpc()
|
||||
self._assert_null(sol)
|
||||
self._assert_null(np.array([sol]))
|
||||
|
||||
def test_y_symmetry(self):
|
||||
sol = []
|
||||
for y_init in [-0.5, 0.5]:
|
||||
sol.append(run_mpc(y_init=y_init))
|
||||
self._assert_simmetry(sol)
|
||||
self._assert_simmetry(np.array(sol))
|
||||
|
||||
def test_poly_symmetry(self):
|
||||
sol = []
|
||||
for poly_shift in [-1., 1.]:
|
||||
sol.append(run_mpc(poly_shift=poly_shift))
|
||||
self._assert_simmetry(sol)
|
||||
self._assert_simmetry(np.array(sol))
|
||||
|
||||
def test_curvature_symmetry(self):
|
||||
sol = []
|
||||
for curvature_init in [-0.1, 0.1]:
|
||||
sol.append(run_mpc(curvature_init=curvature_init))
|
||||
self._assert_simmetry(sol)
|
||||
self._assert_simmetry(np.array(sol))
|
||||
|
||||
def test_psi_symmetry(self):
|
||||
sol = []
|
||||
for psi_init in [-0.1, 0.1]:
|
||||
sol.append(run_mpc(psi_init=psi_init))
|
||||
self._assert_simmetry(sol)
|
||||
|
||||
def test_y_shift_vs_poly_shift(self):
|
||||
shift = 1.
|
||||
sol = []
|
||||
sol.append(run_mpc(y_init=shift))
|
||||
sol.append(run_mpc(poly_shift=-shift))
|
||||
# need larger curvature than standard, otherwise it false triggers.
|
||||
# this is acceptable because the 2 cases are very different from the optimizer standpoint
|
||||
self._assert_identity(sol, ignore_y=True, curvature=1e-5)
|
||||
self._assert_simmetry(np.array(sol))
|
||||
|
||||
def test_no_overshoot(self):
|
||||
y_init = 1.
|
||||
sol = run_mpc(y_init=y_init)
|
||||
for y in list(sol[0].y):
|
||||
for y in list(sol[:,1]):
|
||||
self.assertGreaterEqual(y_init, abs(y))
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -1 +1 @@
|
|||
0ada8ee251248f041ed8092abd9b5368f57bbf6e
|
||||
a256ade033538eeead4f34ad7b12be25237ae443
|
||||
|
|
@ -23,7 +23,7 @@ PROCS = {
|
|||
"selfdrive.controls.controlsd": 50.0,
|
||||
"./loggerd": 45.0,
|
||||
"./locationd": 9.1,
|
||||
"selfdrive.controls.plannerd": 20.0,
|
||||
"selfdrive.controls.plannerd": 27.0,
|
||||
"./_ui": 15.0,
|
||||
"selfdrive.locationd.paramsd": 9.1,
|
||||
"./camerad": 7.07,
|
||||
|
|
@ -53,9 +53,9 @@ if TICI:
|
|||
PROCS.update({
|
||||
"./loggerd": 60.0,
|
||||
"selfdrive.controls.controlsd": 28.0,
|
||||
"selfdrive.controls.plannerd": 15.0,
|
||||
"./camerad": 31.0,
|
||||
"./_ui": 21.0,
|
||||
"selfdrive.controls.plannerd": 12.0,
|
||||
"selfdrive.locationd.paramsd": 5.0,
|
||||
"./_dmonitoringmodeld": 10.0,
|
||||
"selfdrive.thermald.thermald": 1.5,
|
||||
|
|
|
|||
|
|
@ -50,6 +50,7 @@ RUN mkdir -p $HOME/openpilot
|
|||
COPY SConstruct $HOME/openpilot/
|
||||
|
||||
COPY ./phonelibs $HOME/openpilot/phonelibs
|
||||
COPY ./pyextra $HOME/openpilot/pyextra
|
||||
COPY ./site_scons $HOME/openpilot/site_scons
|
||||
COPY ./rednose $HOME/openpilot/rednose
|
||||
COPY ./common $HOME/openpilot/common
|
||||
|
|
|
|||
Loading…
Reference in New Issue