pan131419/openpilot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Release 260111

Browse Source
This commit is contained in:
Comma Device
2026-01-11 18:23:29 +08:00
commit 3721ecbf8a
2601 changed files with 855070 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

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

@@ -0,0 +1,538 @@
{
"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/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": [
"/usr/local/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/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
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/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_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/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 /usr/local/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/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 /usr/local/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
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_ocp_solver_pyx.so Executable file
View File

Binary file not shown.

101
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.h Normal file
View File

@@ -0,0 +1,101 @@
/*
* 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.;
*/
#ifndef ACADOS_SIM_lat_H_
#define ACADOS_SIM_lat_H_
#include "acados_c/sim_interface.h"
#include "acados_c/external_function_interface.h"
#define LAT_NX 4
#define LAT_NZ 0
#define LAT_NU 1
#define LAT_NP 2
#ifdef __cplusplus
extern "C" {
#endif
// ** capsule for solver data **
typedef struct sim_solver_capsule
{
// acados objects
sim_in *acados_sim_in;
sim_out *acados_sim_out;
sim_solver *acados_sim_solver;
sim_opts *acados_sim_opts;
sim_config *acados_sim_config;
void *acados_sim_dims;
/* external functions */
// ERK
external_function_param_casadi * sim_forw_vde_casadi;
external_function_param_casadi * sim_vde_adj_casadi;
external_function_param_casadi * sim_expl_ode_fun_casadi;
external_function_param_casadi * sim_expl_ode_hess;
// IRK
external_function_param_casadi * sim_impl_dae_fun;
external_function_param_casadi * sim_impl_dae_fun_jac_x_xdot_z;
external_function_param_casadi * sim_impl_dae_jac_x_xdot_u_z;
external_function_param_casadi * sim_impl_dae_hess;
// GNSF
external_function_param_casadi * sim_gnsf_phi_fun;
external_function_param_casadi * sim_gnsf_phi_fun_jac_y;
external_function_param_casadi * sim_gnsf_phi_jac_y_uhat;
external_function_param_casadi * sim_gnsf_f_lo_jac_x1_x1dot_u_z;
external_function_param_casadi * sim_gnsf_get_matrices_fun;
} sim_solver_capsule;
ACADOS_SYMBOL_EXPORT int lat_acados_sim_create(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT int lat_acados_sim_solve(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT int lat_acados_sim_free(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT int lat_acados_sim_update_params(sim_solver_capsule *capsule, double *value, int np);
ACADOS_SYMBOL_EXPORT sim_config * lat_acados_get_sim_config(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_in * lat_acados_get_sim_in(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_out * lat_acados_get_sim_out(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT void * lat_acados_get_sim_dims(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_opts * lat_acados_get_sim_opts(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_solver * lat_acados_get_sim_solver(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_solver_capsule * lat_acados_sim_solver_create_capsule(void);
ACADOS_SYMBOL_EXPORT int lat_acados_sim_solver_free_capsule(sim_solver_capsule *capsule);
#ifdef __cplusplus
}
#endif
#endif // ACADOS_SIM_lat_H_

62
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_solver.pxd Normal file
View File

@@ -0,0 +1,62 @@
#
# 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.;
#
cimport acados_solver_common
cdef extern from "acados_solver_lat.h":
ctypedef struct nlp_solver_capsule "lat_solver_capsule":
pass
nlp_solver_capsule * acados_create_capsule "lat_acados_create_capsule"()
int acados_free_capsule "lat_acados_free_capsule"(nlp_solver_capsule *capsule)
int acados_create "lat_acados_create"(nlp_solver_capsule * capsule)
int acados_create_with_discretization "lat_acados_create_with_discretization"(nlp_solver_capsule * capsule, int n_time_steps, double* new_time_steps)
int acados_update_time_steps "lat_acados_update_time_steps"(nlp_solver_capsule * capsule, int N, double* new_time_steps)
int acados_update_qp_solver_cond_N "lat_acados_update_qp_solver_cond_N"(nlp_solver_capsule * capsule, int qp_solver_cond_N)
int acados_update_params "lat_acados_update_params"(nlp_solver_capsule * capsule, int stage, double *value, int np_)
int acados_update_params_sparse "lat_acados_update_params_sparse"(nlp_solver_capsule * capsule, int stage, int *idx, double *p, int n_update)
int acados_solve "lat_acados_solve"(nlp_solver_capsule * capsule)
int acados_reset "lat_acados_reset"(nlp_solver_capsule * capsule, int reset_qp_solver_mem)
int acados_free "lat_acados_free"(nlp_solver_capsule * capsule)
void acados_print_stats "lat_acados_print_stats"(nlp_solver_capsule * capsule)
int acados_custom_update "lat_acados_custom_update"(nlp_solver_capsule* capsule, double * data, int data_len)
acados_solver_common.ocp_nlp_in *acados_get_nlp_in "lat_acados_get_nlp_in"(nlp_solver_capsule * capsule)
acados_solver_common.ocp_nlp_out *acados_get_nlp_out "lat_acados_get_nlp_out"(nlp_solver_capsule * capsule)
acados_solver_common.ocp_nlp_out *acados_get_sens_out "lat_acados_get_sens_out"(nlp_solver_capsule * capsule)
acados_solver_common.ocp_nlp_solver *acados_get_nlp_solver "lat_acados_get_nlp_solver"(nlp_solver_capsule * capsule)
acados_solver_common.ocp_nlp_config *acados_get_nlp_config "lat_acados_get_nlp_config"(nlp_solver_capsule * capsule)
void *acados_get_nlp_opts "lat_acados_get_nlp_opts"(nlp_solver_capsule * capsule)
acados_solver_common.ocp_nlp_dims *acados_get_nlp_dims "lat_acados_get_nlp_dims"(nlp_solver_capsule * capsule)
acados_solver_common.ocp_nlp_plan *acados_get_nlp_plan "lat_acados_get_nlp_plan"(nlp_solver_capsule * capsule)

170
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_solver_lat.h Normal file
View File

@@ -0,0 +1,170 @@
/*
* 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.;
*/
#ifndef ACADOS_SOLVER_lat_H_
#define ACADOS_SOLVER_lat_H_
#include "acados/utils/types.h"
#include "acados_c/ocp_nlp_interface.h"
#include "acados_c/external_function_interface.h"
#define LAT_NX 4
#define LAT_NZ 0
#define LAT_NU 1
#define LAT_NP 2
#define LAT_NBX 2
#define LAT_NBX0 4
#define LAT_NBU 0
#define LAT_NSBX 0
#define LAT_NSBU 0
#define LAT_NSH 0
#define LAT_NSG 0
#define LAT_NSPHI 0
#define LAT_NSHN 0
#define LAT_NSGN 0
#define LAT_NSPHIN 0
#define LAT_NSBXN 0
#define LAT_NS 0
#define LAT_NSN 0
#define LAT_NG 0
#define LAT_NBXN 0
#define LAT_NGN 0
#define LAT_NY0 5
#define LAT_NY 5
#define LAT_NYN 3
#define LAT_N 32
#define LAT_NH 0
#define LAT_NPHI 0
#define LAT_NHN 0
#define LAT_NPHIN 0
#define LAT_NR 0
#ifdef __cplusplus
extern "C" {
#endif
// ** capsule for solver data **
typedef struct lat_solver_capsule
{
// acados objects
ocp_nlp_in *nlp_in;
ocp_nlp_out *nlp_out;
ocp_nlp_out *sens_out;
ocp_nlp_solver *nlp_solver;
void *nlp_opts;
ocp_nlp_plan_t *nlp_solver_plan;
ocp_nlp_config *nlp_config;
ocp_nlp_dims *nlp_dims;
// number of expected runtime parameters
unsigned int nlp_np;
/* external functions */
// dynamics
external_function_param_casadi *forw_vde_casadi;
external_function_param_casadi *expl_ode_fun;
// cost
external_function_param_casadi *cost_y_fun;
external_function_param_casadi *cost_y_fun_jac_ut_xt;
external_function_param_casadi *cost_y_hess;
external_function_param_casadi cost_y_0_fun;
external_function_param_casadi cost_y_0_fun_jac_ut_xt;
external_function_param_casadi cost_y_0_hess;
external_function_param_casadi cost_y_e_fun;
external_function_param_casadi cost_y_e_fun_jac_ut_xt;
external_function_param_casadi cost_y_e_hess;
// constraints
} lat_solver_capsule;
ACADOS_SYMBOL_EXPORT lat_solver_capsule * lat_acados_create_capsule(void);
ACADOS_SYMBOL_EXPORT int lat_acados_free_capsule(lat_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT int lat_acados_create(lat_solver_capsule * capsule);
ACADOS_SYMBOL_EXPORT int lat_acados_reset(lat_solver_capsule* capsule, int reset_qp_solver_mem);
/**
* Generic version of lat_acados_create which allows to use a different number of shooting intervals than
* the number used for code generation. If new_time_steps=NULL and n_time_steps matches the number used for code
* generation, the time-steps from code generation is used.
*/
ACADOS_SYMBOL_EXPORT int lat_acados_create_with_discretization(lat_solver_capsule * capsule, int n_time_steps, double* new_time_steps);
/**
* Update the time step vector. Number N must be identical to the currently set number of shooting nodes in the
* nlp_solver_plan. Returns 0 if no error occurred and a otherwise a value other than 0.
*/
ACADOS_SYMBOL_EXPORT int lat_acados_update_time_steps(lat_solver_capsule * capsule, int N, double* new_time_steps);
/**
* This function is used for updating an already initialized solver with a different number of qp_cond_N.
*/
ACADOS_SYMBOL_EXPORT int lat_acados_update_qp_solver_cond_N(lat_solver_capsule * capsule, int qp_solver_cond_N);
ACADOS_SYMBOL_EXPORT int lat_acados_update_params(lat_solver_capsule * capsule, int stage, double *value, int np);
ACADOS_SYMBOL_EXPORT int lat_acados_update_params_sparse(lat_solver_capsule * capsule, int stage, int *idx, double *p, int n_update);
ACADOS_SYMBOL_EXPORT int lat_acados_solve(lat_solver_capsule * capsule);
ACADOS_SYMBOL_EXPORT int lat_acados_free(lat_solver_capsule * capsule);
ACADOS_SYMBOL_EXPORT void lat_acados_print_stats(lat_solver_capsule * capsule);
ACADOS_SYMBOL_EXPORT int lat_acados_custom_update(lat_solver_capsule* capsule, double* data, int data_len);
ACADOS_SYMBOL_EXPORT ocp_nlp_in *lat_acados_get_nlp_in(lat_solver_capsule * capsule);
ACADOS_SYMBOL_EXPORT ocp_nlp_out *lat_acados_get_nlp_out(lat_solver_capsule * capsule);
ACADOS_SYMBOL_EXPORT ocp_nlp_out *lat_acados_get_sens_out(lat_solver_capsule * capsule);
ACADOS_SYMBOL_EXPORT ocp_nlp_solver *lat_acados_get_nlp_solver(lat_solver_capsule * capsule);
ACADOS_SYMBOL_EXPORT ocp_nlp_config *lat_acados_get_nlp_config(lat_solver_capsule * capsule);
ACADOS_SYMBOL_EXPORT void *lat_acados_get_nlp_opts(lat_solver_capsule * capsule);
ACADOS_SYMBOL_EXPORT ocp_nlp_dims *lat_acados_get_nlp_dims(lat_solver_capsule * capsule);
ACADOS_SYMBOL_EXPORT ocp_nlp_plan_t *lat_acados_get_nlp_plan(lat_solver_capsule * capsule);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_SOLVER_lat_H_

50
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_constraints/lat_constraints.h Normal file
View File

@@ -0,0 +1,50 @@
/*
* 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.;
*/
#ifndef lat_CONSTRAINTS
#define lat_CONSTRAINTS
#ifdef __cplusplus
extern "C" {
#endif
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // lat_CONSTRAINTS

119
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost.h Normal file
View File

@@ -0,0 +1,119 @@
/*
* 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.;
*/
#ifndef lat_COST
#define lat_COST
#ifdef __cplusplus
extern "C" {
#endif
// Cost at initial shooting node
int lat_cost_y_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int lat_cost_y_0_fun_work(int *, int *, int *, int *);
const int *lat_cost_y_0_fun_sparsity_in(int);
const int *lat_cost_y_0_fun_sparsity_out(int);
int lat_cost_y_0_fun_n_in(void);
int lat_cost_y_0_fun_n_out(void);
int lat_cost_y_0_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int lat_cost_y_0_fun_jac_ut_xt_work(int *, int *, int *, int *);
const int *lat_cost_y_0_fun_jac_ut_xt_sparsity_in(int);
const int *lat_cost_y_0_fun_jac_ut_xt_sparsity_out(int);
int lat_cost_y_0_fun_jac_ut_xt_n_in(void);
int lat_cost_y_0_fun_jac_ut_xt_n_out(void);
int lat_cost_y_0_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int lat_cost_y_0_hess_work(int *, int *, int *, int *);
const int *lat_cost_y_0_hess_sparsity_in(int);
const int *lat_cost_y_0_hess_sparsity_out(int);
int lat_cost_y_0_hess_n_in(void);
int lat_cost_y_0_hess_n_out(void);
// Cost at path shooting node
int lat_cost_y_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int lat_cost_y_fun_work(int *, int *, int *, int *);
const int *lat_cost_y_fun_sparsity_in(int);
const int *lat_cost_y_fun_sparsity_out(int);
int lat_cost_y_fun_n_in(void);
int lat_cost_y_fun_n_out(void);
int lat_cost_y_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int lat_cost_y_fun_jac_ut_xt_work(int *, int *, int *, int *);
const int *lat_cost_y_fun_jac_ut_xt_sparsity_in(int);
const int *lat_cost_y_fun_jac_ut_xt_sparsity_out(int);
int lat_cost_y_fun_jac_ut_xt_n_in(void);
int lat_cost_y_fun_jac_ut_xt_n_out(void);
int lat_cost_y_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int lat_cost_y_hess_work(int *, int *, int *, int *);
const int *lat_cost_y_hess_sparsity_in(int);
const int *lat_cost_y_hess_sparsity_out(int);
int lat_cost_y_hess_n_in(void);
int lat_cost_y_hess_n_out(void);
// Cost at terminal shooting node
int lat_cost_y_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int lat_cost_y_e_fun_work(int *, int *, int *, int *);
const int *lat_cost_y_e_fun_sparsity_in(int);
const int *lat_cost_y_e_fun_sparsity_out(int);
int lat_cost_y_e_fun_n_in(void);
int lat_cost_y_e_fun_n_out(void);
int lat_cost_y_e_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int lat_cost_y_e_fun_jac_ut_xt_work(int *, int *, int *, int *);
const int *lat_cost_y_e_fun_jac_ut_xt_sparsity_in(int);
const int *lat_cost_y_e_fun_jac_ut_xt_sparsity_out(int);
int lat_cost_y_e_fun_jac_ut_xt_n_in(void);
int lat_cost_y_e_fun_jac_ut_xt_n_out(void);
int lat_cost_y_e_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int lat_cost_y_e_hess_work(int *, int *, int *, int *);
const int *lat_cost_y_e_hess_sparsity_in(int);
const int *lat_cost_y_e_hess_sparsity_out(int);
int lat_cost_y_e_hess_n_in(void);
int lat_cost_y_e_hess_n_out(void);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // lat_COST

71
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_model/lat_model.h Normal file
View File

@@ -0,0 +1,71 @@
/*
* 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.;
*/
#ifndef lat_MODEL
#define lat_MODEL
#ifdef __cplusplus
extern "C" {
#endif
/* explicit ODE */
// explicit ODE
int lat_expl_ode_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int lat_expl_ode_fun_work(int *, int *, int *, int *);
const int *lat_expl_ode_fun_sparsity_in(int);
const int *lat_expl_ode_fun_sparsity_out(int);
int lat_expl_ode_fun_n_in(void);
int lat_expl_ode_fun_n_out(void);
// explicit forward VDE
int lat_expl_vde_forw(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int lat_expl_vde_forw_work(int *, int *, int *, int *);
const int *lat_expl_vde_forw_sparsity_in(int);
const int *lat_expl_vde_forw_sparsity_out(int);
int lat_expl_vde_forw_n_in(void);
int lat_expl_vde_forw_n_out(void);
// explicit adjoint VDE
int lat_expl_vde_adj(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int lat_expl_vde_adj_work(int *, int *, int *, int *);
const int *lat_expl_vde_adj_sparsity_in(int);
const int *lat_expl_vde_adj_sparsity_out(int);
int lat_expl_vde_adj_n_in(void);
int lat_expl_vde_adj_n_out(void);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // lat_MODEL

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

Binary file not shown.

199
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 openpilot.selfdrive.modeld.constants import ModelConstants
if __name__ == '__main__': # generating code
from openpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
else:
from openpilot.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)