IQ.Lvbs/IQ.Pilot
8
7
Fork 2
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

IQ.Pilot Prebuilt Release @ 27f668a

Browse Source
This commit is contained in:
IQ.Lvbs CI [bot]
2026-09-03 18:23:24 -05:00
commit b073c5182b
2554 changed files with 679696 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

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

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

538
iqpilot/selfdrive/controls/lib/lateral_mpc_lib/acados_ocp_lat.json Normal file
View File

@@ -0,0 +1,538 @@
{
"acados_include_path": "/data/openpilot/iqpilot/third_party/acados/include",
"acados_lib_path": "/data/openpilot/iqpilot/third_party/acados/lib",
"code_export_directory": "/data/openpilot/iqpilot/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code",
"constraints": {
"C": [],
"C_e": [],
"D": [],
"constr_type": "BGH",
"constr_type_e": "BGH",
"idxbu": [],
"idxbx": [
2,
3
],
"idxbx_0": [
0,
1,
2,
3
],
"idxbx_e": [],
"idxbxe_0": [
0,
1,
2,
3
],
"idxsbu": [],
"idxsbx": [],
"idxsbx_e": [],
"idxsg": [],
"idxsg_e": [],
"idxsh": [],
"idxsh_e": [],
"idxsphi": [],
"idxsphi_e": [],
"lbu": [],
"lbx": [
-1.5707963267948966,
-0.8726646259971648
],
"lbx_0": [
0.0,
0.0,
0.0,
0.0
],
"lbx_e": [],
"lg": [],
"lg_e": [],
"lh": [],
"lh_e": [],
"lphi": [],
"lphi_e": [],
"lsbu": [],
"lsbx": [],
"lsbx_e": [],
"lsg": [],
"lsg_e": [],
"lsh": [],
"lsh_e": [],
"lsphi": [],
"lsphi_e": [],
"ubu": [],
"ubx": [
1.5707963267948966,
0.8726646259971648
],
"ubx_0": [
0.0,
0.0,
0.0,
0.0
],
"ubx_e": [],
"ug": [],
"ug_e": [],
"uh": [],
"uh_e": [],
"uphi": [],
"uphi_e": [],
"usbu": [],
"usbx": [],
"usbx_e": [],
"usg": [],
"usg_e": [],
"ush": [],
"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
]
],
"Zl": [],
"Zl_e": [],
"Zu": [],
"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
],
"zl": [],
"zl_e": [],
"zu": [],
"zu_e": []
},
"cython_include_dirs": [
"/data/openpilot/.venv/lib/python3.12/site-packages/numpy/_core/include",
"/usr/include/python3.12"
],
"dims": {
"N": 32,
"nbu": 0,
"nbx": 2,
"nbx_0": 4,
"nbx_e": 0,
"nbxe_0": 4,
"ng": 0,
"ng_e": 0,
"nh": 0,
"nh_e": 0,
"np": 2,
"nphi": 0,
"nphi_e": 0,
"nr": 0,
"nr_e": 0,
"ns": 0,
"ns_e": 0,
"nsbu": 0,
"nsbx": 0,
"nsbx_e": 0,
"nsg": 0,
"nsg_e": 0,
"nsh": 0,
"nsh_e": 0,
"nsphi": 0,
"nsphi_e": 0,
"nu": 1,
"nx": 4,
"ny": 5,
"ny_0": 5,
"ny_e": 3,
"nz": 0
},
"json_file": "/data/openpilot/iqpilot/selfdrive/controls/lib/lateral_mpc_lib/acados_ocp_lat.json",
"model": {
"con_h_expr": null,
"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": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegjaaaaaaaaaaaaaaafaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaajhpffghgpgegdaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegmcaaaaaaaaaaaaaajgkaaaaaaaegpcaaaaaaaaaaaaaahaaaaaaaahdhjgpffghgpgegdaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaamaaaaaaaahdhjgpfchbgehfgpffghgpgegdaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaaahdhjgpfbgdgdgfgmgpffghgpgegeaaaaaaaaaaaaaaachjaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaachcaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaachkjjjjjjjjjjjjlpd",
"cost_y_expr_0": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegjaaaaaaaaaaaaaaafaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaajhpffghgpgegdaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegmcaaaaaaaaaaaaaajgkaaaaaaaegpcaaaaaaaaaaaaaahaaaaaaaahdhjgpffghgpgegdaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaamaaaaaaaahdhjgpfchbgehfgpffghgpgegdaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaaahdhjgpfbgdgdgfgmgpffghgpgegeaaaaaaaaaaaaaaachjaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaachcaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaachkjjjjjjjjjjjjlpd",
"cost_y_expr_e": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaeghaaaaaaaaaaaaaaadaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaajhpffghgpgegdaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegmcaaaaaaaaaaaaaajgkaaaaaaaegpcaaaaaaaaaaaaaahaaaaaaaahdhjgpffghgpgegdaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaamaaaaaaaahdhjgpfchbgehfgpffghgpg",
"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": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegiaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegoaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaahaaaaaaaahdhjgpffghgpgegdaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaapaaaaaaachpgehbgehjgpgogpfchbgegjgfhdhegnaaaaaaaaaaaaaaachcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaamaaaaaaaahdhjgpfchbgehfgpffghgpgegbaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaachbaaaaaaaaaaaaaaaegnaaaaaaaaaaaaaaachcaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaachfaaaaaaaaaaaaaaaegoaaaaaaaaaaaaaaachcaaaaaaaaaaaaaaachiaaaaaaaaaaaaaaachiaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaaahdhjgpfbgdgdgfgmgpffghgpg",
"f_impl_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegiaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaajaaaaaaaihpffghgpgpfegpgehegcaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegoaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaahaaaaaaaahdhjgpffghgpgegdaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaapaaaaaaachpgehbgehjgpgogpfchbgegjgfhdhegnaaaaaaaaaaaaaaachdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaamaaaaaaaahdhjgpfchbgehfgpffghgpgegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaajaaaaaaajhpffghgpgpfegpgehegbaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaachcaaaaaaaaaaaaaaaegnaaaaaaaaaaaaaaachdaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaachgaaaaaaaaaaaaaaaegoaaaaaaaaaaaaaaachdaaaaaaaaaaaaaaachjaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaalaaaaaaaahdhjgpffghgpgpfegpgehchjaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaaabaaaaaaahdhjgpfchbgehfgpffghgpgpfegpgehegpcaaaaaaaaaaaaaanaaaaaaaahdhjgpfbgdgdgfgmgpffghgpg",
"gnsf": {
"nontrivial_f_LO": 1,
"purely_linear": 0
},
"name": "lat",
"p": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaeggaaaaaaaaaaaaaaacaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaacaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegpcaaaaaaaaaaaaaapaaaaaaachpgehbgehjgpgogpfchbgegjgfhdh",
"u": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegfaaaaaaaaaaaaaaabaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaaahdhjgpfbgdgdgfgmgpffghgpg",
"x": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegiaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegpcaaaaaaaaaaaaaafaaaaaaajhpffghgpgegpcaaaaaaaaaaaaaahaaaaaaaahdhjgpffghgpgegpcaaaaaaaaaaaaaamaaaaaaaahdhjgpfchbgehfgpffghgpg",
"xdot": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegiaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaajaaaaaaaihpffghgpgpfegpgehegpcaaaaaaaaaaaaaajaaaaaaajhpffghgpgpfegpgehegpcaaaaaaaaaaaaaalaaaaaaaahdhjgpffghgpgpfegpgehegpcaaaaaaaaaaaaaaabaaaaaaahdhjgpfchbgehfgpffghgpgpfegpgeh",
"z": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa"
},
"parameter_values": [
0.0,
0.0
],
"problem_class": "OCP",
"shared_lib_ext": ".so",
"solver_options": {
"Tsim": 0.009765625,
"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": 1,
"qp_solver_ric_alg": 1,
"qp_solver_tol_comp": null,
"qp_solver_tol_eq": null,
"qp_solver_tol_ineq": null,
"qp_solver_tol_stat": null,
"qp_solver_warm_start": 0,
"regularize_method": null,
"shooting_nodes": [
0.0,
0.009765625,
0.0390625,
0.087890625,
0.15625,
0.244140625,
0.3515625,
0.478515625,
0.625,
0.791015625,
0.9765625,
1.181640625,
1.40625,
1.650390625,
1.9140625,
2.197265625,
2.5,
2.822265625,
3.1640625,
3.525390625,
3.90625,
4.306640625,
4.7265625,
5.166015625,
5.625,
6.103515625,
6.6015625,
7.119140625,
7.65625,
8.212890625,
8.7890625,
9.384765625,
10.0
],
"sim_method_jac_reuse": [
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
],
"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,
4,
4,
4,
4,
4,
4,
4,
4,
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,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1
],
"tf": 10.0,
"time_steps": [
0.009765625,
0.029296875,
0.048828125,
0.068359375,
0.087890625,
0.107421875,
0.126953125,
0.146484375,
0.166015625,
0.185546875,
0.205078125,
0.224609375,
0.244140625,
0.263671875,
0.283203125,
0.302734375,
0.322265625,
0.341796875,
0.361328125,
0.380859375,
0.400390625,
0.419921875,
0.439453125,
0.458984375,
0.478515625,
0.498046875,
0.517578125,
0.537109375,
0.556640625,
0.576171875,
0.595703125,
0.615234375
]
}
}

