mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-02-21 20:33:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

dragonpilot beta3

Browse Source 
date: 2023-08-22T14:21:17
commit: 6148ce3d77530281f890970718e9c42b2acc5ff1
This commit is contained in:
dragonpilot
2023-08-22 14:20:25 -07:00
parent 957d61c2f7
commit d8e5331c6e
722 changed files with 24317 additions and 116310 deletions
Download Patch File Download Diff File Expand all files Collapse all files

44
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/Makefile
View File

@@ -1,8 +1,5 @@
#
# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
# Copyright (c) The acados authors.
#
# This file is part of acados.
#
@@ -41,6 +38,7 @@
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
@@ -54,6 +52,7 @@ 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)
@@ -91,7 +90,7 @@ CPPFLAGS+= -I$(INCLUDE_PATH)/hpipm/include
# define the c-compiler flags for make's implicit rules
CFLAGS = -fPIC -std=c99 #-fno-diagnostics-show-line-numbers -g
CFLAGS = -fPIC -std=c99 -O2#-fno-diagnostics-show-line-numbers -g
# # Debugging
# CFLAGS += -g3
@@ -148,11 +147,11 @@ ocp_cython_o: ocp_cython_c
$(CC) $(ACADOS_FLAGS) -c -O2 \
-fPIC \
-o acados_ocp_solver_pyx.o \
-I /usr/include/python3.8 \
-I $(INCLUDE_PATH)/blasfeo/include/ \
-I $(INCLUDE_PATH)/hpipm/include/ \
-I $(INCLUDE_PATH) \
-I /usr/local/pyenv/versions/3.11.4/lib/python3.11/site-packages/numpy/core/include \
-I /usr/local/pyenv/versions/3.11.4/include/python3.11 \
acados_ocp_solver_pyx.c \
ocp_cython: ocp_cython_o
@@ -163,6 +162,33 @@ ocp_cython: ocp_cython_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/pyenv/versions/3.11.4/lib/python3.11/site-packages/numpy/core/include \
-I /usr/local/pyenv/versions/3.11.4/include/python3.11 \
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)
@@ -177,3 +203,9 @@ clean_ocp_cython:
$(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

7575
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_ocp_solver_pyx.c
View File

File diff suppressed because it is too large Load Diff

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

Binary file not shown.

22
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.c
View File

@@ -1,8 +1,5 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
@@ -82,6 +79,7 @@ int lat_acados_sim_create(sim_solver_capsule * capsule)
// explicit ode
capsule->sim_forw_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
capsule->sim_vde_adj_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
capsule->sim_expl_ode_fun_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
capsule->sim_forw_vde_casadi->casadi_fun = &lat_expl_vde_forw;
@@ -92,6 +90,14 @@ int lat_acados_sim_create(sim_solver_capsule * capsule)
capsule->sim_forw_vde_casadi->casadi_work = &lat_expl_vde_forw_work;
external_function_param_casadi_create(capsule->sim_forw_vde_casadi, np);
capsule->sim_vde_adj_casadi->casadi_fun = &lat_expl_vde_adj;
capsule->sim_vde_adj_casadi->casadi_n_in = &lat_expl_vde_adj_n_in;
capsule->sim_vde_adj_casadi->casadi_n_out = &lat_expl_vde_adj_n_out;
capsule->sim_vde_adj_casadi->casadi_sparsity_in = &lat_expl_vde_adj_sparsity_in;
capsule->sim_vde_adj_casadi->casadi_sparsity_out = &lat_expl_vde_adj_sparsity_out;
capsule->sim_vde_adj_casadi->casadi_work = &lat_expl_vde_adj_work;
external_function_param_casadi_create(capsule->sim_vde_adj_casadi, np);
capsule->sim_expl_ode_fun_casadi->casadi_fun = &lat_expl_ode_fun;
capsule->sim_expl_ode_fun_casadi->casadi_n_in = &lat_expl_ode_fun_n_in;
capsule->sim_expl_ode_fun_casadi->casadi_n_out = &lat_expl_ode_fun_n_out;
@@ -123,6 +129,8 @@ int lat_acados_sim_create(sim_solver_capsule * capsule)
capsule->acados_sim_opts = lat_sim_opts;
int tmp_int = 3;
sim_opts_set(lat_sim_config, lat_sim_opts, "newton_iter", &tmp_int);
double tmp_double = 0.0;
sim_opts_set(lat_sim_config, lat_sim_opts, "newton_tol", &tmp_double);
sim_collocation_type collocation_type = GAUSS_LEGENDRE;
sim_opts_set(lat_sim_config, lat_sim_opts, "collocation_type", &collocation_type);
@@ -146,7 +154,9 @@ int lat_acados_sim_create(sim_solver_capsule * capsule)
// model functions
lat_sim_config->model_set(lat_sim_in->model,
"expl_vde_for", capsule->sim_forw_vde_casadi);
"expl_vde_forw", capsule->sim_forw_vde_casadi);
lat_sim_config->model_set(lat_sim_in->model,
"expl_vde_adj", capsule->sim_vde_adj_casadi);
lat_sim_config->model_set(lat_sim_in->model,
"expl_ode_fun", capsule->sim_expl_ode_fun_casadi);
@@ -223,6 +233,7 @@ int lat_acados_sim_free(sim_solver_capsule *capsule)
// free external function
external_function_param_casadi_free(capsule->sim_forw_vde_casadi);
external_function_param_casadi_free(capsule->sim_vde_adj_casadi);
external_function_param_casadi_free(capsule->sim_expl_ode_fun_casadi);
return 0;
@@ -240,6 +251,7 @@ int lat_acados_sim_update_params(sim_solver_capsule *capsule, double *p, int np)
exit(1);
}
capsule->sim_forw_vde_casadi[0].set_param(capsule->sim_forw_vde_casadi, p);
capsule->sim_vde_adj_casadi[0].set_param(capsule->sim_vde_adj_casadi, p);
capsule->sim_expl_ode_fun_casadi[0].set_param(capsule->sim_expl_ode_fun_casadi, p);
return status;

6
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.h
View File

