J3ff/IQ.Pilot
1
0
Fork 0
forked from IQ.Lvbs/IQ.Pilot
Code Pull Requests Activity

IQ.Pilot Prebuilt Release @ ab07000

Browse Source
This commit is contained in:
IQ.Lvbs history cleanup
2026-08-22 23:42:42 -05:00
commit 9f9c9a70cc
3729 changed files with 778697 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
selfdrive/controls/lib/longitudinal_mpc_lib/.gitignore vendored Normal file
View File

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

0
selfdrive/controls/lib/longitudinal_mpc_lib/__init__.py Normal file
View File

483
selfdrive/controls/lib/longitudinal_mpc_lib/acados_ocp_long.json Normal file
View File

@@ -0,0 +1,483 @@
{
"acados_include_path": "/data/openpilot/third_party/acados/include",
"acados_lib_path": "/data/openpilot/third_party/acados/lib",
"code_export_directory": "/data/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code",
"constraints": {
"C": [],
"C_e": [],
"D": [],
"constr_type": "BGH",
"constr_type_e": "BGH",
"idxbu": [],
"idxbx": [],
"idxbx_0": [
0,
1,
2
],
"idxbx_e": [],
"idxbxe_0": [
0,
1,
2
],
"idxsbu": [],
"idxsbx": [],
"idxsbx_e": [],
"idxsg": [],
"idxsg_e": [],
"idxsh": [
0,
1,
2,
3
],
"idxsh_e": [],
"idxsphi": [],
"idxsphi_e": [],
"lbu": [],
"lbx": [],
"lbx_0": [
0.0,
0.0,
0.0
],
"lbx_e": [],
"lg": [],
"lg_e": [],
"lh": [
0.0,
0.0,
0.0,
0.0
],
"lh_e": [],
"lphi": [],
"lphi_e": [],
"lsbu": [],
"lsbx": [],
"lsbx_e": [],
"lsg": [],
"lsg_e": [],
"lsh": [
0.0,
0.0,
0.0,
0.0
],
"lsh_e": [],
"lsphi": [],
"lsphi_e": [],
"ubu": [],
"ubx": [],
"ubx_0": [
0.0,
0.0,
0.0
],
"ubx_e": [],
"ug": [],
"ug_e": [],
"uh": [
10000.0,
10000.0,
10000.0,
10000.0
],
"uh_e": [],
"uphi": [],
"uphi_e": [],
"usbu": [],
"usbx": [],
"usbx_e": [],
"usg": [],
"usg_e": [],
"ush": [
0.0,
0.0,
0.0,
0.0
],
"ush_e": [],
"usphi": [],
"usphi_e": []
},
"cost": {
"Vu": [],
"Vu_0": [],
"Vx": [],
"Vx_0": [],
"Vx_e": [],
"Vz": [],
"Vz_0": [],
"W": [
[
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0
]
],
"W_0": [
[
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0
]
],
"W_e": [
[
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0
]
],
"Zl": [
0.0,
0.0,
0.0,
0.0
],
"Zl_e": [],
"Zu": [
0.0,
0.0,
0.0,
0.0
],
"Zu_e": [],
"cost_ext_fun_type": "casadi",
"cost_ext_fun_type_0": "casadi",
"cost_ext_fun_type_e": "casadi",
"cost_type": "NONLINEAR_LS",
"cost_type_0": "NONLINEAR_LS",
"cost_type_e": "NONLINEAR_LS",
"yref": [
0.0,
0.0,
0.0,
0.0,
0.0
],
"yref_0": [
0.0,
0.0,
0.0,
0.0,
0.0
],
"yref_e": [
0.0,
0.0,
0.0,
0.0
],
"zl": [
0.0,
0.0,
0.0,
0.0
],
"zl_e": [],
"zu": [
0.0,
0.0,
0.0,
0.0
],
"zu_e": []
},
"cython_include_dirs": [
"/data/openpilot/.venv/lib/python3.12/site-packages/numpy/_core/include",
"/usr/include/python3.12"
],
"dims": {
"N": 12,
"nbu": 0,
"nbx": 0,
"nbx_0": 3,
"nbx_e": 0,
"nbxe_0": 3,
"ng": 0,
"ng_e": 0,
"nh": 4,
"nh_e": 0,
"np": 5,
"nphi": 0,
"nphi_e": 0,
"nr": 0,
"nr_e": 0,
"ns": 4,
"ns_e": 0,
"nsbu": 0,
"nsbx": 0,
"nsbx_e": 0,
"nsg": 0,
"nsg_e": 0,
"nsh": 4,
"nsh_e": 0,
"nsphi": 0,
"nsphi_e": 0,
"nu": 1,
"nx": 3,
"ny": 5,
"ny_0": 5,
"ny_e": 4,
"nz": 0
},
"json_file": "/data/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/acados_ocp_long.json",
"model": {
"con_h_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegiaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegpcaaaaaaaaaaaaaafaaaaaaabgpfngjgogegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpfngbgihchcaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaacbaaaaaamgfgbgegpfegbgoghgfgchpfggbgdgehpgchegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaachbaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgfaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhchbaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgdaaaaaaaegbaaaaaaaaaaaaaaachbaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaa",
"con_h_expr_e": null,
"con_phi_expr": null,
"con_phi_expr_e": null,
"con_r_expr": null,
"con_r_expr_e": null,
"con_r_in_phi": null,
"con_r_in_phi_e": null,
"cost_conl_custom_outer_hess": null,
"cost_conl_custom_outer_hess_0": null,
"cost_conl_custom_outer_hess_e": null,
"cost_expr_ext_cost": null,
"cost_expr_ext_cost_0": null,
"cost_expr_ext_cost_custom_hess": null,
"cost_expr_ext_cost_custom_hess_0": null,
"cost_expr_ext_cost_custom_hess_e": null,
"cost_expr_ext_cost_e": null,
"cost_psi_expr": null,
"cost_psi_expr_0": null,
"cost_psi_expr_e": null,
"cost_r_in_psi_expr": null,
"cost_r_in_psi_expr_0": null,
"cost_r_in_psi_expr_e": null,
"cost_y_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegjaaaaaaaaaaaaaaafaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegmcaaaaaaaaaaaaaajgfaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhcheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgdaaaaaaaegbaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaachcaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegpcaaaaaaaaaaaaaafaaaaaaakgpffghgpg",
"cost_y_expr_0": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegjaaaaaaaaaaaaaaafaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegmcaaaaaaaaaaaaaajgfaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhcheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgdaaaaaaaegbaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaachcaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegpcaaaaaaaaaaaaaafaaaaaaakgpffghgpg",
"cost_y_expr_e": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegiaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegmcaaaaaaaaaaaaaajgfaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhcheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgdaaaaaaaegbaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaachcaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpg",
"disc_dyn_expr": null,
"dyn_disc_fun": null,
"dyn_disc_fun_jac": null,
"dyn_disc_fun_jac_hess": null,
"dyn_ext_fun_type": "casadi",
"dyn_generic_source": null,
"f_expl_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaeghaaaaaaaaaaaaaaadaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegpcaaaaaaaaaaaaaafaaaaaaakgpffghgpg",
"f_impl_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaeghaaaaaaaaaaaaaaadaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaajaaaaaaaihpffghgpgpfegpgehegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaajaaaaaaaghpffghgpgpfegpgehegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaajaaaaaaabgpffghgpgpfegpgehegpcaaaaaaaaaaaaaafaaaaaaakgpffghgpg",
"gnsf": {
"nontrivial_f_LO": 1,
"purely_linear": 0
},
"name": "long",
"p": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegjaaaaaaaaaaaaaaafaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpfngjgogegpcaaaaaaaaaaaaaafaaaaaaabgpfngbgihegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhegpcaaaaaaaaaaaaaacbaaaaaamgfgbgegpfegbgoghgfgchpfggbgdgehpgch",
"u": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegfaaaaaaaaaaaaaaabaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaakgpffghgpg",
"x": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaeghaaaaaaaaaaaaaaadaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpg",
"xdot": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaeghaaaaaaaaaaaaaaadaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaajaaaaaaaihpffghgpgpfegpgehegpcaaaaaaaaaaaaaajaaaaaaaghpffghgpgpfegpgehegpcaaaaaaaaaaaaaajaaaaaaabgpffghgpgpfegpgeh",
"z": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa"
},
"parameter_values": [
-1.2,
1.2,
0.0,
1.45,
0.75
],
"problem_class": "OCP",
"shared_lib_ext": ".so",
"solver_options": {
"Tsim": 0.06944444444444445,
"alpha_min": 0.05,
"alpha_reduction": 0.7,
"collocation_type": "GAUSS_LEGENDRE",
"custom_templates": [],
"custom_update_copy": true,
"custom_update_filename": "",
"custom_update_header_filename": "",
"eps_sufficient_descent": 0.0001,
"exact_hess_constr": 1,
"exact_hess_cost": 1,
"exact_hess_dyn": 1,
"ext_cost_num_hess": 0,
"ext_fun_compile_flags": "-O2",
"full_step_dual": 0,
"globalization": "FIXED_STEP",
"globalization_use_SOC": 0,
"hessian_approx": "GAUSS_NEWTON",
"hpipm_mode": "BALANCE",
"initialize_t_slacks": 0,
"integrator_type": "ERK",
"levenberg_marquardt": 0.0,
"line_search_use_sufficient_descent": 0,
"model_external_shared_lib_dir": null,
"model_external_shared_lib_name": null,
"nlp_solver_ext_qp_res": 0,
"nlp_solver_max_iter": 100,
"nlp_solver_step_length": 1.0,
"nlp_solver_tol_comp": 1e-06,
"nlp_solver_tol_eq": 1e-06,
"nlp_solver_tol_ineq": 1e-06,
"nlp_solver_tol_stat": 1e-06,
"nlp_solver_type": "SQP_RTI",
"print_level": 0,
"qp_solver": "PARTIAL_CONDENSING_HPIPM",
"qp_solver_cond_N": 1,
"qp_solver_cond_ric_alg": 1,
"qp_solver_iter_max": 10,
"qp_solver_ric_alg": 1,
"qp_solver_tol_comp": 0.001,
"qp_solver_tol_eq": 0.001,
"qp_solver_tol_ineq": 0.001,
"qp_solver_tol_stat": 0.001,
"qp_solver_warm_start": 0,
"regularize_method": null,
"shooting_nodes": [
0.0,
0.06944444444444445,
0.2777777777777778,
0.625,
1.1111111111111112,
1.7361111111111114,
2.5,
3.4027777777777786,
4.444444444444445,
5.625,
6.9444444444444455,
8.402777777777777,
10.0
],
"sim_method_jac_reuse": [
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0
],
"sim_method_newton_iter": 3,
"sim_method_newton_tol": 0.0,
"sim_method_num_stages": [
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4
],
"sim_method_num_steps": [
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1
],
"tf": 10.0,
"time_steps": [
0.06944444444444445,
0.20833333333333334,
0.3472222222222222,
0.48611111111111116,
0.6250000000000002,
0.7638888888888886,
0.9027777777777786,
1.041666666666666,
1.1805555555555554,
1.3194444444444455,
1.4583333333333313,
1.5972222222222232
]
}
}

