mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-02-21 11:13: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/longitudinal_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+= long_model/long_expl_ode_fun.c
MODEL_SRC+= long_model/long_expl_vde_forw.c
MODEL_SRC+= long_model/long_expl_vde_adj.c
MODEL_OBJ := $(MODEL_SRC:.c=.o)
# optimal control problem - mostly CasADi exports
@@ -56,6 +54,7 @@ OCP_SRC+= long_cost/long_cost_y_hess.c
OCP_SRC+= long_cost/long_cost_y_e_fun.c
OCP_SRC+= long_cost/long_cost_y_e_fun_jac_ut_xt.c
OCP_SRC+= long_cost/long_cost_y_e_hess.c
OCP_SRC+= acados_solver_long.c
OCP_OBJ := $(OCP_SRC:.c=.o)
@@ -93,7 +92,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
@@ -150,11 +149,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
@@ -165,6 +164,33 @@ ocp_cython: ocp_cython_o
$(abspath .)/libacados_ocp_solver_long.so \
$(LDFLAGS) $(LDLIBS)
# Sim Cython targets
sim_cython_c: sim_shared_lib
cython \
-o acados_sim_solver_pyx.c \
-I $(INCLUDE_PATH)/../interfaces/acados_template/acados_template \
$(INCLUDE_PATH)/../interfaces/acados_template/acados_template/acados_sim_solver_pyx.pyx \
-I /data/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code \
sim_cython_o: sim_cython_c
$(CC) $(ACADOS_FLAGS) -c -O2 \
-fPIC \
-o acados_sim_solver_pyx.o \
-I $(INCLUDE_PATH)/blasfeo/include/ \
-I $(INCLUDE_PATH)/hpipm/include/ \
-I $(INCLUDE_PATH) \
-I /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_long.so \
$(LDFLAGS) $(LDLIBS)
clean:
$(RM) $(OBJ) $(EX_OBJ) $(EX_SIM_OBJ)
$(RM) $(LIBACADOS_SOLVER) $(LIBACADOS_OCP_SOLVER) $(LIBACADOS_SIM_SOLVER)
@@ -179,3 +205,9 @@ clean_ocp_cython:
$(RM) acados_solver_long.o
$(RM) acados_ocp_solver_pyx.so
$(RM) acados_ocp_solver_pyx.o
clean_sim_cython:
$(RM) libacados_sim_solver_long.so
$(RM) acados_sim_solver_long.o
$(RM) acados_sim_solver_pyx.so
$(RM) acados_sim_solver_pyx.o

7575
selfdrive/controls/lib/longitudinal_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/longitudinal_mpc_lib/c_generated_code/acados_ocp_solver_pyx.so
View File

Binary file not shown.

22
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_sim_solver_long.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 long_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 = &long_expl_vde_forw;
@@ -92,6 +90,14 @@ int long_acados_sim_create(sim_solver_capsule * capsule)
capsule->sim_forw_vde_casadi->casadi_work = &long_expl_vde_forw_work;
external_function_param_casadi_create(capsule->sim_forw_vde_casadi, np);
capsule->sim_vde_adj_casadi->casadi_fun = &long_expl_vde_adj;
capsule->sim_vde_adj_casadi->casadi_n_in = &long_expl_vde_adj_n_in;
capsule->sim_vde_adj_casadi->casadi_n_out = &long_expl_vde_adj_n_out;
capsule->sim_vde_adj_casadi->casadi_sparsity_in = &long_expl_vde_adj_sparsity_in;
capsule->sim_vde_adj_casadi->casadi_sparsity_out = &long_expl_vde_adj_sparsity_out;
capsule->sim_vde_adj_casadi->casadi_work = &long_expl_vde_adj_work;
external_function_param_casadi_create(capsule->sim_vde_adj_casadi, np);
capsule->sim_expl_ode_fun_casadi->casadi_fun = &long_expl_ode_fun;
capsule->sim_expl_ode_fun_casadi->casadi_n_in = &long_expl_ode_fun_n_in;
capsule->sim_expl_ode_fun_casadi->casadi_n_out = &long_expl_ode_fun_n_out;
@@ -123,6 +129,8 @@ int long_acados_sim_create(sim_solver_capsule * capsule)
capsule->acados_sim_opts = long_sim_opts;
int tmp_int = 3;
sim_opts_set(long_sim_config, long_sim_opts, "newton_iter", &tmp_int);
double tmp_double = 0.0;
sim_opts_set(long_sim_config, long_sim_opts, "newton_tol", &tmp_double);
sim_collocation_type collocation_type = GAUSS_LEGENDRE;
sim_opts_set(long_sim_config, long_sim_opts, "collocation_type", &collocation_type);
@@ -146,7 +154,9 @@ int long_acados_sim_create(sim_solver_capsule * capsule)
// model functions
long_sim_config->model_set(long_sim_in->model,
"expl_vde_for", capsule->sim_forw_vde_casadi);
"expl_vde_forw", capsule->sim_forw_vde_casadi);
long_sim_config->model_set(long_sim_in->model,
"expl_vde_adj", capsule->sim_vde_adj_casadi);
long_sim_config->model_set(long_sim_in->model,
"expl_ode_fun", capsule->sim_expl_ode_fun_casadi);
@@ -227,6 +237,7 @@ int long_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;
@@ -244,6 +255,7 @@ int long_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/longitudinal_mpc_lib/c_generated_code/acados_sim_solver_long.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/longitudinal_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_long.h":
int acados_update_qp_solver_cond_N "long_acados_update_qp_solver_cond_N"(nlp_solver_capsule * capsule, int qp_solver_cond_N)
int acados_update_params "long_acados_update_params"(nlp_solver_capsule * capsule, int stage, double *value, int np_)
int acados_update_params_sparse "long_acados_update_params_sparse"(nlp_solver_capsule * capsule, int stage, int *idx, double *p, int n_update)
int acados_solve "long_acados_solve"(nlp_solver_capsule * capsule)
int acados_reset "long_acados_reset"(nlp_solver_capsule * capsule)
int acados_reset "long_acados_reset"(nlp_solver_capsule * capsule, int reset_qp_solver_mem)
int acados_free "long_acados_free"(nlp_solver_capsule * capsule)
void acados_print_stats "long_acados_print_stats"(nlp_solver_capsule * capsule)
int acados_custom_update "long_acados_custom_update"(nlp_solver_capsule* capsule, double * data, int data_len)
acados_solver_common.ocp_nlp_in *acados_get_nlp_in "long_acados_get_nlp_in"(nlp_solver_capsule * capsule)
acados_solver_common.ocp_nlp_out *acados_get_nlp_out "long_acados_get_nlp_out"(nlp_solver_capsule * capsule)
acados_solver_common.ocp_nlp_out *acados_get_sens_out "long_acados_get_sens_out"(nlp_solver_capsule * capsule)