@@ -1,8 +1,5 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
@@ -61,6 +58,7 @@ typedef struct sim_solver_capsule
/* 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;

10
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_solver.pxd
View File

@@ -1,8 +1,5 @@
#
# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
# Copyright (c) The acados authors.
#
# This file is part of acados.
#
@@ -47,11 +44,14 @@ cdef extern from "acados_solver_lat.h":
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 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)

170
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_solver_lat.c
View File

@@ -1,8 +1,5 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
@@ -42,14 +39,12 @@
// example specific
#include "lat_model/lat_model.h"
#include "lat_constraints/lat_constraints.h"
#include "lat_cost/lat_cost.h"
#include "lat_cost/lat_cost_y_fun.h"
#include "lat_cost/lat_cost_y_0_fun.h"
#include "lat_cost/lat_cost_y_e_fun.h"
#include "acados_solver_lat.h"
#define NX LAT_NX
@@ -289,7 +284,6 @@ return nlp_dims;
void lat_acados_create_3_create_and_set_functions(lat_solver_capsule* capsule)
{
const int N = capsule->nlp_solver_plan->N;
ocp_nlp_config* nlp_config = capsule->nlp_config;
/************************************************
* external functions
@@ -434,28 +428,38 @@ void lat_acados_create_5_set_nlp_in(lat_solver_capsule* capsule, const int N, do
}
/**** Cost ****/
double* W_0 = calloc(NY0*NY0, sizeof(double));
// change only the non-zero elements:
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "W", W_0);
free(W_0);
double* yref_0 = calloc(NY0, sizeof(double));
// change only the non-zero elements:
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", yref_0);
free(yref_0);
double* W = calloc(NY*NY, sizeof(double));
// change only the non-zero elements:
double* yref = calloc(NY, sizeof(double));
// change only the non-zero elements:
for (int i = 1; i < N; i++)
{
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "W", W);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "yref", yref);
}
free(W);
free(yref);
double* yref_e = calloc(NYN, sizeof(double));
// change only the non-zero elements:
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "yref", yref_e);
free(yref_e);
double* W_0 = calloc(NY0*NY0, sizeof(double));
// change only the non-zero elements:
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "W", W_0);
free(W_0);
double* W = calloc(NY*NY, sizeof(double));
// change only the non-zero elements:
for (int i = 1; i < N; i++)
{
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "W", W);
}
free(W);
double* W_e = calloc(NYN*NYN, sizeof(double));
// change only the non-zero elements:
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "W", W_e);
free(W_e);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "nls_y_fun", &capsule->cost_y_0_fun);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "nls_y_fun_jac", &capsule->cost_y_0_fun_jac_ut_xt);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "nls_y_hess", &capsule->cost_y_0_hess);
@@ -465,17 +469,6 @@ void lat_acados_create_5_set_nlp_in(lat_solver_capsule* capsule, const int N, do
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "nls_y_fun_jac", &capsule->cost_y_fun_jac_ut_xt[i-1]);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "nls_y_hess", &capsule->cost_y_hess[i-1]);
}
// terminal cost
double* yref_e = calloc(NYN, sizeof(double));
// change only the non-zero elements:
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "yref", yref_e);
free(yref_e);
double* W_e = calloc(NYN*NYN, sizeof(double));
// change only the non-zero elements:
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "W", W_e);
free(W_e);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_fun", &capsule->cost_y_e_fun);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_fun_jac", &capsule->cost_y_e_fun_jac_ut_xt);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_hess", &capsule->cost_y_e_hess);
@@ -576,7 +569,6 @@ void lat_acados_create_6_set_opts(lat_solver_capsule* capsule)
{
const int N = capsule->nlp_solver_plan->N;
ocp_nlp_config* nlp_config = capsule->nlp_config;
ocp_nlp_dims* nlp_dims = capsule->nlp_dims;
void *nlp_opts = capsule->nlp_opts;
/************************************************
@@ -614,10 +606,10 @@ void lat_acados_create_6_set_opts(lat_solver_capsule* capsule)
for (int i = 0; i < N; i++)
ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_jac_reuse", &tmp_bool);
double nlp_solver_step_length = 1;
double nlp_solver_step_length = 1.0;
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "step_length", &nlp_solver_step_length);
double levenberg_marquardt = 0;
double levenberg_marquardt = 0.0;
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "levenberg_marquardt", &levenberg_marquardt);
/* options QP solver */
@@ -626,6 +618,9 @@ void lat_acados_create_6_set_opts(lat_solver_capsule* capsule)
const int qp_solver_cond_N_ori = 1;
qp_solver_cond_N = N < qp_solver_cond_N_ori ? N : qp_solver_cond_N_ori; // use the minimum value here
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_cond_N", &qp_solver_cond_N);
int nlp_solver_ext_qp_res = 0;
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "ext_qp_res", &nlp_solver_ext_qp_res);
// set HPIPM mode: should be done before setting other QP solver options
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_hpipm_mode", "BALANCE");
@@ -636,6 +631,11 @@ void lat_acados_create_6_set_opts(lat_solver_capsule* capsule)
int print_level = 0;
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "print_level", &print_level);
int qp_solver_cond_ric_alg = 1;
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_cond_ric_alg", &qp_solver_cond_ric_alg);
int qp_solver_ric_alg = 1;
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_ric_alg", &qp_solver_ric_alg);
int ext_cost_num_hess = 0;
@@ -744,6 +744,7 @@ int lat_acados_create_with_discretization(lat_solver_capsule* capsule, int N, do
// 9) do precomputations
int status = lat_acados_create_9_precompute(capsule);
return status;
}
@@ -771,7 +772,7 @@ int lat_acados_update_qp_solver_cond_N(lat_solver_capsule* capsule, int qp_solve
}
int lat_acados_reset(lat_solver_capsule* capsule)
int lat_acados_reset(lat_solver_capsule* capsule, int reset_qp_solver_mem)
{
// set initialization to all zeros
@@ -783,8 +784,6 @@ int lat_acados_reset(lat_solver_capsule* capsule)
ocp_nlp_in* nlp_in = capsule->nlp_in;
ocp_nlp_solver* nlp_solver = capsule->nlp_solver;
int nx, nu, nv, ns, nz, ni, dim;
double* buffer = calloc(NX+NU+NZ+2*NS+2*NSN+NBX+NBU+NG+NH+NPHI+NBX0+NBXN+NHN+NPHIN+NGN, sizeof(double));
for(int i=0; i<N+1; i++)
@@ -804,7 +803,7 @@ int lat_acados_reset(lat_solver_capsule* capsule)
// get qp_status: if NaN -> reset memory
int qp_status;
ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "qp_status", &qp_status);
if (qp_status == 3)
if (reset_qp_solver_mem || (qp_status == 3))
{
// printf("\nin reset qp_status %d -> resetting QP memory\n", qp_status);
ocp_nlp_solver_reset_qp_memory(nlp_solver, nlp_in, nlp_out);
@@ -827,6 +826,7 @@ int lat_acados_update_params(lat_solver_capsule* capsule, int stage, double *p,
" External function has %i parameters. Exiting.\n", np, casadi_np);
exit(1);
}
const int N = capsule->nlp_solver_plan->N;
if (stage < N && stage >= 0)
{
@@ -863,15 +863,72 @@ int lat_acados_update_params(lat_solver_capsule* capsule, int stage, double *p,
}
return solver_status;
}
int lat_acados_update_params_sparse(lat_solver_capsule * capsule, int stage, int *idx, double *p, int n_update)
{
int solver_status = 0;
int casadi_np = 2;
if (casadi_np < n_update) {
printf("lat_acados_update_params_sparse: trying to set %d parameters for external functions."
" External function has %d parameters. Exiting.\n", n_update, casadi_np);
exit(1);
}
// for (int i = 0; i < n_update; i++)
// {
// if (idx[i] > casadi_np) {
// printf("lat_acados_update_params_sparse: attempt to set parameters with index %d, while"
// " external functions only has %d parameters. Exiting.\n", idx[i], casadi_np);
// exit(1);
// }
// printf("param %d value %e\n", idx[i], p[i]);
// }
const int N = capsule->nlp_solver_plan->N;
if (stage < N && stage >= 0)
{
capsule->forw_vde_casadi[stage].set_param_sparse(capsule->forw_vde_casadi+stage, n_update, idx, p);
capsule->expl_ode_fun[stage].set_param_sparse(capsule->expl_ode_fun+stage, n_update, idx, p);
// constraints
// cost
if (stage == 0)
{
capsule->cost_y_0_fun.set_param_sparse(&capsule->cost_y_0_fun, n_update, idx, p);
capsule->cost_y_0_fun_jac_ut_xt.set_param_sparse(&capsule->cost_y_0_fun_jac_ut_xt, n_update, idx, p);
capsule->cost_y_0_hess.set_param_sparse(&capsule->cost_y_0_hess, n_update, idx, p);
}
else // 0 < stage < N
{
capsule->cost_y_fun[stage-1].set_param_sparse(capsule->cost_y_fun+stage-1, n_update, idx, p);
capsule->cost_y_fun_jac_ut_xt[stage-1].set_param_sparse(capsule->cost_y_fun_jac_ut_xt+stage-1, n_update, idx, p);
capsule->cost_y_hess[stage-1].set_param_sparse(capsule->cost_y_hess+stage-1, n_update, idx, p);
}
}
else // stage == N
{
// terminal shooting node has no dynamics
// cost
capsule->cost_y_e_fun.set_param_sparse(&capsule->cost_y_e_fun, n_update, idx, p);
capsule->cost_y_e_fun_jac_ut_xt.set_param_sparse(&capsule->cost_y_e_fun_jac_ut_xt, n_update, idx, p);
capsule->cost_y_e_hess.set_param_sparse(&capsule->cost_y_e_hess, n_update, idx, p);
// constraints
}
return solver_status;
}
int lat_acados_solve(lat_solver_capsule* capsule)
{
// solve NLP
// solve NLP
int solver_status = ocp_nlp_solve(capsule->nlp_solver, capsule->nlp_in, capsule->nlp_out);
return solver_status;
@@ -924,15 +981,6 @@ int lat_acados_free(lat_solver_capsule* capsule)
return 0;
}
ocp_nlp_in *lat_acados_get_nlp_in(lat_solver_capsule* capsule) { return capsule->nlp_in; }
ocp_nlp_out *lat_acados_get_nlp_out(lat_solver_capsule* capsule) { return capsule->nlp_out; }
ocp_nlp_out *lat_acados_get_sens_out(lat_solver_capsule* capsule) { return capsule->sens_out; }
ocp_nlp_solver *lat_acados_get_nlp_solver(lat_solver_capsule* capsule) { return capsule->nlp_solver; }
ocp_nlp_config *lat_acados_get_nlp_config(lat_solver_capsule* capsule) { return capsule->nlp_config; }
void *lat_acados_get_nlp_opts(lat_solver_capsule* capsule) { return capsule->nlp_opts; }
ocp_nlp_dims *lat_acados_get_nlp_dims(lat_solver_capsule* capsule) { return capsule->nlp_dims; }
ocp_nlp_plan_t *lat_acados_get_nlp_plan(lat_solver_capsule* capsule) { return capsule->nlp_solver_plan; }
void lat_acados_print_stats(lat_solver_capsule* capsule)
{
@@ -946,6 +994,11 @@ void lat_acados_print_stats(lat_solver_capsule* capsule)
ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "statistics", stat);
int nrow = sqp_iter+1 < stat_m ? sqp_iter+1 : stat_m;
printf("iter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter\talpha");
if (stat_n > 8)
printf("\t\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp");
printf("\n");
printf("iter\tqp_stat\tqp_iter\n");
for (int i = 0; i < nrow; i++)
{
@@ -958,3 +1011,24 @@ void lat_acados_print_stats(lat_solver_capsule* capsule)
}
}
int lat_acados_custom_update(lat_solver_capsule* capsule, double* data, int data_len)
{
(void)capsule;
(void)data;
(void)data_len;
printf("\ndummy function that can be called in between solver calls to update parameters or numerical data efficiently in C.\n");
printf("nothing set yet..\n");
return 1;
}
ocp_nlp_in *lat_acados_get_nlp_in(lat_solver_capsule* capsule) { return capsule->nlp_in; }
ocp_nlp_out *lat_acados_get_nlp_out(lat_solver_capsule* capsule) { return capsule->nlp_out; }
ocp_nlp_out *lat_acados_get_sens_out(lat_solver_capsule* capsule) { return capsule->sens_out; }
ocp_nlp_solver *lat_acados_get_nlp_solver(lat_solver_capsule* capsule) { return capsule->nlp_solver; }
ocp_nlp_config *lat_acados_get_nlp_config(lat_solver_capsule* capsule) { return capsule->nlp_config; }
void *lat_acados_get_nlp_opts(lat_solver_capsule* capsule) { return capsule->nlp_opts; }
ocp_nlp_dims *lat_acados_get_nlp_dims(lat_solver_capsule* capsule) { return capsule->nlp_dims; }
ocp_nlp_plan_t *lat_acados_get_nlp_plan(lat_solver_capsule* capsule) { return capsule->nlp_solver_plan; }

