acados long merged (#22224)
* 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
commatwo_master
HaraldSchafer
4 years ago
committed by
GitHub
parent
654695809f
commit
2b470f4e38
13 changed files with 300 additions and 447 deletions
@ -1,2 +0,0 @@ |
|||||||
acados_ocp_lead.json |
|
||||||
c_generated_code/ |
|
||||||
@ -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']) |
|
||||||
@ -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) |
|
||||||
@ -1 +1 @@ |
|||||||
8c733450bb28bcdb11d6b9991c8784e1f720f7b2 |
2282e3f208438237fe61d7bf636d6ad6b507c571 |
||||||
Loading…
Reference in new issue