191
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_solver_long.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,15 +39,11 @@
// example specific
#include "long_model/long_model.h"
#include "long_constraints/long_constraints.h"
#include "long_cost/long_cost.h"
#include "long_constraints/long_h_constraint.h"
#include "long_cost/long_cost_y_fun.h"
#include "long_cost/long_cost_y_0_fun.h"
#include "long_cost/long_cost_y_e_fun.h"
#include "acados_solver_long.h"
@@ -293,7 +286,6 @@ return nlp_dims;
void long_acados_create_3_create_and_set_functions(long_solver_capsule* capsule)
{
const int N = capsule->nlp_solver_plan->N;
ocp_nlp_config* nlp_config = capsule->nlp_config;
/************************************************
* external functions
@@ -433,28 +425,38 @@ void long_acados_create_5_set_nlp_in(long_solver_capsule* capsule, const int N,
}
/**** 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);
@@ -464,6 +466,10 @@ void long_acados_create_5_set_nlp_in(long_solver_capsule* capsule, const int N,
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]);
}
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);
// slacks
double* zlumem = calloc(4*NS, sizeof(double));
double* Zl = zlumem+NS*0;
double* Zu = zlumem+NS*1;
@@ -480,20 +486,6 @@ void long_acados_create_5_set_nlp_in(long_solver_capsule* capsule, const int N,
}
free(zlumem);
// 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);
/**** Constraints ****/
@@ -565,11 +557,11 @@ void long_acados_create_5_set_nlp_in(long_solver_capsule* capsule, const int N,
uh[0] = 10000;
uh[1] = 10000;
uh[2] = 10000;
uh[3] = 10000;
uh[0] = 10000.0;
uh[1] = 10000.0;
uh[2] = 10000.0;
uh[3] = 10000.0;
for (int i = 0; i < N; i++)
{
// nonlinear constraints for stages 0 to N-1
@@ -611,7 +603,6 @@ void long_acados_create_6_set_opts(long_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;
/************************************************
@@ -649,10 +640,10 @@ void long_acados_create_6_set_opts(long_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 */
@@ -661,6 +652,9 @@ void long_acados_create_6_set_opts(long_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");
@@ -679,6 +673,11 @@ void long_acados_create_6_set_opts(long_solver_capsule* capsule)
double qp_solver_tol_comp = 0.001;
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_tol_comp", &qp_solver_tol_comp);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;
@@ -787,6 +786,7 @@ int long_acados_create_with_discretization(long_solver_capsule* capsule, int N,
// 9) do precomputations
int status = long_acados_create_9_precompute(capsule);
return status;
}
@@ -814,7 +814,7 @@ int long_acados_update_qp_solver_cond_N(long_solver_capsule* capsule, int qp_sol
}
int long_acados_reset(long_solver_capsule* capsule)
int long_acados_reset(long_solver_capsule* capsule, int reset_qp_solver_mem)
{
// set initialization to all zeros
@@ -826,8 +826,6 @@ int long_acados_reset(long_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++)
@@ -847,7 +845,7 @@ int long_acados_reset(long_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);
@@ -870,6 +868,7 @@ int long_acados_update_params(long_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)
{
@@ -908,15 +907,74 @@ int long_acados_update_params(long_solver_capsule* capsule, int stage, double *p
}
return solver_status;
}
int long_acados_update_params_sparse(long_solver_capsule * capsule, int stage, int *idx, double *p, int n_update)
{
int solver_status = 0;
int casadi_np = 6;
if (casadi_np < n_update) {
printf("long_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("long_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
capsule->nl_constr_h_fun_jac[stage].set_param_sparse(capsule->nl_constr_h_fun_jac+stage, n_update, idx, p);
capsule->nl_constr_h_fun[stage].set_param_sparse(capsule->nl_constr_h_fun+stage, n_update, idx, p);
// 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 long_acados_solve(long_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;
@@ -976,15 +1034,6 @@ int long_acados_free(long_solver_capsule* capsule)
return 0;
}
ocp_nlp_in *long_acados_get_nlp_in(long_solver_capsule* capsule) { return capsule->nlp_in; }
ocp_nlp_out *long_acados_get_nlp_out(long_solver_capsule* capsule) { return capsule->nlp_out; }
ocp_nlp_out *long_acados_get_sens_out(long_solver_capsule* capsule) { return capsule->sens_out; }
ocp_nlp_solver *long_acados_get_nlp_solver(long_solver_capsule* capsule) { return capsule->nlp_solver; }
ocp_nlp_config *long_acados_get_nlp_config(long_solver_capsule* capsule) { return capsule->nlp_config; }
void *long_acados_get_nlp_opts(long_solver_capsule* capsule) { return capsule->nlp_opts; }
ocp_nlp_dims *long_acados_get_nlp_dims(long_solver_capsule* capsule) { return capsule->nlp_dims; }
ocp_nlp_plan_t *long_acados_get_nlp_plan(long_solver_capsule* capsule) { return capsule->nlp_solver_plan; }
void long_acados_print_stats(long_solver_capsule* capsule)
{
@@ -998,6 +1047,11 @@ void long_acados_print_stats(long_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++)
{
@@ -1010,3 +1064,24 @@ void long_acados_print_stats(long_solver_capsule* capsule)
}
}
int long_acados_custom_update(long_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 *long_acados_get_nlp_in(long_solver_capsule* capsule) { return capsule->nlp_in; }
ocp_nlp_out *long_acados_get_nlp_out(long_solver_capsule* capsule) { return capsule->nlp_out; }
ocp_nlp_out *long_acados_get_sens_out(long_solver_capsule* capsule) { return capsule->sens_out; }
ocp_nlp_solver *long_acados_get_nlp_solver(long_solver_capsule* capsule) { return capsule->nlp_solver; }
ocp_nlp_config *long_acados_get_nlp_config(long_solver_capsule* capsule) { return capsule->nlp_config; }
void *long_acados_get_nlp_opts(long_solver_capsule* capsule) { return capsule->nlp_opts; }
ocp_nlp_dims *long_acados_get_nlp_dims(long_solver_capsule* capsule) { return capsule->nlp_dims; }
ocp_nlp_plan_t *long_acados_get_nlp_plan(long_solver_capsule* capsule) { return capsule->nlp_solver_plan; }

16
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_solver_long.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 long_solver_capsule
{
@@ -106,6 +104,7 @@ typedef struct long_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;
@@ -120,7 +119,6 @@ typedef struct long_solver_capsule
// constraints
external_function_param_casadi *nl_constr_h_fun_jac;
external_function_param_casadi *nl_constr_h_fun;
external_function_param_casadi *nl_constr_h_fun_jac_hess;
@@ -132,7 +130,7 @@ ACADOS_SYMBOL_EXPORT int long_acados_free_capsule(long_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT int long_acados_create(long_solver_capsule * capsule);
ACADOS_SYMBOL_EXPORT int long_acados_reset(long_solver_capsule* capsule);
ACADOS_SYMBOL_EXPORT int long_acados_reset(long_solver_capsule* capsule, int reset_qp_solver_mem);
/**
* Generic version of long_acados_create which allows to use a different number of shooting intervals than
@@ -150,10 +148,14 @@ ACADOS_SYMBOL_EXPORT int long_acados_update_time_steps(long_solver_capsule * cap
*/
ACADOS_SYMBOL_EXPORT int long_acados_update_qp_solver_cond_N(long_solver_capsule * capsule, int qp_solver_cond_N);
ACADOS_SYMBOL_EXPORT int long_acados_update_params(long_solver_capsule * capsule, int stage, double *value, int np);
ACADOS_SYMBOL_EXPORT int long_acados_update_params_sparse(long_solver_capsule * capsule, int stage, int *idx, double *p, int n_update);
ACADOS_SYMBOL_EXPORT int long_acados_solve(long_solver_capsule * capsule);
ACADOS_SYMBOL_EXPORT int long_acados_free(long_solver_capsule * capsule);
ACADOS_SYMBOL_EXPORT void long_acados_print_stats(long_solver_capsule * capsule);
ACADOS_SYMBOL_EXPORT int long_acados_custom_update(long_solver_capsule* capsule, double* data, int data_len);
ACADOS_SYMBOL_EXPORT ocp_nlp_in *long_acados_get_nlp_in(long_solver_capsule * capsule);
ACADOS_SYMBOL_EXPORT ocp_nlp_out *long_acados_get_nlp_out(long_solver_capsule * capsule);
ACADOS_SYMBOL_EXPORT ocp_nlp_out *long_acados_get_sens_out(long_solver_capsule * capsule);

264
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_solver_sfunction_long.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_long
#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 "long_model/long_model.h"
#include "acados_solver_long.h"
#include "simstruc.h"
#define SAMPLINGTIME 0.06944444444444445
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, 3);
// ubx_0
ssSetInputPortVectorDimension(S, 1, 3);
// parameters
ssSetInputPortVectorDimension(S, 2, (12+1) * 6);
// y_ref_0
ssSetInputPortVectorDimension(S, 3, 6);
// y_ref
ssSetInputPortVectorDimension(S, 4, 66);
// y_ref_e
ssSetInputPortVectorDimension(S, 5, 5);
// lh
ssSetInputPortVectorDimension(S, 6, 4);
// uh
ssSetInputPortVectorDimension(S, 7, 4);/* specify dimension information for the OUTPUT ports */
ssSetOutputPortVectorDimension(S, 0, 1 );
ssSetOutputPortVectorDimension(S, 1, 1 );
ssSetOutputPortVectorDimension(S, 2, 1 );
ssSetOutputPortVectorDimension(S, 3, 3 ); // 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)
{
long_solver_capsule *capsule = long_acados_create_capsule();
long_acados_create(capsule);
ssSetUserData(S, (void*)capsule);
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
long_solver_capsule *capsule = ssGetUserData(S);
ocp_nlp_config *nlp_config = long_acados_get_nlp_config(capsule);
ocp_nlp_dims *nlp_dims = long_acados_get_nlp_dims(capsule);
ocp_nlp_in *nlp_in = long_acados_get_nlp_in(capsule);
ocp_nlp_out *nlp_out = long_acados_get_nlp_out(capsule);
InputRealPtrsType in_sign;
// local buffer
real_t buffer[6];
/* go through inputs */
// lbx_0
in_sign = ssGetInputPortRealSignalPtrs(S, 0);
for (int i = 0; i < 3; 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 < 3; 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 <= 12; ii++)
{
for (int jj = 0; jj < 6; jj++)
buffer[jj] = (double)(*in_sign[ii*6+jj]);
long_acados_update_params(capsule, ii, buffer, 6);
}
// y_ref_0
in_sign = ssGetInputPortRealSignalPtrs(S, 3);
for (int i = 0; i < 6; 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 < 12; ii++)
{
for (int jj = 0; jj < 6; jj++)
buffer[jj] = (double)(*in_sign[(ii-1)*6+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 < 5; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 12, "yref", (void *) buffer);
// lh
in_sign = ssGetInputPortRealSignalPtrs(S, 6);
for (int i = 0; i < 4; i++)
buffer[i] = (double)(*in_sign[i]);
for (int ii = 0; ii < 12; ii++)
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lh", buffer);
// uh
in_sign = ssGetInputPortRealSignalPtrs(S, 7);
for (int i = 0; i < 4; i++)
buffer[i] = (double)(*in_sign[i]);
for (int ii = 0; ii < 12; ii++)
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "uh", buffer);
/* call solver */
int rti_phase = 0;
ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "rti_phase", &rti_phase);
int acados_status = long_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)
{
long_solver_capsule *capsule = ssGetUserData(S);
long_acados_free(capsule);
long_acados_free_capsule(capsule);
}
#ifdef MATLAB_MEX_FILE
#include "simulink.c"
#else
#include "cg_sfun.h"
#endif

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

Binary file not shown.

17
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_constraints/long_h_constraint.h → selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_constraints/long_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,14 +28,18 @@
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef long_H_CONSTRAINT
#define long_H_CONSTRAINT
#ifndef long_CONSTRAINTS
#define long_CONSTRAINTS
#ifdef __cplusplus
extern "C" {
#endif
int long_constr_h_fun_jac_uxt_zt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int long_constr_h_fun_jac_uxt_zt_work(int *, int *, int *, int *);
const int *long_constr_h_fun_jac_uxt_zt_sparsity_in(int);
@@ -56,8 +57,10 @@ int long_constr_h_fun_n_out(void);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // long_H_CONSTRAINT
#endif // long_CONSTRAINTS

64
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost_y_0_fun.h → selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_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 long_Y_0_COST
#define long_Y_0_COST
#ifndef long_COST
#define long_COST
#ifdef __cplusplus
extern "C" {
#endif
// Cost at initial shooting node
int long_cost_y_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int long_cost_y_0_fun_work(int *, int *, int *, int *);
const int *long_cost_y_0_fun_sparsity_in(int);
@@ -62,8 +61,59 @@ int long_cost_y_0_hess_n_in(void);
int long_cost_y_0_hess_n_out(void);
// Cost at path shooting node
int long_cost_y_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int long_cost_y_fun_work(int *, int *, int *, int *);
const int *long_cost_y_fun_sparsity_in(int);
const int *long_cost_y_fun_sparsity_out(int);
int long_cost_y_fun_n_in(void);
int long_cost_y_fun_n_out(void);
int long_cost_y_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int long_cost_y_fun_jac_ut_xt_work(int *, int *, int *, int *);
const int *long_cost_y_fun_jac_ut_xt_sparsity_in(int);
const int *long_cost_y_fun_jac_ut_xt_sparsity_out(int);
int long_cost_y_fun_jac_ut_xt_n_in(void);
int long_cost_y_fun_jac_ut_xt_n_out(void);
int long_cost_y_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int long_cost_y_hess_work(int *, int *, int *, int *);
const int *long_cost_y_hess_sparsity_in(int);
const int *long_cost_y_hess_sparsity_out(int);
int long_cost_y_hess_n_in(void);
int long_cost_y_hess_n_out(void);
// Cost at terminal shooting node
int long_cost_y_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int long_cost_y_e_fun_work(int *, int *, int *, int *);
const int *long_cost_y_e_fun_sparsity_in(int);
const int *long_cost_y_e_fun_sparsity_out(int);
int long_cost_y_e_fun_n_in(void);
int long_cost_y_e_fun_n_out(void);
int long_cost_y_e_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int long_cost_y_e_fun_jac_ut_xt_work(int *, int *, int *, int *);
const int *long_cost_y_e_fun_jac_ut_xt_sparsity_in(int);
const int *long_cost_y_e_fun_jac_ut_xt_sparsity_out(int);
int long_cost_y_e_fun_jac_ut_xt_n_in(void);
int long_cost_y_e_fun_jac_ut_xt_n_out(void);
int long_cost_y_e_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int long_cost_y_e_hess_work(int *, int *, int *, int *);
const int *long_cost_y_e_hess_sparsity_in(int);
const int *long_cost_y_e_hess_sparsity_out(int);
int long_cost_y_e_hess_n_in(void);
int long_cost_y_e_hess_n_out(void);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // long_Y_0_COST
#endif // long_COST

20
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost_y_0_fun.c
View File

@@ -33,6 +33,7 @@ extern "C" {
#define casadi_s0 CASADI_PREFIX(s0)
#define casadi_s1 CASADI_PREFIX(s1)
#define casadi_s2 CASADI_PREFIX(s2)
#define casadi_s3 CASADI_PREFIX(s3)
#define casadi_sq CASADI_PREFIX(sq)
/* Symbol visibility in DLLs */
@@ -54,19 +55,20 @@ casadi_real casadi_sq(casadi_real x) { return x*x;}
static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
static const casadi_int casadi_s2[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
static const casadi_int casadi_s2[3] = {0, 0, 0};
static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
/* long_cost_y_0_fun:(i0[3],i1,i2[6])->(o0[6]) */
/* long_cost_y_0_fun:(i0[3],i1,i2[],i3[6])->(o0[6]) */
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, a4;
a0=arg[2]? arg[2][2] : 0;
a0=arg[3]? arg[3][2] : 0;
a1=arg[0]? arg[0][0] : 0;
a0=(a0-a1);
a2=arg[0]? arg[0][1] : 0;
a3=casadi_sq(a2);
a4=5.;
a3=(a3/a4);
a4=arg[2]? arg[2][4] : 0;
a4=arg[3]? arg[3][4] : 0;
a4=(a4*a2);
a3=(a3+a4);
a4=6.;
@@ -80,7 +82,7 @@ static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw,
if (res[0]!=0) res[0][2]=a2;
a2=arg[0]? arg[0][2] : 0;
if (res[0]!=0) res[0][3]=a2;
a1=arg[2]? arg[2][3] : 0;
a1=arg[3]? arg[3][3] : 0;
a2=(a2-a1);
if (res[0]!=0) res[0][4]=a2;
a2=arg[1]? arg[1][0] : 0;
@@ -116,7 +118,7 @@ CASADI_SYMBOL_EXPORT void long_cost_y_0_fun_incref(void) {
CASADI_SYMBOL_EXPORT void long_cost_y_0_fun_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_0_fun_n_in(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_0_fun_n_in(void) { return 4;}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_0_fun_n_out(void) { return 1;}
@@ -131,6 +133,7 @@ CASADI_SYMBOL_EXPORT const char* long_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;
}
}
@@ -147,19 +150,20 @@ CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_0_fun_sparsity_in(casadi_int
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* long_cost_y_0_fun_sparsity_out(casadi_int i) {
switch (i) {
case 0: return casadi_s2;
case 0: return casadi_s3;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT int long_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;

32
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost_y_0_fun_jac_ut_xt.c
View File

@@ -34,6 +34,8 @@ 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)
#define casadi_s5 CASADI_PREFIX(s5)
#define casadi_sq CASADI_PREFIX(sq)
/* Symbol visibility in DLLs */
@@ -55,20 +57,22 @@ casadi_real casadi_sq(casadi_real x) { return x*x;}
static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
static const casadi_int casadi_s2[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
static const casadi_int casadi_s3[16] = {4, 6, 0, 2, 3, 4, 5, 6, 7, 1, 2, 1, 2, 3, 3, 0};
static const casadi_int casadi_s2[3] = {0, 0, 0};
static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
static const casadi_int casadi_s4[16] = {4, 6, 0, 2, 3, 4, 5, 6, 7, 1, 2, 1, 2, 3, 3, 0};
static const casadi_int casadi_s5[3] = {6, 0, 0};
/* long_cost_y_0_fun_jac_ut_xt:(i0[3],i1,i2[6])->(o0[6],o1[4x6,7nz]) */
/* long_cost_y_0_fun_jac_ut_xt:(i0[3],i1,i2[],i3[6])->(o0[6],o1[4x6,7nz],o2[6x0]) */
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, a4, a5;
a0=arg[2]? arg[2][2] : 0;
a0=arg[3]? arg[3][2] : 0;
a1=arg[0]? arg[0][0] : 0;
a0=(a0-a1);
a2=arg[0]? arg[0][1] : 0;
a3=casadi_sq(a2);
a4=5.;
a3=(a3/a4);
a4=arg[2]? arg[2][4] : 0;
a4=arg[3]? arg[3][4] : 0;
a5=(a4*a2);
a3=(a3+a5);
a5=6.;
@@ -82,7 +86,7 @@ static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw,
if (res[0]!=0) res[0][2]=a2;
a1=arg[0]? arg[0][2] : 0;
if (res[0]!=0) res[0][3]=a1;
a5=arg[2]? arg[2][3] : 0;
a5=arg[3]? arg[3][3] : 0;
a1=(a1-a5);
if (res[0]!=0) res[0][4]=a1;
a1=arg[1]? arg[1][0] : 0;
@@ -136,9 +140,9 @@ CASADI_SYMBOL_EXPORT void long_cost_y_0_fun_jac_ut_xt_incref(void) {
CASADI_SYMBOL_EXPORT void long_cost_y_0_fun_jac_ut_xt_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_0_fun_jac_ut_xt_n_in(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_0_fun_jac_ut_xt_n_in(void) { return 4;}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_0_fun_jac_ut_xt_n_out(void) { return 2;}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_0_fun_jac_ut_xt_n_out(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_real long_cost_y_0_fun_jac_ut_xt_default_in(casadi_int i) {
switch (i) {
@@ -151,6 +155,7 @@ CASADI_SYMBOL_EXPORT const char* long_cost_y_0_fun_jac_ut_xt_name_in(casadi_int
case 0: return "i0";
case 1: return "i1";
case 2: return "i2";
case 3: return "i3";
default: return 0;
}
}
@@ -159,6 +164,7 @@ CASADI_SYMBOL_EXPORT const char* long_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;
}
}
@@ -168,21 +174,23 @@ CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_0_fun_jac_ut_xt_sparsity_in(c
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* long_cost_y_0_fun_jac_ut_xt_sparsity_out(casadi_int i) {
switch (i) {
case 0: return casadi_s2;
case 1: return casadi_s3;
case 0: return casadi_s3;
case 1: return casadi_s4;
case 2: return casadi_s5;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT int long_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;

24
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost_y_0_hess.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)
#define casadi_sq CASADI_PREFIX(sq)
/* Symbol visibility in DLLs */
@@ -55,13 +56,14 @@ casadi_real casadi_sq(casadi_real x) { return x*x;}
static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
static const casadi_int casadi_s2[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
static const casadi_int casadi_s3[10] = {4, 4, 0, 0, 1, 3, 3, 2, 1, 2};
static const casadi_int casadi_s2[3] = {0, 0, 0};
static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
static const casadi_int casadi_s4[10] = {4, 4, 0, 0, 1, 3, 3, 2, 1, 2};
/* long_cost_y_0_hess:(i0[3],i1,i2[6],i3[6])->(o0[4x4,3nz]) */
/* long_cost_y_0_hess:(i0[3],i1,i2[],i3[6],i4[6])->(o0[4x4,3nz]) */
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
casadi_real a0, a1, a10, a2, a3, a4, a5, a6, a7, a8, a9;
a0=arg[2]? arg[2][0] : 0;
a0=arg[3]? arg[3][0] : 0;
a1=arg[0]? arg[0][1] : 0;
a2=10.;
a2=(a1+a2);
@@ -75,10 +77,10 @@ static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw,
a5=2.0000000000000001e-01;
a6=(a1+a1);
a6=(a5*a6);
a7=arg[3]? arg[3][4] : 0;
a7=arg[4]? arg[4][4] : 0;
a6=(a6+a7);
a6=(a6/a2);
a8=arg[3]? arg[3][2] : 0;
a8=arg[4]? arg[4][2] : 0;
a9=arg[0]? arg[0][0] : 0;
a8=(a8-a9);
a9=casadi_sq(a1);
@@ -139,7 +141,7 @@ CASADI_SYMBOL_EXPORT void long_cost_y_0_hess_incref(void) {
CASADI_SYMBOL_EXPORT void long_cost_y_0_hess_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_0_hess_n_in(void) { return 4;}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_0_hess_n_in(void) { return 5;}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_0_hess_n_out(void) { return 1;}
@@ -155,6 +157,7 @@ CASADI_SYMBOL_EXPORT const char* long_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;
}
}
@@ -171,20 +174,21 @@ CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_0_hess_sparsity_in(casadi_int
case 0: return casadi_s0;
case 1: return casadi_s1;
case 2: return casadi_s2;
case 3: return casadi_s2;
case 3: return casadi_s3;
case 4: return casadi_s3;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_0_hess_sparsity_out(casadi_int i) {
switch (i) {
case 0: return casadi_s3;
case 0: return casadi_s4;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT int long_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;

16
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost_y_e_fun.c
View File

@@ -58,17 +58,17 @@ static const casadi_int casadi_s1[3] = {0, 0, 0};
static const casadi_int casadi_s2[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
static const casadi_int casadi_s3[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
/* long_cost_y_e_fun:(i0[3],i1[],i2[6])->(o0[5]) */
/* long_cost_y_e_fun:(i0[3],i1[],i2[],i3[6])->(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, a3, a4;
a0=arg[2]? arg[2][2] : 0;
a0=arg[3]? arg[3][2] : 0;
a1=arg[0]? arg[0][0] : 0;
a0=(a0-a1);
a2=arg[0]? arg[0][1] : 0;
a3=casadi_sq(a2);
a4=5.;
a3=(a3/a4);
a4=arg[2]? arg[2][4] : 0;
a4=arg[3]? arg[3][4] : 0;
a4=(a4*a2);
a3=(a3+a4);
a4=6.;
@@ -82,7 +82,7 @@ static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw,
if (res[0]!=0) res[0][2]=a2;
a2=arg[0]? arg[0][2] : 0;
if (res[0]!=0) res[0][3]=a2;
a1=arg[2]? arg[2][3] : 0;
a1=arg[3]? arg[3][3] : 0;
a2=(a2-a1);
if (res[0]!=0) res[0][4]=a2;
return 0;
@@ -116,7 +116,7 @@ CASADI_SYMBOL_EXPORT void long_cost_y_e_fun_incref(void) {
CASADI_SYMBOL_EXPORT void long_cost_y_e_fun_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_e_fun_n_in(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_e_fun_n_in(void) { return 4;}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_e_fun_n_out(void) { return 1;}
@@ -131,6 +131,7 @@ CASADI_SYMBOL_EXPORT const char* long_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;
}
}
@@ -146,7 +147,8 @@ CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_e_fun_sparsity_in(casadi_int
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;
}
}
@@ -159,7 +161,7 @@ CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_e_fun_sparsity_out(casadi_int
}
CASADI_SYMBOL_EXPORT int long_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/longitudinal_mpc_lib/c_generated_code/long_cost/long_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 long_Y_E_COST
#define long_Y_E_COST
#ifdef __cplusplus
extern "C" {
#endif
int long_cost_y_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int long_cost_y_e_fun_work(int *, int *, int *, int *);
const int *long_cost_y_e_fun_sparsity_in(int);
const int *long_cost_y_e_fun_sparsity_out(int);
int long_cost_y_e_fun_n_in(void);
int long_cost_y_e_fun_n_out(void);
int long_cost_y_e_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int long_cost_y_e_fun_jac_ut_xt_work(int *, int *, int *, int *);
const int *long_cost_y_e_fun_jac_ut_xt_sparsity_in(int);
const int *long_cost_y_e_fun_jac_ut_xt_sparsity_out(int);
int long_cost_y_e_fun_jac_ut_xt_n_in(void);
int long_cost_y_e_fun_jac_ut_xt_n_out(void);
int long_cost_y_e_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int long_cost_y_e_hess_work(int *, int *, int *, int *);
const int *long_cost_y_e_hess_sparsity_in(int);
const int *long_cost_y_e_hess_sparsity_out(int);
int long_cost_y_e_hess_n_in(void);
int long_cost_y_e_hess_n_out(void);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // long_Y_E_COST

24
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_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)
#define casadi_sq CASADI_PREFIX(sq)
/* Symbol visibility in DLLs */
@@ -59,18 +60,19 @@ static const casadi_int casadi_s1[3] = {0, 0, 0};
static const casadi_int casadi_s2[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
static const casadi_int casadi_s3[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
static const casadi_int casadi_s4[14] = {3, 5, 0, 2, 3, 4, 5, 6, 0, 1, 0, 1, 2, 2};
static const casadi_int casadi_s5[3] = {5, 0, 0};
/* long_cost_y_e_fun_jac_ut_xt:(i0[3],i1[],i2[6])->(o0[5],o1[3x5,6nz]) */
/* long_cost_y_e_fun_jac_ut_xt:(i0[3],i1[],i2[],i3[6])->(o0[5],o1[3x5,6nz],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, a4, a5;
a0=arg[2]? arg[2][2] : 0;
a0=arg[3]? arg[3][2] : 0;
a1=arg[0]? arg[0][0] : 0;
a0=(a0-a1);
a2=arg[0]? arg[0][1] : 0;
a3=casadi_sq(a2);
a4=5.;
a3=(a3/a4);
a4=arg[2]? arg[2][4] : 0;
a4=arg[3]? arg[3][4] : 0;
a5=(a4*a2);
a3=(a3+a5);
a5=6.;
@@ -84,7 +86,7 @@ static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw,
if (res[0]!=0) res[0][2]=a2;
a1=arg[0]? arg[0][2] : 0;
if (res[0]!=0) res[0][3]=a1;
a5=arg[2]? arg[2][3] : 0;
a5=arg[3]? arg[3][3] : 0;
a1=(a1-a5);
if (res[0]!=0) res[0][4]=a1;
a1=(1./a3);
@@ -135,9 +137,9 @@ CASADI_SYMBOL_EXPORT void long_cost_y_e_fun_jac_ut_xt_incref(void) {
CASADI_SYMBOL_EXPORT void long_cost_y_e_fun_jac_ut_xt_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_e_fun_jac_ut_xt_n_in(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_e_fun_jac_ut_xt_n_in(void) { return 4;}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_e_fun_jac_ut_xt_n_out(void) { return 2;}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_e_fun_jac_ut_xt_n_out(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_real long_cost_y_e_fun_jac_ut_xt_default_in(casadi_int i) {
switch (i) {
@@ -150,6 +152,7 @@ CASADI_SYMBOL_EXPORT const char* long_cost_y_e_fun_jac_ut_xt_name_in(casadi_int
case 0: return "i0";
case 1: return "i1";
case 2: return "i2";
case 3: return "i3";
default: return 0;
}
}
@@ -158,6 +161,7 @@ CASADI_SYMBOL_EXPORT const char* long_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;
}
}
@@ -166,7 +170,8 @@ CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_e_fun_jac_ut_xt_sparsity_in(c
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;
}
}
@@ -175,13 +180,14 @@ CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_e_fun_jac_ut_xt_sparsity_out(
switch (i) {
case 0: return casadi_s3;
case 1: return casadi_s4;
case 2: return casadi_s5;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT int long_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;

18
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost_y_e_hess.c
View File

@@ -60,10 +60,10 @@ static const casadi_int casadi_s2[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
static const casadi_int casadi_s4[9] = {3, 3, 0, 1, 3, 3, 1, 0, 1};
/* long_cost_y_e_hess:(i0[3],i1[],i2[5],i3[6])->(o0[3x3,3nz]) */
/* long_cost_y_e_hess:(i0[3],i1[],i2[],i3[5],i4[6])->(o0[3x3,3nz]) */
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
casadi_real a0, a1, a10, a2, a3, a4, a5, a6, a7, a8, a9;
a0=arg[2]? arg[2][0] : 0;
a0=arg[3]? arg[3][0] : 0;
a1=arg[0]? arg[0][1] : 0;
a2=10.;
a2=(a1+a2);
@@ -77,10 +77,10 @@ static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw,
a5=2.0000000000000001e-01;
a6=(a1+a1);
a6=(a5*a6);
a7=arg[3]? arg[3][4] : 0;
a7=arg[4]? arg[4][4] : 0;
a6=(a6+a7);
a6=(a6/a2);
a8=arg[3]? arg[3][2] : 0;
a8=arg[4]? arg[4][2] : 0;
a9=arg[0]? arg[0][0] : 0;
a8=(a8-a9);
a9=casadi_sq(a1);
@@ -141,7 +141,7 @@ CASADI_SYMBOL_EXPORT void long_cost_y_e_hess_incref(void) {
CASADI_SYMBOL_EXPORT void long_cost_y_e_hess_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_e_hess_n_in(void) { return 4;}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_e_hess_n_in(void) { return 5;}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_e_hess_n_out(void) { return 1;}
@@ -157,6 +157,7 @@ CASADI_SYMBOL_EXPORT const char* long_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;
}
}
@@ -172,8 +173,9 @@ CASADI_SYMBOL_EXPORT const casadi_int* long_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;
}
}
@@ -186,7 +188,7 @@ CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_e_hess_sparsity_out(casadi_in
}
CASADI_SYMBOL_EXPORT int long_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;

20
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost_y_fun.c
View File

@@ -33,6 +33,7 @@ extern "C" {
#define casadi_s0 CASADI_PREFIX(s0)
#define casadi_s1 CASADI_PREFIX(s1)
#define casadi_s2 CASADI_PREFIX(s2)
#define casadi_s3 CASADI_PREFIX(s3)
#define casadi_sq CASADI_PREFIX(sq)
/* Symbol visibility in DLLs */
@@ -54,19 +55,20 @@ casadi_real casadi_sq(casadi_real x) { return x*x;}
static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
static const casadi_int casadi_s2[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
static const casadi_int casadi_s2[3] = {0, 0, 0};
static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
/* long_cost_y_fun:(i0[3],i1,i2[6])->(o0[6]) */
/* long_cost_y_fun:(i0[3],i1,i2[],i3[6])->(o0[6]) */
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, a4;
a0=arg[2]? arg[2][2] : 0;
a0=arg[3]? arg[3][2] : 0;
a1=arg[0]? arg[0][0] : 0;
a0=(a0-a1);
a2=arg[0]? arg[0][1] : 0;
a3=casadi_sq(a2);
a4=5.;
a3=(a3/a4);
a4=arg[2]? arg[2][4] : 0;
a4=arg[3]? arg[3][4] : 0;
a4=(a4*a2);
a3=(a3+a4);
a4=6.;
@@ -80,7 +82,7 @@ static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw,
if (res[0]!=0) res[0][2]=a2;
a2=arg[0]? arg[0][2] : 0;
if (res[0]!=0) res[0][3]=a2;
a1=arg[2]? arg[2][3] : 0;
a1=arg[3]? arg[3][3] : 0;
a2=(a2-a1);
if (res[0]!=0) res[0][4]=a2;
a2=arg[1]? arg[1][0] : 0;
@@ -116,7 +118,7 @@ CASADI_SYMBOL_EXPORT void long_cost_y_fun_incref(void) {
CASADI_SYMBOL_EXPORT void long_cost_y_fun_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_fun_n_in(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_fun_n_in(void) { return 4;}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_fun_n_out(void) { return 1;}
@@ -131,6 +133,7 @@ CASADI_SYMBOL_EXPORT const char* long_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;
}
}
@@ -147,19 +150,20 @@ CASADI_SYMBOL_EXPORT const casadi_int* long_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* long_cost_y_fun_sparsity_out(casadi_int i) {
switch (i) {
case 0: return casadi_s2;
case 0: return casadi_s3;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT int long_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;

69
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost_y_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 long_Y_COST
#define long_Y_COST
#ifdef __cplusplus
extern "C" {
#endif
int long_cost_y_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int long_cost_y_fun_work(int *, int *, int *, int *);
const int *long_cost_y_fun_sparsity_in(int);
const int *long_cost_y_fun_sparsity_out(int);
int long_cost_y_fun_n_in(void);
int long_cost_y_fun_n_out(void);
int long_cost_y_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int long_cost_y_fun_jac_ut_xt_work(int *, int *, int *, int *);
const int *long_cost_y_fun_jac_ut_xt_sparsity_in(int);
const int *long_cost_y_fun_jac_ut_xt_sparsity_out(int);
int long_cost_y_fun_jac_ut_xt_n_in(void);
int long_cost_y_fun_jac_ut_xt_n_out(void);
int long_cost_y_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int long_cost_y_hess_work(int *, int *, int *, int *);
const int *long_cost_y_hess_sparsity_in(int);
const int *long_cost_y_hess_sparsity_out(int);
int long_cost_y_hess_n_in(void);
int long_cost_y_hess_n_out(void);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // long_Y_COST

32
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost_y_fun_jac_ut_xt.c
View File

@@ -34,6 +34,8 @@ 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)
#define casadi_s5 CASADI_PREFIX(s5)
#define casadi_sq CASADI_PREFIX(sq)
/* Symbol visibility in DLLs */
@@ -55,20 +57,22 @@ casadi_real casadi_sq(casadi_real x) { return x*x;}
static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
static const casadi_int casadi_s2[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
static const casadi_int casadi_s3[16] = {4, 6, 0, 2, 3, 4, 5, 6, 7, 1, 2, 1, 2, 3, 3, 0};
static const casadi_int casadi_s2[3] = {0, 0, 0};
static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
static const casadi_int casadi_s4[16] = {4, 6, 0, 2, 3, 4, 5, 6, 7, 1, 2, 1, 2, 3, 3, 0};
static const casadi_int casadi_s5[3] = {6, 0, 0};
/* long_cost_y_fun_jac_ut_xt:(i0[3],i1,i2[6])->(o0[6],o1[4x6,7nz]) */
/* long_cost_y_fun_jac_ut_xt:(i0[3],i1,i2[],i3[6])->(o0[6],o1[4x6,7nz],o2[6x0]) */
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, a4, a5;
a0=arg[2]? arg[2][2] : 0;
a0=arg[3]? arg[3][2] : 0;
a1=arg[0]? arg[0][0] : 0;
a0=(a0-a1);
a2=arg[0]? arg[0][1] : 0;
a3=casadi_sq(a2);
a4=5.;
a3=(a3/a4);
a4=arg[2]? arg[2][4] : 0;
a4=arg[3]? arg[3][4] : 0;
a5=(a4*a2);
a3=(a3+a5);
a5=6.;
@@ -82,7 +86,7 @@ static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw,
if (res[0]!=0) res[0][2]=a2;
a1=arg[0]? arg[0][2] : 0;
if (res[0]!=0) res[0][3]=a1;
a5=arg[2]? arg[2][3] : 0;
a5=arg[3]? arg[3][3] : 0;
a1=(a1-a5);
if (res[0]!=0) res[0][4]=a1;
a1=arg[1]? arg[1][0] : 0;
@@ -136,9 +140,9 @@ CASADI_SYMBOL_EXPORT void long_cost_y_fun_jac_ut_xt_incref(void) {
CASADI_SYMBOL_EXPORT void long_cost_y_fun_jac_ut_xt_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_fun_jac_ut_xt_n_in(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_fun_jac_ut_xt_n_in(void) { return 4;}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_fun_jac_ut_xt_n_out(void) { return 2;}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_fun_jac_ut_xt_n_out(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_real long_cost_y_fun_jac_ut_xt_default_in(casadi_int i) {
switch (i) {
@@ -151,6 +155,7 @@ CASADI_SYMBOL_EXPORT const char* long_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;
}
}
@@ -159,6 +164,7 @@ CASADI_SYMBOL_EXPORT const char* long_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;
}
}
@@ -168,21 +174,23 @@ CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_fun_jac_ut_xt_sparsity_in(cas
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* long_cost_y_fun_jac_ut_xt_sparsity_out(casadi_int i) {
switch (i) {
case 0: return casadi_s2;
case 1: return casadi_s3;
case 0: return casadi_s3;
case 1: return casadi_s4;
case 2: return casadi_s5;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT int long_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;

24
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost_y_hess.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)
#define casadi_sq CASADI_PREFIX(sq)
/* Symbol visibility in DLLs */
@@ -55,13 +56,14 @@ casadi_real casadi_sq(casadi_real x) { return x*x;}
static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
static const casadi_int casadi_s2[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
static const casadi_int casadi_s3[10] = {4, 4, 0, 0, 1, 3, 3, 2, 1, 2};
static const casadi_int casadi_s2[3] = {0, 0, 0};
static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
static const casadi_int casadi_s4[10] = {4, 4, 0, 0, 1, 3, 3, 2, 1, 2};
/* long_cost_y_hess:(i0[3],i1,i2[6],i3[6])->(o0[4x4,3nz]) */
/* long_cost_y_hess:(i0[3],i1,i2[],i3[6],i4[6])->(o0[4x4,3nz]) */
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
casadi_real a0, a1, a10, a2, a3, a4, a5, a6, a7, a8, a9;
a0=arg[2]? arg[2][0] : 0;
a0=arg[3]? arg[3][0] : 0;
a1=arg[0]? arg[0][1] : 0;
a2=10.;
a2=(a1+a2);
@@ -75,10 +77,10 @@ static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw,
a5=2.0000000000000001e-01;
a6=(a1+a1);
a6=(a5*a6);
a7=arg[3]? arg[3][4] : 0;
a7=arg[4]? arg[4][4] : 0;
a6=(a6+a7);
a6=(a6/a2);
a8=arg[3]? arg[3][2] : 0;
a8=arg[4]? arg[4][2] : 0;
a9=arg[0]? arg[0][0] : 0;
a8=(a8-a9);
a9=casadi_sq(a1);
@@ -139,7 +141,7 @@ CASADI_SYMBOL_EXPORT void long_cost_y_hess_incref(void) {
CASADI_SYMBOL_EXPORT void long_cost_y_hess_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_hess_n_in(void) { return 4;}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_hess_n_in(void) { return 5;}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_hess_n_out(void) { return 1;}
@@ -155,6 +157,7 @@ CASADI_SYMBOL_EXPORT const char* long_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;
}
}
@@ -171,20 +174,21 @@ CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_hess_sparsity_in(casadi_int i
case 0: return casadi_s0;
case 1: return casadi_s1;
case 2: return casadi_s2;
case 3: return casadi_s2;
case 3: return casadi_s3;
case 4: return casadi_s3;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_hess_sparsity_out(casadi_int i) {
switch (i) {
case 0: return casadi_s3;
case 0: return casadi_s4;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT int long_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/longitudinal_mpc_lib/c_generated_code/long_model/long_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.
*

24
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/main_long.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_long.h"
// blasfeo
#include "blasfeo/include/blasfeo_d_aux_ext_dep.h"
#define NX LONG_NX
#define NZ LONG_NZ
#define NU LONG_NU
@@ -104,12 +104,12 @@ 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[0] = 0.0;
ubx0[0] = 0.0;
lbx0[1] = 0.0;
ubx0[1] = 0.0;
lbx0[2] = 0.0;
ubx0[2] = 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);
@@ -128,8 +128,8 @@ int main()
double p[NP];
p[0] = -1.2;
p[1] = 1.2;
p[2] = 0;
p[3] = 0;
p[2] = 0.0;
p[3] = 0.0;
p[4] = 1.45;
p[5] = 0.75;

15
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/main_sim_long.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.
*
@@ -71,9 +68,9 @@ int main()
x_current[2] = 0.0;
x_current[0] = 0;
x_current[1] = 0;
x_current[2] = 0;
x_current[0] = 0.0;
x_current[1] = 0.0;
x_current[2] = 0.0;
@@ -85,8 +82,8 @@ int main()
double p[NP];
p[0] = -1.2;
p[1] = 1.2;
p[2] = 0;
p[3] = 0;
p[2] = 0.0;
p[3] = 0.0;
p[4] = 1.45;
p[5] = 0.75;

128
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/make_sfun_long.m
View File

@@ -1,128 +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 = { ...
'long_model/long_expl_ode_fun.c', ...
'long_model/long_expl_vde_forw.c',...
'long_cost/long_cost_y_0_fun.c',...
'long_cost/long_cost_y_0_fun_jac_ut_xt.c',...
'long_cost/long_cost_y_0_hess.c',...
'long_cost/long_cost_y_fun.c',...
'long_cost/long_cost_y_fun_jac_ut_xt.c',...
'long_cost/long_cost_y_hess.c',...
'long_cost/long_cost_y_e_fun.c',...
'long_cost/long_cost_y_e_fun_jac_ut_xt.c',...
'long_cost/long_cost_y_e_hess.c',...
'long_constraints/long_constr_h_fun.c', ...
'long_constraints/long_constr_h_fun_jac_uxt_zt_hess.c', ...
'long_constraints/long_constr_h_fun_jac_uxt_zt.c', ...
'acados_solver_sfunction_long.c', ...
'acados_solver_long.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_long' );
fprintf( [ '\n\nSuccessfully created sfunction:\nacados_solver_sfunction_long', '.', ...
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 [3]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') ubx_0 - upper bound on x for stage 0,',...
' size [3]\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 [78]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') y_ref_0, size [6]\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 [66]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') y_ref_e, size [5]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') lh, size [4]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') uh, size [4]\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)