213
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/Makefile Normal file
View File

@@ -0,0 +1,213 @@
#
# Copyright (c) The acados authors.
#
# This file is part of acados.
#
# The 2-Clause BSD License
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# 1. Redistributions of source code must retain the above copyright notice,
# this list of conditions and the following disclaimer.
#
# 2. Redistributions in binary form must reproduce the above copyright notice,
# this list of conditions and the following disclaimer in the documentation
# and/or other materials provided with the distribution.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.;
#
# define sources and use make's implicit rules to generate object files (*.o)
# model
MODEL_SRC=
MODEL_SRC+= long_model/long_expl_ode_fun.c
MODEL_SRC+= long_model/long_expl_vde_forw.c
MODEL_SRC+= long_model/long_expl_vde_adj.c
MODEL_OBJ := $(MODEL_SRC:.c=.o)
# optimal control problem - mostly CasADi exports
OCP_SRC=
OCP_SRC+= long_constraints/long_constr_h_fun_jac_uxt_zt.c
OCP_SRC+= long_constraints/long_constr_h_fun.c
OCP_SRC+= long_cost/long_cost_y_0_fun.c
OCP_SRC+= long_cost/long_cost_y_0_fun_jac_ut_xt.c
OCP_SRC+= long_cost/long_cost_y_0_hess.c
OCP_SRC+= long_cost/long_cost_y_fun.c
OCP_SRC+= long_cost/long_cost_y_fun_jac_ut_xt.c
OCP_SRC+= long_cost/long_cost_y_hess.c
OCP_SRC+= long_cost/long_cost_y_e_fun.c
OCP_SRC+= long_cost/long_cost_y_e_fun_jac_ut_xt.c
OCP_SRC+= long_cost/long_cost_y_e_hess.c
OCP_SRC+= acados_solver_long.c
OCP_OBJ := $(OCP_SRC:.c=.o)
# for sim solver
SIM_SRC= acados_sim_solver_long.c
SIM_OBJ := $(SIM_SRC:.c=.o)
# for target example
EX_SRC= main_long.c
EX_OBJ := $(EX_SRC:.c=.o)
EX_EXE := $(EX_SRC:.c=)
# for target example_sim
EX_SIM_SRC= main_sim_long.c
EX_SIM_OBJ := $(EX_SIM_SRC:.c=.o)
EX_SIM_EXE := $(EX_SIM_SRC:.c=)
# combine model, sim and ocp object files
OBJ=
OBJ+= $(MODEL_OBJ)
OBJ+= $(SIM_OBJ)
OBJ+= $(OCP_OBJ)
EXTERNAL_DIR=
EXTERNAL_LIB=
INCLUDE_PATH = /data/openpilot/third_party/acados/include
LIB_PATH = /data/openpilot/third_party/acados/lib
# preprocessor flags for make's implicit rules
CPPFLAGS+= -I$(INCLUDE_PATH)
CPPFLAGS+= -I$(INCLUDE_PATH)/acados
CPPFLAGS+= -I$(INCLUDE_PATH)/blasfeo/include
CPPFLAGS+= -I$(INCLUDE_PATH)/hpipm/include
# define the c-compiler flags for make's implicit rules
CFLAGS = -fPIC -std=c99 -O2#-fno-diagnostics-show-line-numbers -g
# # Debugging
# CFLAGS += -g3
# linker flags
LDFLAGS+= -L$(LIB_PATH)
# link to libraries
LDLIBS+= -lacados
LDLIBS+= -lhpipm
LDLIBS+= -lblasfeo
LDLIBS+= -lm
LDLIBS+=
# libraries
LIBACADOS_SOLVER=libacados_solver_long.so
LIBACADOS_OCP_SOLVER=libacados_ocp_solver_long.so
LIBACADOS_SIM_SOLVER=lib$(SIM_SRC:.c=.so)
# virtual targets
.PHONY : all clean
#all: clean example_sim example shared_lib
all: clean example_sim example
shared_lib: bundled_shared_lib ocp_shared_lib sim_shared_lib
# some linker targets
example: $(EX_OBJ) $(OBJ)
$(CC) $^ -o $(EX_EXE) $(LDFLAGS) $(LDLIBS)
example_sim: $(EX_SIM_OBJ) $(MODEL_OBJ) $(SIM_OBJ)
$(CC) $^ -o $(EX_SIM_EXE) $(LDFLAGS) $(LDLIBS)
bundled_shared_lib: $(OBJ)
$(CC) -shared $^ -o $(LIBACADOS_SOLVER) $(LDFLAGS) $(LDLIBS)
ocp_shared_lib: $(OCP_OBJ) $(MODEL_OBJ)
$(CC) -shared $^ -o $(LIBACADOS_OCP_SOLVER) $(LDFLAGS) $(LDLIBS) \
-L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB)
sim_shared_lib: $(SIM_OBJ) $(MODEL_OBJ)
$(CC) -shared $^ -o $(LIBACADOS_SIM_SOLVER) $(LDFLAGS) $(LDLIBS)
# Cython targets
ocp_cython_c: ocp_shared_lib
cython \
-o acados_ocp_solver_pyx.c \
-I $(INCLUDE_PATH)/../interfaces/acados_template/acados_template \
$(INCLUDE_PATH)/../interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx \
-I /data/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code \
ocp_cython_o: ocp_cython_c
$(CC) $(ACADOS_FLAGS) -c -O2 \
-fPIC \
-o acados_ocp_solver_pyx.o \
-I $(INCLUDE_PATH)/blasfeo/include/ \
-I $(INCLUDE_PATH)/hpipm/include/ \
-I $(INCLUDE_PATH) \
-I /data/openpilot/.venv/lib/python3.12/site-packages/numpy/_core/include \
-I /usr/include/python3.12 \
acados_ocp_solver_pyx.c \
ocp_cython: ocp_cython_o
$(CC) $(ACADOS_FLAGS) -shared \
-o acados_ocp_solver_pyx.so \
-Wl,-rpath=$(LIB_PATH) \
acados_ocp_solver_pyx.o \
$(abspath .)/libacados_ocp_solver_long.so \
$(LDFLAGS) $(LDLIBS)
# Sim Cython targets
sim_cython_c: sim_shared_lib
cython \
-o acados_sim_solver_pyx.c \
-I $(INCLUDE_PATH)/../interfaces/acados_template/acados_template \
$(INCLUDE_PATH)/../interfaces/acados_template/acados_template/acados_sim_solver_pyx.pyx \
-I /data/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code \
sim_cython_o: sim_cython_c
$(CC) $(ACADOS_FLAGS) -c -O2 \
-fPIC \
-o acados_sim_solver_pyx.o \
-I $(INCLUDE_PATH)/blasfeo/include/ \
-I $(INCLUDE_PATH)/hpipm/include/ \
-I $(INCLUDE_PATH) \
-I /data/openpilot/.venv/lib/python3.12/site-packages/numpy/_core/include \
-I /usr/include/python3.12 \
acados_sim_solver_pyx.c \
sim_cython: sim_cython_o
$(CC) $(ACADOS_FLAGS) -shared \
-o acados_sim_solver_pyx.so \
-Wl,-rpath=$(LIB_PATH) \
acados_sim_solver_pyx.o \
$(abspath .)/libacados_sim_solver_long.so \
$(LDFLAGS) $(LDLIBS)
clean:
$(RM) $(OBJ) $(EX_OBJ) $(EX_SIM_OBJ)
$(RM) $(LIBACADOS_SOLVER) $(LIBACADOS_OCP_SOLVER) $(LIBACADOS_SIM_SOLVER)
$(RM) $(EX_EXE) $(EX_SIM_EXE)
clean_ocp_shared_lib:
$(RM) $(LIBACADOS_OCP_SOLVER)
$(RM) $(OCP_OBJ)
clean_ocp_cython:
$(RM) libacados_ocp_solver_long.so
$(RM) acados_solver_long.o
$(RM) acados_ocp_solver_pyx.so
$(RM) acados_ocp_solver_pyx.o
clean_sim_cython:
$(RM) libacados_sim_solver_long.so
$(RM) acados_sim_solver_long.o
$(RM) acados_sim_solver_pyx.so
$(RM) acados_sim_solver_pyx.o