211
iqpilot/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/Makefile Normal file
View File

@@ -0,0 +1,211 @@
#
# 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+= lat_model/lat_expl_ode_fun.c
MODEL_SRC+= lat_model/lat_expl_vde_forw.c
MODEL_SRC+= lat_model/lat_expl_vde_adj.c
MODEL_OBJ := $(MODEL_SRC:.c=.o)
# optimal control problem - mostly CasADi exports
OCP_SRC=
OCP_SRC+= lat_cost/lat_cost_y_0_fun.c
OCP_SRC+= lat_cost/lat_cost_y_0_fun_jac_ut_xt.c
OCP_SRC+= lat_cost/lat_cost_y_0_hess.c
OCP_SRC+= lat_cost/lat_cost_y_fun.c
OCP_SRC+= lat_cost/lat_cost_y_fun_jac_ut_xt.c
OCP_SRC+= lat_cost/lat_cost_y_hess.c
OCP_SRC+= lat_cost/lat_cost_y_e_fun.c
OCP_SRC+= lat_cost/lat_cost_y_e_fun_jac_ut_xt.c
OCP_SRC+= lat_cost/lat_cost_y_e_hess.c
OCP_SRC+= acados_solver_lat.c
OCP_OBJ := $(OCP_SRC:.c=.o)
# for sim solver
SIM_SRC= acados_sim_solver_lat.c
SIM_OBJ := $(SIM_SRC:.c=.o)
# for target example
EX_SRC= main_lat.c
EX_OBJ := $(EX_SRC:.c=.o)
EX_EXE := $(EX_SRC:.c=)
# for target example_sim
EX_SIM_SRC= main_sim_lat.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/iqpilot/third_party/acados/include
LIB_PATH = /data/openpilot/iqpilot/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_lat.so
LIBACADOS_OCP_SOLVER=libacados_ocp_solver_lat.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/iqpilot/selfdrive/controls/lib/lateral_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_lat.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/iqpilot/selfdrive/controls/lib/lateral_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_lat.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_lat.so
$(RM) acados_solver_lat.o
$(RM) acados_ocp_solver_pyx.so
$(RM) acados_ocp_solver_pyx.o
clean_sim_cython:
$(RM) libacados_sim_solver_lat.so
$(RM) acados_sim_solver_lat.o
$(RM) acados_sim_solver_pyx.so
$(RM) acados_sim_solver_pyx.o

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