15
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_solver_lat.h
View File

@@ -1,8 +1,5 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
@@ -74,6 +71,7 @@
extern "C" {
#endif
// ** capsule for solver data **
typedef struct lat_solver_capsule
{
@@ -106,6 +104,7 @@ typedef struct lat_solver_capsule
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;
@@ -129,7 +128,7 @@ 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);
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
@@ -147,10 +146,14 @@ ACADOS_SYMBOL_EXPORT int lat_acados_update_time_steps(lat_solver_capsule * capsu
*/
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);

264
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_solver_sfunction_lat.c
View File

@@ -1,264 +0,0 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* 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 S_FUNCTION_NAME acados_solver_sfunction_lat
#define S_FUNCTION_LEVEL 2
#define MDL_START
// acados
// #include "acados/utils/print.h"
#include "acados_c/sim_interface.h"
#include "acados_c/external_function_interface.h"
// example specific
#include "lat_model/lat_model.h"
#include "acados_solver_lat.h"
#include "simstruc.h"
#define SAMPLINGTIME 0.009765625
static void mdlInitializeSizes (SimStruct *S)
{
// specify the number of continuous and discrete states
ssSetNumContStates(S, 0);
ssSetNumDiscStates(S, 0);// specify the number of input ports
if ( !ssSetNumInputPorts(S, 8) )
return;
// specify the number of output ports
if ( !ssSetNumOutputPorts(S, 6) )
return;
// specify dimension information for the input ports
// lbx_0
ssSetInputPortVectorDimension(S, 0, 4);
// ubx_0
ssSetInputPortVectorDimension(S, 1, 4);
// parameters
ssSetInputPortVectorDimension(S, 2, (32+1) * 2);
// y_ref_0
ssSetInputPortVectorDimension(S, 3, 5);
// y_ref
ssSetInputPortVectorDimension(S, 4, 155);
// y_ref_e
ssSetInputPortVectorDimension(S, 5, 3);
// lbx
ssSetInputPortVectorDimension(S, 6, 62);
// ubx
ssSetInputPortVectorDimension(S, 7, 62);/* specify dimension information for the OUTPUT ports */
ssSetOutputPortVectorDimension(S, 0, 1 );
ssSetOutputPortVectorDimension(S, 1, 1 );
ssSetOutputPortVectorDimension(S, 2, 1 );
ssSetOutputPortVectorDimension(S, 3, 4 ); // state at shooting node 1
ssSetOutputPortVectorDimension(S, 4, 1);
ssSetOutputPortVectorDimension(S, 5, 1 );
// specify the direct feedthrough status
// should be set to 1 for all inputs used in mdlOutputs
ssSetInputPortDirectFeedThrough(S, 0, 1);
ssSetInputPortDirectFeedThrough(S, 1, 1);
ssSetInputPortDirectFeedThrough(S, 2, 1);
ssSetInputPortDirectFeedThrough(S, 3, 1);
ssSetInputPortDirectFeedThrough(S, 4, 1);
ssSetInputPortDirectFeedThrough(S, 5, 1);
ssSetInputPortDirectFeedThrough(S, 6, 1);
ssSetInputPortDirectFeedThrough(S, 7, 1);
// one sample time
ssSetNumSampleTimes(S, 1);
}
#if defined(MATLAB_MEX_FILE)
#define MDL_SET_INPUT_PORT_DIMENSION_INFO
#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO
static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
{
if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) )
return;
}
static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
{
if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) )
return;
}
#endif /* MATLAB_MEX_FILE */
static void mdlInitializeSampleTimes(SimStruct *S)
{
ssSetSampleTime(S, 0, SAMPLINGTIME);
ssSetOffsetTime(S, 0, 0.0);
}
static void mdlStart(SimStruct *S)
{
lat_solver_capsule *capsule = lat_acados_create_capsule();
lat_acados_create(capsule);
ssSetUserData(S, (void*)capsule);
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
lat_solver_capsule *capsule = ssGetUserData(S);
ocp_nlp_config *nlp_config = lat_acados_get_nlp_config(capsule);
ocp_nlp_dims *nlp_dims = lat_acados_get_nlp_dims(capsule);
ocp_nlp_in *nlp_in = lat_acados_get_nlp_in(capsule);
ocp_nlp_out *nlp_out = lat_acados_get_nlp_out(capsule);
InputRealPtrsType in_sign;
// local buffer
real_t buffer[5];
/* go through inputs */
// lbx_0
in_sign = ssGetInputPortRealSignalPtrs(S, 0);
for (int i = 0; i < 4; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", buffer);
// ubx_0
in_sign = ssGetInputPortRealSignalPtrs(S, 1);
for (int i = 0; i < 4; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", buffer);
// parameters - stage-variant !!!
in_sign = ssGetInputPortRealSignalPtrs(S, 2);
// update value of parameters
for (int ii = 0; ii <= 32; ii++)
{
for (int jj = 0; jj < 2; jj++)
buffer[jj] = (double)(*in_sign[ii*2+jj]);
lat_acados_update_params(capsule, ii, buffer, 2);
}
// y_ref_0
in_sign = ssGetInputPortRealSignalPtrs(S, 3);
for (int i = 0; i < 5; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", (void *) buffer);
// y_ref - for stages 1 to N-1
in_sign = ssGetInputPortRealSignalPtrs(S, 4);
for (int ii = 1; ii < 32; ii++)
{
for (int jj = 0; jj < 5; jj++)
buffer[jj] = (double)(*in_sign[(ii-1)*5+jj]);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, ii, "yref", (void *) buffer);
}
// y_ref_e
in_sign = ssGetInputPortRealSignalPtrs(S, 5);
for (int i = 0; i < 3; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 32, "yref", (void *) buffer);
// lbx
in_sign = ssGetInputPortRealSignalPtrs(S, 6);
for (int ii = 1; ii < 32; ii++)
{
for (int jj = 0; jj < 2; jj++)
buffer[jj] = (double)(*in_sign[(ii-1)*2+jj]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lbx", (void *) buffer);
}
// ubx
in_sign = ssGetInputPortRealSignalPtrs(S, 7);
for (int ii = 1; ii < 32; ii++)
{
for (int jj = 0; jj < 2; jj++)
buffer[jj] = (double)(*in_sign[(ii-1)*2+jj]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ubx", (void *) buffer);
}
/* call solver */
int rti_phase = 0;
ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "rti_phase", &rti_phase);
int acados_status = lat_acados_solve(capsule);
/* set outputs */
// assign pointers to output signals
real_t *out_u0, *out_utraj, *out_xtraj, *out_status, *out_sqp_iter, *out_KKT_res, *out_x1, *out_cpu_time, *out_cpu_time_sim, *out_cpu_time_qp, *out_cpu_time_lin;
int tmp_int;
out_u0 = ssGetOutputPortRealSignal(S, 0);
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "u", (void *) out_u0);
out_status = ssGetOutputPortRealSignal(S, 1);
*out_status = (real_t) acados_status;
out_KKT_res = ssGetOutputPortRealSignal(S, 2);
*out_KKT_res = (real_t) nlp_out->inf_norm_res;
out_x1 = ssGetOutputPortRealSignal(S, 3);
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 1, "x", (void *) out_x1);
out_cpu_time = ssGetOutputPortRealSignal(S, 4);
// get solution time
ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_tot", (void *) out_cpu_time);
out_sqp_iter = ssGetOutputPortRealSignal(S, 5);
// get sqp iter
ocp_nlp_get(nlp_config, capsule->nlp_solver, "sqp_iter", (void *) &tmp_int);
*out_sqp_iter = (real_t) tmp_int;
}
static void mdlTerminate(SimStruct *S)
{
lat_solver_capsule *capsule = ssGetUserData(S);
lat_acados_free(capsule);
lat_acados_free_capsule(capsule);
}
#ifdef MATLAB_MEX_FILE
#include "simulink.c"
#else
#include "cg_sfun.h"
#endif

33
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_fun.h → selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_constraints/lat_constraints.h
View File

@@ -1,8 +1,5 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
@@ -31,39 +28,23 @@
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef lat_Y_COST
#define lat_Y_COST
#ifndef lat_CONSTRAINTS
#define lat_CONSTRAINTS
#ifdef __cplusplus
extern "C" {
#endif
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);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // lat_Y_COST
#endif // lat_CONSTRAINTS

64
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_0_fun.h → selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost.h
View File

@@ -1,8 +1,5 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
@@ -32,14 +29,16 @@
*/
#ifndef lat_Y_0_COST
#define lat_Y_0_COST
#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);
@@ -62,8 +61,59 @@ 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_Y_0_COST
#endif // lat_COST

18
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_0_fun.c
View File

@@ -34,6 +34,7 @@ extern "C" {
#define casadi_s1 CASADI_PREFIX(s1)
#define casadi_s2 CASADI_PREFIX(s2)
#define casadi_s3 CASADI_PREFIX(s3)
#define casadi_s4 CASADI_PREFIX(s4)
/* Symbol visibility in DLLs */
#ifndef CASADI_SYMBOL_EXPORT
@@ -52,15 +53,16 @@ extern "C" {
static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
static const casadi_int casadi_s2[6] = {2, 1, 0, 2, 0, 1};
static const casadi_int casadi_s3[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
static const casadi_int casadi_s2[3] = {0, 0, 0};
static const casadi_int casadi_s3[6] = {2, 1, 0, 2, 0, 1};
static const casadi_int casadi_s4[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
/* lat_cost_y_0_fun:(i0[4],i1,i2[2])->(o0[5]) */
/* lat_cost_y_0_fun:(i0[4],i1,i2[],i3[2])->(o0[5]) */
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
casadi_real a0, a1, a2;
a0=arg[0]? arg[0][1] : 0;
if (res[0]!=0) res[0][0]=a0;
a0=arg[2]? arg[2][0] : 0;
a0=arg[3]? arg[3][0] : 0;
a1=10.;
a1=(a0+a1);
a2=arg[0]? arg[0][2] : 0;
@@ -107,7 +109,7 @@ CASADI_SYMBOL_EXPORT void lat_cost_y_0_fun_incref(void) {
CASADI_SYMBOL_EXPORT void lat_cost_y_0_fun_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_0_fun_n_in(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_0_fun_n_in(void) { return 4;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_0_fun_n_out(void) { return 1;}
@@ -122,6 +124,7 @@ CASADI_SYMBOL_EXPORT const char* lat_cost_y_0_fun_name_in(casadi_int i) {
case 0: return "i0";
case 1: return "i1";
case 2: return "i2";
case 3: return "i3";
default: return 0;
}
}
@@ -138,19 +141,20 @@ CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_0_fun_sparsity_in(casadi_int i
case 0: return casadi_s0;
case 1: return casadi_s1;
case 2: return casadi_s2;
case 3: return casadi_s3;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_0_fun_sparsity_out(casadi_int i) {
switch (i) {
case 0: return casadi_s3;
case 0: return casadi_s4;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT int lat_cost_y_0_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
if (sz_arg) *sz_arg = 3;
if (sz_arg) *sz_arg = 4;
if (sz_res) *sz_res = 1;
if (sz_iw) *sz_iw = 0;
if (sz_w) *sz_w = 0;

30
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_0_fun_jac_ut_xt.c
View File

@@ -35,6 +35,8 @@ extern "C" {
#define casadi_s2 CASADI_PREFIX(s2)
#define casadi_s3 CASADI_PREFIX(s3)
#define casadi_s4 CASADI_PREFIX(s4)
#define casadi_s5 CASADI_PREFIX(s5)
#define casadi_s6 CASADI_PREFIX(s6)
/* Symbol visibility in DLLs */
#ifndef CASADI_SYMBOL_EXPORT
@@ -53,16 +55,18 @@ extern "C" {
static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
static const casadi_int casadi_s2[6] = {2, 1, 0, 2, 0, 1};
static const casadi_int casadi_s3[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
static const casadi_int casadi_s4[13] = {5, 5, 0, 1, 2, 3, 4, 5, 2, 3, 4, 0, 0};
static const casadi_int casadi_s2[3] = {0, 0, 0};
static const casadi_int casadi_s3[6] = {2, 1, 0, 2, 0, 1};
static const casadi_int casadi_s4[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
static const casadi_int casadi_s5[13] = {5, 5, 0, 1, 2, 3, 4, 5, 2, 3, 4, 0, 0};
static const casadi_int casadi_s6[3] = {5, 0, 0};
/* lat_cost_y_0_fun_jac_ut_xt:(i0[4],i1,i2[2])->(o0[5],o1[5x5,5nz]) */
/* lat_cost_y_0_fun_jac_ut_xt:(i0[4],i1,i2[],i3[2])->(o0[5],o1[5x5,5nz],o2[5x0]) */
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
casadi_real a0, a1, a2, a3;
a0=arg[0]? arg[0][1] : 0;
if (res[0]!=0) res[0][0]=a0;
a0=arg[2]? arg[2][0] : 0;
a0=arg[3]? arg[3][0] : 0;
a1=10.;
a1=(a0+a1);
a2=arg[0]? arg[0][2] : 0;
@@ -116,9 +120,9 @@ CASADI_SYMBOL_EXPORT void lat_cost_y_0_fun_jac_ut_xt_incref(void) {
CASADI_SYMBOL_EXPORT void lat_cost_y_0_fun_jac_ut_xt_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_0_fun_jac_ut_xt_n_in(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_0_fun_jac_ut_xt_n_in(void) { return 4;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_0_fun_jac_ut_xt_n_out(void) { return 2;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_0_fun_jac_ut_xt_n_out(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_real lat_cost_y_0_fun_jac_ut_xt_default_in(casadi_int i) {
switch (i) {
@@ -131,6 +135,7 @@ CASADI_SYMBOL_EXPORT const char* lat_cost_y_0_fun_jac_ut_xt_name_in(casadi_int i
case 0: return "i0";
case 1: return "i1";
case 2: return "i2";
case 3: return "i3";
default: return 0;
}
}
@@ -139,6 +144,7 @@ CASADI_SYMBOL_EXPORT const char* lat_cost_y_0_fun_jac_ut_xt_name_out(casadi_int
switch (i) {
case 0: return "o0";
case 1: return "o1";
case 2: return "o2";
default: return 0;
}
}
@@ -148,21 +154,23 @@ CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_0_fun_jac_ut_xt_sparsity_in(ca
case 0: return casadi_s0;
case 1: return casadi_s1;
case 2: return casadi_s2;
case 3: return casadi_s3;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_0_fun_jac_ut_xt_sparsity_out(casadi_int i) {
switch (i) {
case 0: return casadi_s3;
case 1: return casadi_s4;
case 0: return casadi_s4;
case 1: return casadi_s5;
case 2: return casadi_s6;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT int lat_cost_y_0_fun_jac_ut_xt_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
if (sz_arg) *sz_arg = 3;
if (sz_res) *sz_res = 2;
if (sz_arg) *sz_arg = 4;
if (sz_res) *sz_res = 3;
if (sz_iw) *sz_iw = 0;
if (sz_w) *sz_w = 0;
return 0;

18
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_0_hess.c
View File

@@ -35,6 +35,7 @@ extern "C" {
#define casadi_s2 CASADI_PREFIX(s2)
#define casadi_s3 CASADI_PREFIX(s3)
#define casadi_s4 CASADI_PREFIX(s4)
#define casadi_s5 CASADI_PREFIX(s5)
/* Symbol visibility in DLLs */
#ifndef CASADI_SYMBOL_EXPORT
@@ -53,11 +54,12 @@ extern "C" {
static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
static const casadi_int casadi_s2[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
static const casadi_int casadi_s3[6] = {2, 1, 0, 2, 0, 1};
static const casadi_int casadi_s4[8] = {5, 5, 0, 0, 0, 0, 0, 0};
static const casadi_int casadi_s2[3] = {0, 0, 0};
static const casadi_int casadi_s3[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
static const casadi_int casadi_s4[6] = {2, 1, 0, 2, 0, 1};
static const casadi_int casadi_s5[8] = {5, 5, 0, 0, 0, 0, 0, 0};
/* lat_cost_y_0_hess:(i0[4],i1,i2[5],i3[2])->(o0[5x5,0nz]) */
/* lat_cost_y_0_hess:(i0[4],i1,i2[],i3[5],i4[2])->(o0[5x5,0nz]) */
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
return 0;
}
@@ -90,7 +92,7 @@ CASADI_SYMBOL_EXPORT void lat_cost_y_0_hess_incref(void) {
CASADI_SYMBOL_EXPORT void lat_cost_y_0_hess_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_0_hess_n_in(void) { return 4;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_0_hess_n_in(void) { return 5;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_0_hess_n_out(void) { return 1;}
@@ -106,6 +108,7 @@ CASADI_SYMBOL_EXPORT const char* lat_cost_y_0_hess_name_in(casadi_int i) {
case 1: return "i1";
case 2: return "i2";
case 3: return "i3";
case 4: return "i4";
default: return 0;
}
}
@@ -123,19 +126,20 @@ CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_0_hess_sparsity_in(casadi_int
case 1: return casadi_s1;
case 2: return casadi_s2;
case 3: return casadi_s3;
case 4: return casadi_s4;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_0_hess_sparsity_out(casadi_int i) {
switch (i) {
case 0: return casadi_s4;
case 0: return casadi_s5;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT int lat_cost_y_0_hess_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
if (sz_arg) *sz_arg = 4;
if (sz_arg) *sz_arg = 5;
if (sz_res) *sz_res = 1;
if (sz_iw) *sz_iw = 0;
if (sz_w) *sz_w = 0;

12
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_e_fun.c
View File

@@ -55,12 +55,12 @@ static const casadi_int casadi_s1[3] = {0, 0, 0};
static const casadi_int casadi_s2[6] = {2, 1, 0, 2, 0, 1};
static const casadi_int casadi_s3[7] = {3, 1, 0, 3, 0, 1, 2};
/* lat_cost_y_e_fun:(i0[4],i1[],i2[2])->(o0[3]) */
/* lat_cost_y_e_fun:(i0[4],i1[],i2[],i3[2])->(o0[3]) */
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
casadi_real a0, a1;
a0=arg[0]? arg[0][1] : 0;
if (res[0]!=0) res[0][0]=a0;
a0=arg[2]? arg[2][0] : 0;
a0=arg[3]? arg[3][0] : 0;
a1=10.;
a0=(a0+a1);
a1=arg[0]? arg[0][2] : 0;
@@ -100,7 +100,7 @@ CASADI_SYMBOL_EXPORT void lat_cost_y_e_fun_incref(void) {
CASADI_SYMBOL_EXPORT void lat_cost_y_e_fun_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_e_fun_n_in(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_e_fun_n_in(void) { return 4;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_e_fun_n_out(void) { return 1;}
@@ -115,6 +115,7 @@ CASADI_SYMBOL_EXPORT const char* lat_cost_y_e_fun_name_in(casadi_int i) {
case 0: return "i0";
case 1: return "i1";
case 2: return "i2";
case 3: return "i3";
default: return 0;
}
}
@@ -130,7 +131,8 @@ CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_e_fun_sparsity_in(casadi_int i
switch (i) {
case 0: return casadi_s0;
case 1: return casadi_s1;
case 2: return casadi_s2;
case 2: return casadi_s1;
case 3: return casadi_s2;
default: return 0;
}
}
@@ -143,7 +145,7 @@ CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_e_fun_sparsity_out(casadi_int
}
CASADI_SYMBOL_EXPORT int lat_cost_y_e_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
if (sz_arg) *sz_arg = 3;
if (sz_arg) *sz_arg = 4;
if (sz_res) *sz_res = 1;
if (sz_iw) *sz_iw = 0;
if (sz_w) *sz_w = 0;

69
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_e_fun.h
View File

@@ -1,69 +0,0 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* 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_Y_E_COST
#define lat_Y_E_COST
#ifdef __cplusplus
extern "C" {
#endif
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_Y_E_COST

20
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_e_fun_jac_ut_xt.c
View File

@@ -35,6 +35,7 @@ extern "C" {
#define casadi_s2 CASADI_PREFIX(s2)
#define casadi_s3 CASADI_PREFIX(s3)
#define casadi_s4 CASADI_PREFIX(s4)
#define casadi_s5 CASADI_PREFIX(s5)
/* Symbol visibility in DLLs */
#ifndef CASADI_SYMBOL_EXPORT
@@ -56,13 +57,14 @@ static const casadi_int casadi_s1[3] = {0, 0, 0};
static const casadi_int casadi_s2[6] = {2, 1, 0, 2, 0, 1};
static const casadi_int casadi_s3[7] = {3, 1, 0, 3, 0, 1, 2};
static const casadi_int casadi_s4[9] = {4, 3, 0, 1, 2, 3, 1, 2, 3};
static const casadi_int casadi_s5[3] = {3, 0, 0};
/* lat_cost_y_e_fun_jac_ut_xt:(i0[4],i1[],i2[2])->(o0[3],o1[4x3,3nz]) */
/* lat_cost_y_e_fun_jac_ut_xt:(i0[4],i1[],i2[],i3[2])->(o0[3],o1[4x3,3nz],o2[3x0]) */
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
casadi_real a0, a1;
a0=arg[0]? arg[0][1] : 0;
if (res[0]!=0) res[0][0]=a0;
a0=arg[2]? arg[2][0] : 0;
a0=arg[3]? arg[3][0] : 0;
a1=10.;
a0=(a0+a1);
a1=arg[0]? arg[0][2] : 0;
@@ -106,9 +108,9 @@ CASADI_SYMBOL_EXPORT void lat_cost_y_e_fun_jac_ut_xt_incref(void) {
CASADI_SYMBOL_EXPORT void lat_cost_y_e_fun_jac_ut_xt_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_e_fun_jac_ut_xt_n_in(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_e_fun_jac_ut_xt_n_in(void) { return 4;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_e_fun_jac_ut_xt_n_out(void) { return 2;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_e_fun_jac_ut_xt_n_out(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_real lat_cost_y_e_fun_jac_ut_xt_default_in(casadi_int i) {
switch (i) {
@@ -121,6 +123,7 @@ CASADI_SYMBOL_EXPORT const char* lat_cost_y_e_fun_jac_ut_xt_name_in(casadi_int i
case 0: return "i0";
case 1: return "i1";
case 2: return "i2";
case 3: return "i3";
default: return 0;
}
}
@@ -129,6 +132,7 @@ CASADI_SYMBOL_EXPORT const char* lat_cost_y_e_fun_jac_ut_xt_name_out(casadi_int
switch (i) {
case 0: return "o0";
case 1: return "o1";
case 2: return "o2";
default: return 0;
}
}
@@ -137,7 +141,8 @@ CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_e_fun_jac_ut_xt_sparsity_in(ca
switch (i) {
case 0: return casadi_s0;
case 1: return casadi_s1;
case 2: return casadi_s2;
case 2: return casadi_s1;
case 3: return casadi_s2;
default: return 0;
}
}
@@ -146,13 +151,14 @@ CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_e_fun_jac_ut_xt_sparsity_out(c
switch (i) {
case 0: return casadi_s3;
case 1: return casadi_s4;
case 2: return casadi_s5;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT int lat_cost_y_e_fun_jac_ut_xt_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
if (sz_arg) *sz_arg = 3;
if (sz_res) *sz_res = 2;
if (sz_arg) *sz_arg = 4;
if (sz_res) *sz_res = 3;
if (sz_iw) *sz_iw = 0;
if (sz_w) *sz_w = 0;
return 0;

12
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_e_hess.c
View File

@@ -57,7 +57,7 @@ static const casadi_int casadi_s2[7] = {3, 1, 0, 3, 0, 1, 2};
static const casadi_int casadi_s3[6] = {2, 1, 0, 2, 0, 1};
static const casadi_int casadi_s4[7] = {4, 4, 0, 0, 0, 0, 0};
/* lat_cost_y_e_hess:(i0[4],i1[],i2[3],i3[2])->(o0[4x4,0nz]) */
/* lat_cost_y_e_hess:(i0[4],i1[],i2[],i3[3],i4[2])->(o0[4x4,0nz]) */
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
return 0;
}
@@ -90,7 +90,7 @@ CASADI_SYMBOL_EXPORT void lat_cost_y_e_hess_incref(void) {
CASADI_SYMBOL_EXPORT void lat_cost_y_e_hess_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_e_hess_n_in(void) { return 4;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_e_hess_n_in(void) { return 5;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_e_hess_n_out(void) { return 1;}
@@ -106,6 +106,7 @@ CASADI_SYMBOL_EXPORT const char* lat_cost_y_e_hess_name_in(casadi_int i) {
case 1: return "i1";
case 2: return "i2";
case 3: return "i3";
case 4: return "i4";
default: return 0;
}
}
@@ -121,8 +122,9 @@ CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_e_hess_sparsity_in(casadi_int
switch (i) {
case 0: return casadi_s0;
case 1: return casadi_s1;
case 2: return casadi_s2;
case 3: return casadi_s3;
case 2: return casadi_s1;
case 3: return casadi_s2;
case 4: return casadi_s3;
default: return 0;
}
}
@@ -135,7 +137,7 @@ CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_e_hess_sparsity_out(casadi_int
}
CASADI_SYMBOL_EXPORT int lat_cost_y_e_hess_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
if (sz_arg) *sz_arg = 4;
if (sz_arg) *sz_arg = 5;
if (sz_res) *sz_res = 1;
if (sz_iw) *sz_iw = 0;
if (sz_w) *sz_w = 0;

18
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_fun.c
View File

@@ -34,6 +34,7 @@ extern "C" {
#define casadi_s1 CASADI_PREFIX(s1)
#define casadi_s2 CASADI_PREFIX(s2)
#define casadi_s3 CASADI_PREFIX(s3)
#define casadi_s4 CASADI_PREFIX(s4)
/* Symbol visibility in DLLs */
#ifndef CASADI_SYMBOL_EXPORT
@@ -52,15 +53,16 @@ extern "C" {
static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
static const casadi_int casadi_s2[6] = {2, 1, 0, 2, 0, 1};
static const casadi_int casadi_s3[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
static const casadi_int casadi_s2[3] = {0, 0, 0};
static const casadi_int casadi_s3[6] = {2, 1, 0, 2, 0, 1};
static const casadi_int casadi_s4[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
/* lat_cost_y_fun:(i0[4],i1,i2[2])->(o0[5]) */
/* lat_cost_y_fun:(i0[4],i1,i2[],i3[2])->(o0[5]) */
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
casadi_real a0, a1, a2;
a0=arg[0]? arg[0][1] : 0;
if (res[0]!=0) res[0][0]=a0;
a0=arg[2]? arg[2][0] : 0;
a0=arg[3]? arg[3][0] : 0;
a1=10.;
a1=(a0+a1);
a2=arg[0]? arg[0][2] : 0;
@@ -107,7 +109,7 @@ CASADI_SYMBOL_EXPORT void lat_cost_y_fun_incref(void) {
CASADI_SYMBOL_EXPORT void lat_cost_y_fun_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_fun_n_in(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_fun_n_in(void) { return 4;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_fun_n_out(void) { return 1;}
@@ -122,6 +124,7 @@ CASADI_SYMBOL_EXPORT const char* lat_cost_y_fun_name_in(casadi_int i) {
case 0: return "i0";
case 1: return "i1";
case 2: return "i2";
case 3: return "i3";
default: return 0;
}
}
@@ -138,19 +141,20 @@ CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_fun_sparsity_in(casadi_int i)
case 0: return casadi_s0;
case 1: return casadi_s1;
case 2: return casadi_s2;
case 3: return casadi_s3;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_fun_sparsity_out(casadi_int i) {
switch (i) {
case 0: return casadi_s3;
case 0: return casadi_s4;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT int lat_cost_y_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
if (sz_arg) *sz_arg = 3;
if (sz_arg) *sz_arg = 4;
if (sz_res) *sz_res = 1;
if (sz_iw) *sz_iw = 0;
if (sz_w) *sz_w = 0;

30
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_fun_jac_ut_xt.c
View File

@@ -35,6 +35,8 @@ extern "C" {
#define casadi_s2 CASADI_PREFIX(s2)
#define casadi_s3 CASADI_PREFIX(s3)
#define casadi_s4 CASADI_PREFIX(s4)
#define casadi_s5 CASADI_PREFIX(s5)
#define casadi_s6 CASADI_PREFIX(s6)
/* Symbol visibility in DLLs */
#ifndef CASADI_SYMBOL_EXPORT
@@ -53,16 +55,18 @@ extern "C" {
static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
static const casadi_int casadi_s2[6] = {2, 1, 0, 2, 0, 1};
static const casadi_int casadi_s3[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
static const casadi_int casadi_s4[13] = {5, 5, 0, 1, 2, 3, 4, 5, 2, 3, 4, 0, 0};
static const casadi_int casadi_s2[3] = {0, 0, 0};
static const casadi_int casadi_s3[6] = {2, 1, 0, 2, 0, 1};
static const casadi_int casadi_s4[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
static const casadi_int casadi_s5[13] = {5, 5, 0, 1, 2, 3, 4, 5, 2, 3, 4, 0, 0};
static const casadi_int casadi_s6[3] = {5, 0, 0};
/* lat_cost_y_fun_jac_ut_xt:(i0[4],i1,i2[2])->(o0[5],o1[5x5,5nz]) */
/* lat_cost_y_fun_jac_ut_xt:(i0[4],i1,i2[],i3[2])->(o0[5],o1[5x5,5nz],o2[5x0]) */
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
casadi_real a0, a1, a2, a3;
a0=arg[0]? arg[0][1] : 0;
if (res[0]!=0) res[0][0]=a0;
a0=arg[2]? arg[2][0] : 0;
a0=arg[3]? arg[3][0] : 0;
a1=10.;
a1=(a0+a1);
a2=arg[0]? arg[0][2] : 0;
@@ -116,9 +120,9 @@ CASADI_SYMBOL_EXPORT void lat_cost_y_fun_jac_ut_xt_incref(void) {
CASADI_SYMBOL_EXPORT void lat_cost_y_fun_jac_ut_xt_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_fun_jac_ut_xt_n_in(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_fun_jac_ut_xt_n_in(void) { return 4;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_fun_jac_ut_xt_n_out(void) { return 2;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_fun_jac_ut_xt_n_out(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_real lat_cost_y_fun_jac_ut_xt_default_in(casadi_int i) {
switch (i) {
@@ -131,6 +135,7 @@ CASADI_SYMBOL_EXPORT const char* lat_cost_y_fun_jac_ut_xt_name_in(casadi_int i)
case 0: return "i0";
case 1: return "i1";
case 2: return "i2";
case 3: return "i3";
default: return 0;
}
}
@@ -139,6 +144,7 @@ CASADI_SYMBOL_EXPORT const char* lat_cost_y_fun_jac_ut_xt_name_out(casadi_int i)
switch (i) {
case 0: return "o0";
case 1: return "o1";
case 2: return "o2";
default: return 0;
}
}
@@ -148,21 +154,23 @@ CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_fun_jac_ut_xt_sparsity_in(casa
case 0: return casadi_s0;
case 1: return casadi_s1;
case 2: return casadi_s2;
case 3: return casadi_s3;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_fun_jac_ut_xt_sparsity_out(casadi_int i) {
switch (i) {
case 0: return casadi_s3;
case 1: return casadi_s4;
case 0: return casadi_s4;
case 1: return casadi_s5;
case 2: return casadi_s6;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT int lat_cost_y_fun_jac_ut_xt_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
if (sz_arg) *sz_arg = 3;
if (sz_res) *sz_res = 2;
if (sz_arg) *sz_arg = 4;
if (sz_res) *sz_res = 3;
if (sz_iw) *sz_iw = 0;
if (sz_w) *sz_w = 0;
return 0;

18
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_hess.c
View File

@@ -35,6 +35,7 @@ extern "C" {
#define casadi_s2 CASADI_PREFIX(s2)
#define casadi_s3 CASADI_PREFIX(s3)
#define casadi_s4 CASADI_PREFIX(s4)
#define casadi_s5 CASADI_PREFIX(s5)
/* Symbol visibility in DLLs */
#ifndef CASADI_SYMBOL_EXPORT
@@ -53,11 +54,12 @@ extern "C" {
static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
static const casadi_int casadi_s2[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
static const casadi_int casadi_s3[6] = {2, 1, 0, 2, 0, 1};
static const casadi_int casadi_s4[8] = {5, 5, 0, 0, 0, 0, 0, 0};
static const casadi_int casadi_s2[3] = {0, 0, 0};
static const casadi_int casadi_s3[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
static const casadi_int casadi_s4[6] = {2, 1, 0, 2, 0, 1};
static const casadi_int casadi_s5[8] = {5, 5, 0, 0, 0, 0, 0, 0};
/* lat_cost_y_hess:(i0[4],i1,i2[5],i3[2])->(o0[5x5,0nz]) */
/* lat_cost_y_hess:(i0[4],i1,i2[],i3[5],i4[2])->(o0[5x5,0nz]) */
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
return 0;
}
@@ -90,7 +92,7 @@ CASADI_SYMBOL_EXPORT void lat_cost_y_hess_incref(void) {
CASADI_SYMBOL_EXPORT void lat_cost_y_hess_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_hess_n_in(void) { return 4;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_hess_n_in(void) { return 5;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_hess_n_out(void) { return 1;}
@@ -106,6 +108,7 @@ CASADI_SYMBOL_EXPORT const char* lat_cost_y_hess_name_in(casadi_int i) {
case 1: return "i1";
case 2: return "i2";
case 3: return "i3";
case 4: return "i4";
default: return 0;
}
}
@@ -123,19 +126,20 @@ CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_hess_sparsity_in(casadi_int i)
case 1: return casadi_s1;
case 2: return casadi_s2;
case 3: return casadi_s3;
case 4: return casadi_s4;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_hess_sparsity_out(casadi_int i) {
switch (i) {
case 0: return casadi_s4;
case 0: return casadi_s5;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT int lat_cost_y_hess_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
if (sz_arg) *sz_arg = 4;
if (sz_arg) *sz_arg = 5;
if (sz_res) *sz_res = 1;
if (sz_iw) *sz_iw = 0;
if (sz_w) *sz_w = 0;

5
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_model/lat_model.h
View File

@@ -1,8 +1,5 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
* Copyright (c) The acados authors.
*
* This file is part of acados.
*

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

Binary file not shown.

28
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/main_lat.c
View File

@@ -1,8 +1,5 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
@@ -42,6 +39,9 @@
#include "acados_c/external_function_interface.h"
#include "acados_solver_lat.h"
// blasfeo
#include "blasfeo/include/blasfeo_d_aux_ext_dep.h"
#define NX LAT_NX
#define NZ LAT_NZ
#define NU LAT_NU
@@ -105,14 +105,14 @@ int main()
double lbx0[NBX0];
double ubx0[NBX0];
lbx0[0] = 0;
ubx0[0] = 0;
lbx0[1] = 0;
ubx0[1] = 0;
lbx0[2] = 0;
ubx0[2] = 0;
lbx0[3] = 0;
ubx0[3] = 0;
lbx0[0] = 0.0;
ubx0[0] = 0.0;
lbx0[1] = 0.0;
ubx0[1] = 0.0;
lbx0[2] = 0.0;
ubx0[2] = 0.0;
lbx0[3] = 0.0;
ubx0[3] = 0.0;
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxbx", idxbx0);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", lbx0);
@@ -130,8 +130,8 @@ int main()
u0[0] = 0.0;
// set parameters
double p[NP];
p[0] = 0;
p[1] = 0;
p[0] = 0.0;
p[1] = 0.0;
for (int ii = 0; ii <= N; ii++)
{

17
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/main_sim_lat.c
View File

@@ -1,8 +1,5 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
@@ -72,10 +69,10 @@ int main()
x_current[3] = 0.0;
x_current[0] = 0;
x_current[1] = 0;
x_current[2] = 0;
x_current[3] = 0;
x_current[0] = 0.0;
x_current[1] = 0.0;
x_current[2] = 0.0;
x_current[3] = 0.0;
@@ -85,8 +82,8 @@ int main()
u0[0] = 0.0;
// set parameters
double p[NP];
p[0] = 0;
p[1] = 0;
p[0] = 0.0;
p[1] = 0.0;
lat_acados_sim_update_params(capsule, p, NP);

125
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/make_sfun_lat.m
View File

@@ -1,125 +0,0 @@
%
% Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
% Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
% Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
% Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
%
% 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.;
%
SOURCES = { ...
'lat_model/lat_expl_ode_fun.c', ...
'lat_model/lat_expl_vde_forw.c',...
'lat_cost/lat_cost_y_0_fun.c',...
'lat_cost/lat_cost_y_0_fun_jac_ut_xt.c',...
'lat_cost/lat_cost_y_0_hess.c',...
'lat_cost/lat_cost_y_fun.c',...
'lat_cost/lat_cost_y_fun_jac_ut_xt.c',...
'lat_cost/lat_cost_y_hess.c',...
'lat_cost/lat_cost_y_e_fun.c',...
'lat_cost/lat_cost_y_e_fun_jac_ut_xt.c',...
'lat_cost/lat_cost_y_e_hess.c',...
'acados_solver_sfunction_lat.c', ...
'acados_solver_lat.c'
};
INC_PATH = '/data/openpilot/third_party/acados/include';
INCS = {['-I', fullfile(INC_PATH, 'blasfeo', 'include')], ...
['-I', fullfile(INC_PATH, 'hpipm', 'include')], ...
['-I', fullfile(INC_PATH, 'acados')], ...
['-I', fullfile(INC_PATH)]};
CFLAGS = 'CFLAGS=$CFLAGS';
LDFLAGS = 'LDFLAGS=$LDFLAGS';
COMPFLAGS = 'COMPFLAGS=$COMPFLAGS';
COMPDEFINES = 'COMPDEFINES=$COMPDEFINES';
LIB_PATH = ['-L', fullfile('/data/openpilot/third_party/acados/lib')];
LIBS = {'-lacados', '-lhpipm', '-lblasfeo'};
% acados linking libraries and flags
mex('-v', '-O', CFLAGS, LDFLAGS, COMPFLAGS, COMPDEFINES, INCS{:}, ...
LIB_PATH, LIBS{:}, SOURCES{:}, ...
'-output', 'acados_solver_sfunction_lat' );
fprintf( [ '\n\nSuccessfully created sfunction:\nacados_solver_sfunction_lat', '.', ...
eval('mexext')] );
%% print note on usage of s-function
fprintf('\n\nNote: Usage of Sfunction is as follows:\n')
input_note = 'Inputs are:\n';
i_in = 1;
input_note = strcat(input_note, num2str(i_in), ') lbx_0 - lower bound on x for stage 0,',...
' size [4]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') ubx_0 - upper bound on x for stage 0,',...
' size [4]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') parameters - concatenated for all shooting nodes 0 to N+1,',...
' size [66]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') y_ref_0, size [5]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') y_ref - concatenated for shooting nodes 1 to N-1,',...
' size [155]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') y_ref_e, size [3]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') lbx for shooting nodes 1 to N-1, size [62]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') ubx for shooting nodes 1 to N-1, size [62]\n ');
i_in = i_in + 1;
fprintf(input_note)
disp(' ')
output_note = 'Outputs are:\n';
i_out = 0;
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') u0, control input at node 0, size [1]\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') acados solver status (0 = SUCCESS)\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') KKT residual\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') x1, state at node 1\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') CPU time\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') SQP iterations\n ');
fprintf(output_note)