BIN
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_ocp_solver_pyx.so Executable file
View File

Binary file not shown.

BIN
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/libacados_ocp_solver_long.so Executable file
View File

Binary file not shown.

3
selfdrive/controls/lib/longitudinal_mpc_lib/gen.log Normal file
View File

@@ -0,0 +1,3 @@
Warning: Please note that the following versions of CasADi are officially supported: 3.5.6 or 3.5.5 or 3.5.4 or 3.5.3 or 3.5.2 or 3.5.1 or 3.4.5 or 3.4.0.
If there is an incompatibility with the CasADi generated code, please consider changing your CasADi version.
Version 3.7.2 currently in use.

429
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py Executable file
View File

@@ -0,0 +1,429 @@
#!/usr/bin/env python3
import os
import time
import numpy as np
from cereal import log
from iqdbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
from openpilot.common.realtime import DT_MDL
from openpilot.common.swaglog import cloudlog
# WARNING: imports outside of constants will not trigger a rebuild
from openpilot.selfdrive.modeld.constants import index_function, ModelConstants
from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU # legacy lead extrapolation (newLeadMpc=False)
from openpilot.common.params import Params, UnknownKeyName
LEAD_T_IDXS_MODEL = np.array(ModelConstants.LEAD_T_IDXS) # [0, 2, 4, 6, 8, 10]s
if __name__ == '__main__': # generating code
from openpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
else:
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython
from casadi import SX, vertcat
MODEL_NAME = 'long'
LONG_MPC_DIR = os.path.dirname(os.path.abspath(__file__))
EXPORT_DIR = os.path.join(LONG_MPC_DIR, "c_generated_code")
JSON_FILE = os.path.join(LONG_MPC_DIR, "acados_ocp_long.json")
LongitudinalPlanSource = log.LongitudinalPlan.LongitudinalPlanSource
MPC_SOURCES = (LongitudinalPlanSource.lead0, LongitudinalPlanSource.lead1)
X_DIM = 3
U_DIM = 1
PARAM_DIM = 5
COST_E_DIM = 4
COST_DIM = COST_E_DIM + 1
CONSTR_DIM = 4
X_EGO_OBSTACLE_COST = 3.
X_EGO_COST = 0.
V_EGO_COST = 0.
A_EGO_COST = 0.
J_EGO_COST = 20.
DANGER_ZONE_COST = 100.
CRASH_DISTANCE = .25
LEAD_DANGER_FACTOR = 0.75
LIMIT_COST = 1e6
ACADOS_SOLVER_TYPE = 'SQP_RTI'
# Fewer timestamps don't hurt performance and lead to
# much better convergence of the MPC with low iterations
N = 12
MAX_T = 10.0
T_IDXS_LST = [index_function(idx, max_val=MAX_T, max_idx=N) for idx in range(N+1)]
T_IDXS = np.array(T_IDXS_LST)
FCW_IDXS = T_IDXS < 5.0
T_DIFFS = np.diff(T_IDXS, prepend=[0.])
COMFORT_BRAKE = 2.5
STOP_DISTANCE = 3.0
MIN_X_LEAD_FACTOR = 0.5
LEAD_PULLAWAY_VREL = 0.5
LEAD_PULLAWAY_ABRAKE = -0.5
def get_jerk_factor(personality=log.LongitudinalPersonality.standard):
if personality==log.LongitudinalPersonality.relaxed:
return 1.0
elif personality==log.LongitudinalPersonality.standard:
return 1.0
elif personality==log.LongitudinalPersonality.aggressive:
return 0.5
else:
raise NotImplementedError("Longitudinal personality not supported")
def get_T_FOLLOW(personality=log.LongitudinalPersonality.standard):
if personality==log.LongitudinalPersonality.relaxed:
return 1.75
elif personality==log.LongitudinalPersonality.standard:
return 1.45
elif personality==log.LongitudinalPersonality.aggressive:
return 1.25
else:
raise NotImplementedError("Longitudinal personality not supported")
def get_stopped_equivalence_factor(v_lead):
return (v_lead**2) / (2 * COMFORT_BRAKE)
def get_safe_obstacle_distance(v_ego, t_follow):
return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + STOP_DISTANCE
def gen_long_model():
model = AcadosModel()
model.name = MODEL_NAME
# states
x_ego, v_ego, a_ego = SX.sym('x_ego'), SX.sym('v_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
a_min = SX.sym('a_min')
a_max = SX.sym('a_max')
x_obstacle = SX.sym('x_obstacle')
lead_t_follow = SX.sym('lead_t_follow')
lead_danger_factor = SX.sym('lead_danger_factor')
model.p = vertcat(a_min, a_max, x_obstacle, lead_t_follow, lead_danger_factor)
# 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_ocp():
ocp = AcadosOcp()
ocp.model = gen_long_model()
Tf = T_IDXS[-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.zeros((COST_DIM, COST_DIM))
Q = np.zeros((COST_E_DIM, COST_E_DIM))
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]
a_min, a_max = ocp.model.p[0], ocp.model.p[1]
x_obstacle = ocp.model.p[2]
lead_t_follow = ocp.model.p[3]
lead_danger_factor = ocp.model.p[4]
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, lead_t_follow)
# The main cost in normal operation is how close you are to the "desired" distance
# from an obstacle at every timestep. This obstacle can be a lead car
# or other object. In e2e mode we can use x_position targets as a cost
# instead.
costs = [((x_obstacle - x_ego) - (desired_dist_comfort)) / (v_ego + 10.),
x_ego,
v_ego,
a_ego,
j_ego]
ocp.model.cost_y_expr = vertcat(*costs)
ocp.model.cost_y_expr_e = vertcat(*costs[:-1])
# Constraints on speed, acceleration and desired distance to
# the obstacle, which is treated as a slack constraint so it
# behaves like an asymmetrical cost.
constraints = vertcat(v_ego,
(a_ego - a_min),
(a_max - a_ego),
((x_obstacle - x_ego) - lead_danger_factor * (desired_dist_comfort)) / (v_ego + 10.))
ocp.model.con_h_expr = constraints
x0 = np.zeros(X_DIM)
ocp.constraints.x0 = x0
ocp.parameter_values = np.array([-1.2, 1.2, 0.0, get_T_FOLLOW(), LEAD_DANGER_FACTOR])
# We put all constraint cost weights to 0 and only set them at runtime
cost_weights = np.zeros(CONSTR_DIM)
ocp.cost.zl = cost_weights
ocp.cost.Zl = cost_weights
ocp.cost.Zu = cost_weights
ocp.cost.zu = cost_weights
ocp.constraints.lh = np.zeros(CONSTR_DIM)
ocp.constraints.uh = 1e4*np.ones(CONSTR_DIM)
ocp.constraints.idxsh = np.arange(CONSTR_DIM)
# The HPIPM solver can give decent solutions even when it is stopped early
# Which is critical for our purpose where compute time is strictly bounded
# We use HPIPM in the SPEED_ABS mode, which ensures fastest runtime. This
# does not cause issues since the problem is well bounded.
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 = ACADOS_SOLVER_TYPE
ocp.solver_options.qp_solver_cond_N = 1
# More iterations take too much time and less lead to inaccurate convergence in
# some situations. Ideally we would run just 1 iteration to ensure fixed runtime.
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 = T_IDXS
ocp.code_export_directory = EXPORT_DIR
return ocp
class LongitudinalMpc:
def __init__(self, dt=DT_MDL):
self.dt = dt
self._params = Params()
self.new_lead_mpc = self._read_new_lead_mpc()
self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
self.reset()
self.source = LongitudinalPlanSource.cruise
def _read_new_lead_mpc(self) -> bool:
try:
return self._params.get_bool("newLeadMpc")
except UnknownKeyName:
return True
def reset(self):
self.solver.reset()
self.x_sol = np.zeros((N+1, X_DIM))
self.u_sol = np.zeros((N, 1))
self.v_solution = np.zeros(N+1)
self.a_solution = np.zeros(N+1)
self.j_solution = np.zeros(N)
self.yref = np.zeros((N+1, COST_DIM))
for i in range(N):
self.solver.cost_set(i, "yref", self.yref[i])
self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM])
self.params = np.zeros((N+1, PARAM_DIM))
for i in range(N+1):
self.solver.set(i, 'x', np.zeros(X_DIM))
self.last_cloudlog_t = 0
self.status = False
self.crash_cnt = 0.0
self.solution_status = 0
# timers
self.solve_time = 0.0
self.time_qp_solution = 0.0
self.time_linearization = 0.0
self.time_integrator = 0.0
self.x0 = np.zeros(X_DIM)
self.lead_xv_0 = np.zeros((N+1, 2))
self.lead_xv_1 = np.zeros((N+1, 2))
self.set_weights()
def set_cost_weights(self, cost_weights, constraint_cost_weights):
W = np.asfortranarray(np.diag(cost_weights))
for i in range(N):
self.solver.cost_set(i, 'W', W)
# Setting the slice without the copy make the array not contiguous,
# causing issues with the C interface.
self.solver.cost_set(N, 'W', np.copy(W[:COST_E_DIM, :COST_E_DIM]))
# Set L2 slack cost on lower bound constraints
Zl = np.array(constraint_cost_weights)
for i in range(N):
self.solver.cost_set(i, 'Zl', Zl)
def set_weights(self, personality=log.LongitudinalPersonality.standard):
jerk_factor = get_jerk_factor(personality)
cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * J_EGO_COST]
constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST]
self.set_cost_weights(cost_weights, constraint_cost_weights)
def set_cur_state(self, v, a):
v_prev = self.x0[1]
self.x0[1] = v
self.x0[2] = a
if abs(v_prev - v) > 2.: # probably only helps if v < v_prev
for i in range(N+1):
self.solver.set(i, 'x', self.x0)
@staticmethod
def extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau):
a_lead_traj = a_lead * np.exp(-a_lead_tau * (T_IDXS**2)/2.)
v_lead_traj = np.clip(v_lead + np.cumsum(T_DIFFS * a_lead_traj), 0.0, 1e8)
x_lead_traj = x_lead + np.cumsum(T_DIFFS * v_lead_traj)
lead_xv = np.column_stack((x_lead_traj, v_lead_traj))
return lead_xv
def process_lead_legacy(self, lead):
# behavior before PR #37824 (newLeadMpc=False): one immediate radar lead prediction
# extrapolated forward with acceleration decaying to 0
v_ego = self.x0[1]
if lead is not None and lead.status:
x_lead = lead.dRel
v_lead = lead.vLead
a_lead = lead.aLeadK
a_lead_tau = lead.aLeadTau
else:
# 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
a_lead_tau = _LEAD_ACCEL_TAU
# MPC will not converge if immediate crash is expected
# Clip lead distance to what is still possible to brake for
min_x_lead = MIN_X_LEAD_FACTOR * (v_ego + v_lead) * (v_ego - v_lead) / (-ACCEL_MIN * 2)
x_lead = np.clip(x_lead, min_x_lead, 1e8)
v_lead = np.clip(v_lead, 0.0, 1e8)
a_lead = np.clip(a_lead, -10., 5.)
lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau)
return lead_xv
def process_lead(self, model_lead, radar_lead):
v_ego = self.x0[1]
if model_lead.prob > 0.5 and radar_lead.status:
# Anchor at radar's trusted h=0, use model's delta for h>0. On radarless, radarState
# is synthesized from the model (radard.get_RadarState_from_vision), so this collapses
# to `x - RADAR_TO_CAMERA` and `v_ego + (model.v - model_v_ego)` — identical to the
# prior formula. On radar cars, real radar measurements anchor the trajectory.
x_lead_traj = float(radar_lead.dRel) + (np.asarray(model_lead.x, dtype=np.float64) - model_lead.x[0])
v_lead_traj = float(radar_lead.vLead) + (np.asarray(model_lead.v, dtype=np.float64) - model_lead.v[0])
else:
# Fake a fast lead so MPC stays in the same mode.
x_lead_traj = 50.0 + (v_ego + 10.0) * LEAD_T_IDXS_MODEL
v_lead_traj = np.full_like(LEAD_T_IDXS_MODEL, v_ego + 10.0)
# MPC won't converge on immediate crashes; lift h=0 to the minimum braking distance.
v_lead_0 = v_lead_traj[0]
min_x_lead = MIN_X_LEAD_FACTOR * (v_ego + v_lead_0) * (v_ego - v_lead_0) / (-ACCEL_MIN * 2)
x_lead_traj[0] = max(x_lead_traj[0], min_x_lead)
v_lead_traj = np.clip(v_lead_traj, 0.0, 1e8)
x_lead_mpc = np.maximum.accumulate(np.interp(T_IDXS, LEAD_T_IDXS_MODEL, x_lead_traj))
v_lead_mpc = np.interp(T_IDXS, LEAD_T_IDXS_MODEL, v_lead_traj)
if radar_lead.status and radar_lead.vRel > LEAD_PULLAWAY_VREL and radar_lead.aLeadK > LEAD_PULLAWAY_ABRAKE:
# ty spysyweeb for lead pull away fix phantom launch braking so you don't ram the lead in edge cases.
radar_velocity_floor = np.full_like(T_IDXS, float(radar_lead.vLead))
radar_distance_floor = float(radar_lead.dRel) + float(radar_lead.vLead) * T_IDXS
v_lead_mpc = np.maximum(v_lead_mpc, radar_velocity_floor)
x_lead_mpc = np.maximum(x_lead_mpc, radar_distance_floor)
return np.column_stack((x_lead_mpc, v_lead_mpc))
def update(self, modelV2, radarstate, personality=log.LongitudinalPersonality.standard):
self.new_lead_mpc = self._read_new_lead_mpc()
t_follow = get_T_FOLLOW(personality)
model_leads = modelV2.leadsV3
if self.new_lead_mpc:
# PR #37824: use the model's full predicted lead horizon
self.status = model_leads[0].prob > 0.5 or model_leads[1].prob > 0.5
lead_xv_0 = self.process_lead(model_leads[0], radarstate.leadOne)
lead_xv_1 = self.process_lead(model_leads[1], radarstate.leadTwo)
else:
# pre-PR behavior: radar lead extrapolated with accel decay
self.status = radarstate.leadOne.status or radarstate.leadTwo.status
lead_xv_0 = self.process_lead_legacy(radarstate.leadOne)
lead_xv_1 = self.process_lead_legacy(radarstate.leadTwo)
self.lead_xv_0 = lead_xv_0
self.lead_xv_1 = lead_xv_1
# To estimate a safe distance from a moving lead, we calculate how much stopping
# distance that lead needs as a minimum. We can add that to the current distance
# and then treat that as a stopped car/obstacle at this new distance.
lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1])
lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1])
x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle])
self.source = MPC_SOURCES[np.argmin(x_obstacles[0])]
self.yref[:,:] = 0.0
for i in range(N):
self.solver.set(i, "yref", self.yref[i])
self.solver.set(N, "yref", self.yref[N][:COST_E_DIM])
self.params[:,0] = ACCEL_MIN
self.params[:,1] = ACCEL_MAX
self.params[:,2] = np.min(x_obstacles, axis=1)
self.params[:,3] = t_follow
self.params[:,4] = LEAD_DANGER_FACTOR
self.run()
lead_crash_prob = model_leads[0].prob if self.new_lead_mpc else radarstate.leadOne.modelProb
if (np.any(lead_xv_0[FCW_IDXS,0] - self.x_sol[FCW_IDXS,0] < CRASH_DISTANCE) and
lead_crash_prob > 0.9):
self.crash_cnt += 1
else:
self.crash_cnt = 0
def run(self):
for i in range(N+1):
self.solver.set(i, 'p', self.params[i])
self.solver.constraints_set(0, "lbx", self.x0)
self.solver.constraints_set(0, "ubx", self.x0)
self.solution_status = self.solver.solve()
self.solve_time = float(self.solver.get_stats('time_tot')[0])
self.time_qp_solution = float(self.solver.get_stats('time_qp')[0])
self.time_linearization = float(self.solver.get_stats('time_lin')[0])
self.time_integrator = float(self.solver.get_stats('time_sim')[0])
for i in range(N+1):
self.x_sol[i] = self.solver.get(i, 'x')
for i in range(N):
self.u_sol[i] = self.solver.get(i, 'u')
self.v_solution = self.x_sol[:,1]
self.a_solution = self.x_sol[:,2]
self.j_solution = self.u_sol[:,0]
t = time.monotonic()
if self.solution_status != 0:
if t > self.last_cloudlog_t + 5.0:
self.last_cloudlog_t = t
cloudlog.warning(f"Long mpc reset, solution_status: {self.solution_status}")
self.reset()
if __name__ == "__main__":
ocp = gen_long_ocp()
AcadosOcpSolver.generate(ocp, json_file=JSON_FILE)