Binary file not shown.

BIN
iqpilot/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/libacados_ocp_solver_lat.so Executable file
View File

Binary file not shown.

49
iqpilot/selfdrive/controls/lib/lateral_mpc_lib/gen.log Normal file
View File

@@ -0,0 +1,49 @@
/data/openpilot/iqpilot/third_party/acados/acados_template/acados_model.py:54: SyntaxWarning: invalid escape sequence '\d'
"""
/data/openpilot/iqpilot/third_party/acados/acados_template/acados_model.py:60: SyntaxWarning: invalid escape sequence '\d'
"""
/data/openpilot/iqpilot/third_party/acados/acados_template/acados_model.py:111: SyntaxWarning: invalid escape sequence '\p'
"""
/data/openpilot/iqpilot/third_party/acados/acados_template/acados_model.py:116: SyntaxWarning: invalid escape sequence '\p'
"""
/data/openpilot/iqpilot/third_party/acados/acados_template/acados_model.py:121: SyntaxWarning: invalid escape sequence '\p'
"""
/data/openpilot/iqpilot/third_party/acados/acados_template/acados_model.py:126: SyntaxWarning: invalid escape sequence '\p'
"""
/data/openpilot/iqpilot/third_party/acados/acados_template/acados_model.py:131: SyntaxWarning: invalid escape sequence '\p'
"""
/data/openpilot/iqpilot/third_party/acados/acados_template/acados_model.py:136: SyntaxWarning: invalid escape sequence '\p'
"""
/data/openpilot/iqpilot/third_party/acados/acados_template/acados_ocp.py:119: SyntaxWarning: invalid escape sequence '\p'
""":math:`n_{\pi}` - dimension of the image of the inner nonlinear function in positive definite constraints.
/data/openpilot/iqpilot/third_party/acados/acados_template/acados_ocp.py:125: SyntaxWarning: invalid escape sequence '\p'
""":math:`n_{\pi}^e` - dimension of the image of the inner nonlinear function in positive definite constraints.
/data/openpilot/iqpilot/third_party/acados/acados_template/acados_ocp.py:143: SyntaxWarning: invalid escape sequence '\p'
""":math:`n_{\phi}` - number of convex-over-nonlinear constraints.
/data/openpilot/iqpilot/third_party/acados/acados_template/acados_ocp.py:149: SyntaxWarning: invalid escape sequence '\p'
""":math:`n_{\phi}^e` - number of convex-over-nonlinear constraints at terminal shooting node N.
/data/openpilot/iqpilot/third_party/acados/acados_template/acados_ocp.py:227: SyntaxWarning: invalid escape sequence '\p'
""":math:`n_{{s\phi}}` - number of soft convex-over-nonlinear constraints.
/data/openpilot/iqpilot/third_party/acados/acados_template/acados_ocp.py:233: SyntaxWarning: invalid escape sequence '\p'
""":math:`n_{{s\phi}^e}` - number of soft convex-over-nonlinear constraints at terminal shooting node N.
/data/openpilot/iqpilot/third_party/acados/acados_template/acados_ocp.py:496: SyntaxWarning: invalid escape sequence '\D'
"""
/data/openpilot/iqpilot/third_party/acados/acados_template/acados_ocp.py:1193: SyntaxWarning: invalid escape sequence '\,'
""":math:`C` - C matrix in :math:`\\underline{g} \\leq D \, u + C \, x \\leq \\bar{g}`
/data/openpilot/iqpilot/third_party/acados/acados_template/acados_ocp.py:1201: SyntaxWarning: invalid escape sequence '\,'
""":math:`D` - D matrix in :math:`\\underline{g} \\leq D \, u + C \, x \\leq \\bar{g}`
/data/openpilot/iqpilot/third_party/acados/acados_template/acados_ocp.py:1285: SyntaxWarning: invalid escape sequence '\p'
""":math:`\\underline{\phi}` - lower bound for convex-over-nonlinear inequalities
/data/openpilot/iqpilot/third_party/acados/acados_template/acados_ocp.py:1293: SyntaxWarning: invalid escape sequence '\p'
""":math:`\\bar{\phi}` - upper bound for convex-over-nonlinear inequalities
/data/openpilot/iqpilot/third_party/acados/acados_template/acados_ocp.py:1302: SyntaxWarning: invalid escape sequence '\p'
""":math:`\\underline{\phi}^e` - lower bound on convex-over-nonlinear inequalities
/data/openpilot/iqpilot/third_party/acados/acados_template/acados_ocp.py:1310: SyntaxWarning: invalid escape sequence '\p'
""":math:`\\bar{\phi}^e` - upper bound on convex-over-nonlinear inequalities
/data/openpilot/iqpilot/third_party/acados/acados_template/acados_ocp.py:1485: SyntaxWarning: invalid escape sequence '\p'
""":math:`J_{s, \phi}` - matrix coefficient for soft bounds on convex-over-nonlinear constraints.
/data/openpilot/iqpilot/third_party/acados/acados_template/acados_ocp.py:1572: SyntaxWarning: invalid escape sequence '\m'
""":math:`x_0 \\in \mathbb{R}^{n_x}` - initial state --
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.

199
iqpilot/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py Executable file
View File

@@ -0,0 +1,199 @@
#!/usr/bin/env python3
import os
import time
import numpy as np
from casadi import SX, vertcat, sin, cos
# WARNING: imports outside of constants will not trigger a rebuild
from iqpilot.selfdrive.iqmodeld.config import ModelConstants
if __name__ == '__main__': # generating code
from iqpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
else:
from iqpilot.selfdrive.controls.lib.lateral_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython
LAT_MPC_DIR = os.path.dirname(os.path.abspath(__file__))
EXPORT_DIR = os.path.join(LAT_MPC_DIR, "c_generated_code")
JSON_FILE = os.path.join(LAT_MPC_DIR, "acados_ocp_lat.json")
X_DIM = 4
P_DIM = 2
COST_E_DIM = 3
COST_DIM = COST_E_DIM + 2
SPEED_OFFSET = 10.0
MODEL_NAME = 'lat'
ACADOS_SOLVER_TYPE = 'SQP_RTI'
N = 32
def gen_lat_model():
model = AcadosModel()
model.name = MODEL_NAME
# set up states & controls
x_ego = SX.sym('x_ego')
y_ego = SX.sym('y_ego')
psi_ego = SX.sym('psi_ego')
psi_rate_ego = SX.sym('psi_rate_ego')
model.x = vertcat(x_ego, y_ego, psi_ego, psi_rate_ego)
# parameters
v_ego = SX.sym('v_ego')
rotation_radius = SX.sym('rotation_radius')
model.p = vertcat(v_ego, rotation_radius)
# controls
psi_accel_ego = SX.sym('psi_accel_ego')
model.u = vertcat(psi_accel_ego)
# 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')
psi_rate_ego_dot = SX.sym('psi_rate_ego_dot')
model.xdot = vertcat(x_ego_dot, y_ego_dot, psi_ego_dot, psi_rate_ego_dot)
# dynamics model
f_expl = vertcat(v_ego * cos(psi_ego) - rotation_radius * sin(psi_ego) * psi_rate_ego,
v_ego * sin(psi_ego) + rotation_radius * cos(psi_ego) * psi_rate_ego,
psi_rate_ego,
psi_accel_ego)
model.f_impl_expr = model.xdot - f_expl
model.f_expl_expr = f_expl
return model
def gen_lat_ocp():
ocp = AcadosOcp()
ocp.model = gen_lat_model()
Tf = np.array(ModelConstants.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(np.zeros(COST_E_DIM))
QR = np.diag(np.zeros(COST_DIM))
ocp.cost.W = QR
ocp.cost.W_e = Q
y_ego, psi_ego, psi_rate_ego = ocp.model.x[1], ocp.model.x[2], ocp.model.x[3]
psi_rate_ego_dot = ocp.model.u[0]
v_ego = ocp.model.p[0]
ocp.parameter_values = np.zeros((P_DIM, ))
ocp.cost.yref = np.zeros((COST_DIM, ))
ocp.cost.yref_e = np.zeros((COST_E_DIM, ))
# Add offset to smooth out low speed control
# TODO unclear if this right solution long term
v_ego_offset = v_ego + SPEED_OFFSET
# TODO there are two costs on psi_rate_ego_dot, one
# is correlated to jerk the other to steering wheel movement
# the steering wheel movement cost is added to prevent excessive
# wheel movements
ocp.model.cost_y_expr = vertcat(y_ego,
v_ego_offset * psi_ego,
v_ego_offset * psi_rate_ego,
v_ego_offset * psi_rate_ego_dot,
psi_rate_ego_dot / (v_ego + 0.1))
ocp.model.cost_y_expr_e = vertcat(y_ego,
v_ego_offset * psi_ego,
v_ego_offset * psi_rate_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.zeros((X_DIM,))
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 = ACADOS_SOLVER_TYPE
ocp.solver_options.qp_solver_iter_max = 1
ocp.solver_options.qp_solver_cond_N = 1
# set prediction horizon
ocp.solver_options.tf = Tf
ocp.solver_options.shooting_nodes = np.array(ModelConstants.T_IDXS)[:N+1]
ocp.code_export_directory = EXPORT_DIR
return ocp
class LateralMpc:
def __init__(self, x0=None):
if x0 is None:
x0 = np.zeros(X_DIM)
self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
self.reset(x0)
def reset(self, x0=None):
if x0 is None:
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, 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])
# Somehow needed for stable init
for i in range(N+1):
self.solver.set(i, 'x', np.zeros(X_DIM))
self.solver.set(i, 'p', np.zeros(P_DIM))
self.solver.constraints_set(0, "lbx", x0)
self.solver.constraints_set(0, "ubx", x0)
self.solver.solve()
self.solution_status = 0
self.solve_time = 0.0
self.cost = 0
def set_weights(self, path_weight, heading_weight,
lat_accel_weight, lat_jerk_weight,
steering_rate_weight):
W = np.asfortranarray(np.diag([path_weight, heading_weight,
lat_accel_weight, lat_jerk_weight,
steering_rate_weight]))
for i in range(N):
self.solver.cost_set(i, 'W', W)
self.solver.cost_set(N, 'W', W[:COST_E_DIM,:COST_E_DIM])
def run(self, x0, p, y_pts, heading_pts, yaw_rate_pts):
x0_cp = np.copy(x0)
p_cp = np.copy(p)
self.solver.constraints_set(0, "lbx", x0_cp)
self.solver.constraints_set(0, "ubx", x0_cp)
self.yref[:,0] = y_pts
v_ego = p_cp[0, 0]
# rotation_radius = p_cp[1]
self.yref[:,1] = heading_pts * (v_ego + SPEED_OFFSET)
self.yref[:,2] = yaw_rate_pts * (v_ego + SPEED_OFFSET)
for i in range(N):
self.solver.cost_set(i, "yref", self.yref[i])
self.solver.set(i, "p", p_cp[i])
self.solver.set(N, "p", p_cp[N])
self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM])
t = time.monotonic()
self.solution_status = self.solver.solve()
self.solve_time = time.monotonic() - t
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.cost = self.solver.get_cost()
if __name__ == "__main__":
ocp = gen_lat_ocp()
AcadosOcpSolver.generate(ocp, json_file=JSON_FILE)
# AcadosOcpSolver.build(ocp.code_export_directory, with_cython=True)