From 7c43a60bda6c129c0c5a34cdb7fe74d8243729c5 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Thu, 7 Jan 2021 15:18:23 +0100 Subject: [PATCH] Rebuild acado and add scons command to regenerate mpc (#19685) * rebuild acado and add scons command to regenerate mpc * update scons help --- .gitattributes | 10 +- SConstruct | 4 + phonelibs/acado/aarch64/lib/libacado_casadi.a | 3 - .../acado/aarch64/lib/libacado_csparse.a | 3 - .../acado/aarch64/lib/libacado_qpoases.a | 3 - .../acado/aarch64/lib/libacado_toolkit.a | 3 - .../acado/aarch64/lib/libacado_toolkit_s.so | 1 - .../lib/libacado_toolkit_s.so.1.2.2beta | 3 - phonelibs/acado/build.sh | 29 ++ phonelibs/acado/build.txt | 16 - .../templates/acado_forces_generator.m.in | 442 +++++++++--------- .../templates/qpdunes_interface.in | 154 +++--- .../templates/qpdunes_split_interface.in | 192 ++++---- .../templates/qpoases3_interface.c.in | 120 ++--- .../code_generation/templates/templates.hpp | 2 +- .../external_packages/eigen3/COPYING.MINPACK | 104 ++--- phonelibs/acado/x64/lib/libacado_casadi.a | 3 - phonelibs/acado/x64/lib/libacado_csparse.a | 3 - phonelibs/acado/x64/lib/libacado_qpoases.a | 3 - phonelibs/acado/x64/lib/libacado_toolkit.a | 3 - phonelibs/acado/x64/lib/libacado_toolkit_s.so | 1 - .../acado/x64/lib/libacado_toolkit_s.so.1 | 1 - .../x64/lib/libacado_toolkit_s.so.1.2.2beta | 3 - .../lib/libacado_toolkit_s.so.1 | 0 .../lib/libacado_toolkit_s.so.1.2.2beta | 3 + selfdrive/controls/lib/lateral_mpc/SConscript | 39 +- .../controls/lib/longitudinal_mpc/SConscript | 40 +- 27 files changed, 604 insertions(+), 584 deletions(-) delete mode 100644 phonelibs/acado/aarch64/lib/libacado_casadi.a delete mode 100644 phonelibs/acado/aarch64/lib/libacado_csparse.a delete mode 100644 phonelibs/acado/aarch64/lib/libacado_qpoases.a delete mode 100644 phonelibs/acado/aarch64/lib/libacado_toolkit.a delete mode 120000 phonelibs/acado/aarch64/lib/libacado_toolkit_s.so delete mode 100755 phonelibs/acado/aarch64/lib/libacado_toolkit_s.so.1.2.2beta create mode 100755 phonelibs/acado/build.sh delete mode 100644 phonelibs/acado/build.txt delete mode 100644 phonelibs/acado/x64/lib/libacado_casadi.a delete mode 100644 phonelibs/acado/x64/lib/libacado_csparse.a delete mode 100644 phonelibs/acado/x64/lib/libacado_qpoases.a delete mode 100644 phonelibs/acado/x64/lib/libacado_toolkit.a delete mode 120000 phonelibs/acado/x64/lib/libacado_toolkit_s.so delete mode 120000 phonelibs/acado/x64/lib/libacado_toolkit_s.so.1 delete mode 100755 phonelibs/acado/x64/lib/libacado_toolkit_s.so.1.2.2beta rename phonelibs/acado/{aarch64 => x86_64}/lib/libacado_toolkit_s.so.1 (100%) create mode 100644 phonelibs/acado/x86_64/lib/libacado_toolkit_s.so.1.2.2beta diff --git a/.gitattributes b/.gitattributes index 215eda57..527b1aae 100644 --- a/.gitattributes +++ b/.gitattributes @@ -7,11 +7,11 @@ *.ipynb filter=nbstripout -diff external/ffmpeg/bin/ffmpeg_cuda filter=lfs diff=lfs merge=lfs -text models/segnet.keras filter=lfs diff=lfs merge=lfs -text -phonelibs/acado/x64/lib/libacado_toolkit.a filter=lfs diff=lfs merge=lfs -text -phonelibs/acado/x64/lib/libacado_toolkit_s.so.1.2.2beta filter=lfs diff=lfs merge=lfs -text -phonelibs/acado/x64/lib/libacado_casadi.a filter=lfs diff=lfs merge=lfs -text -phonelibs/acado/x64/lib/libacado_csparse.a filter=lfs diff=lfs merge=lfs -text -phonelibs/acado/x64/lib/libacado_qpoases.a filter=lfs diff=lfs merge=lfs -text +phonelibs/acado/x86_64/lib/libacado_toolkit.a filter=lfs diff=lfs merge=lfs -text +phonelibs/acado/x86_64/lib/libacado_toolkit_s.so.1.2.2beta filter=lfs diff=lfs merge=lfs -text +phonelibs/acado/x86_64/lib/libacado_casadi.a filter=lfs diff=lfs merge=lfs -text +phonelibs/acado/x86_64/lib/libacado_csparse.a filter=lfs diff=lfs merge=lfs -text +phonelibs/acado/x86_64/lib/libacado_qpoases.a filter=lfs diff=lfs merge=lfs -text phonelibs/acado/aarch64/lib/libacado_toolkit.a filter=lfs diff=lfs merge=lfs -text phonelibs/acado/aarch64/lib/libacado_toolkit_s.so.1.2.2beta filter=lfs diff=lfs merge=lfs -text phonelibs/acado/aarch64/lib/libacado_casadi.a filter=lfs diff=lfs merge=lfs -text diff --git a/SConstruct b/SConstruct index 0b4135de..9b79ef2e 100644 --- a/SConstruct +++ b/SConstruct @@ -25,6 +25,10 @@ AddOption('--compile_db', action='store_true', help='build clang compilation database') +AddOption('--mpc-generate', + action='store_true', + help='regenerates the mpc sources') + real_arch = arch = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip() if platform.system() == "Darwin": arch = "Darwin" diff --git a/phonelibs/acado/aarch64/lib/libacado_casadi.a b/phonelibs/acado/aarch64/lib/libacado_casadi.a deleted file mode 100644 index 8574aa8c..00000000 --- a/phonelibs/acado/aarch64/lib/libacado_casadi.a +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:02c9bdd43a4c5692c16cb437ab000e74efc908d4dcefb39144f7a0eb0e5b67ab -size 33782 diff --git a/phonelibs/acado/aarch64/lib/libacado_csparse.a b/phonelibs/acado/aarch64/lib/libacado_csparse.a deleted file mode 100644 index 6a8efe0f..00000000 --- a/phonelibs/acado/aarch64/lib/libacado_csparse.a +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:9a6bd05cc21d157f8705575df0ef69d02974375bc800edd353d504635ca090bd -size 92090 diff --git a/phonelibs/acado/aarch64/lib/libacado_qpoases.a b/phonelibs/acado/aarch64/lib/libacado_qpoases.a deleted file mode 100644 index 1f3c3cc3..00000000 --- a/phonelibs/acado/aarch64/lib/libacado_qpoases.a +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:be4f11bcff1de30751fbf8fb014d0e14d7334c17fac3dfd4cb2f445391a08946 -size 634068 diff --git a/phonelibs/acado/aarch64/lib/libacado_toolkit.a b/phonelibs/acado/aarch64/lib/libacado_toolkit.a deleted file mode 100644 index 489ba4a4..00000000 --- a/phonelibs/acado/aarch64/lib/libacado_toolkit.a +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:80a5b92a8162edd22507d2d9e9abf794ebd4aa4b5affa398443a01f6ab71fde3 -size 23341704 diff --git a/phonelibs/acado/aarch64/lib/libacado_toolkit_s.so b/phonelibs/acado/aarch64/lib/libacado_toolkit_s.so deleted file mode 120000 index 08cbf4e4..00000000 --- a/phonelibs/acado/aarch64/lib/libacado_toolkit_s.so +++ /dev/null @@ -1 +0,0 @@ -libacado_toolkit_s.so.1 \ No newline at end of file diff --git a/phonelibs/acado/aarch64/lib/libacado_toolkit_s.so.1.2.2beta b/phonelibs/acado/aarch64/lib/libacado_toolkit_s.so.1.2.2beta deleted file mode 100755 index efed9cd5..00000000 --- a/phonelibs/acado/aarch64/lib/libacado_toolkit_s.so.1.2.2beta +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:b21d9a8c8b68715062830ed91420bad10622ae157f8daecca98adb7082c7de43 -size 8813880 diff --git a/phonelibs/acado/build.sh b/phonelibs/acado/build.sh new file mode 100755 index 00000000..60288ea6 --- /dev/null +++ b/phonelibs/acado/build.sh @@ -0,0 +1,29 @@ +#!/usr/bin/env sh +set -e + +rm -rf acado +git clone https://github.com/acado/acado.git +cd acado +git reset --hard 5adb8cbcff5a5464706a48eaf073218ac87c9044 + +# Clang 8 fixes +git apply ../01.patch +sed -i '100d' cmake/CompilerOptions.cmake +sed -i '100d' cmake/CompilerOptions.cmake + +mkdir build +cd build +cmake -DACADO_WITH_EXAMPLES=OFF -DACADO_BUILD_STATIC=ON -DCMAKE_INSTALL_PREFIX="$HOME/openpilot/phonelibs/acado" .. +make -j$(nproc) +make install + +cd .. +cd .. + +rm -r x86_64 +mkdir x86_64 +mv lib x86_64/lib +cp acado/build/lib/* x86_64/lib/ + +rm -rf acado +rm -r share diff --git a/phonelibs/acado/build.txt b/phonelibs/acado/build.txt deleted file mode 100644 index 2f606662..00000000 --- a/phonelibs/acado/build.txt +++ /dev/null @@ -1,16 +0,0 @@ -git clone https://github.com/acado/acado.git -cd acado -git reset --hard 5adb8cbcff5a5464706a48eaf073218ac87c9044 - - -# Clang 8 fixes -# git apply 01.patch -# sed -i '100d' cmake/CompilerOptions.cmake -# sed -i '100d' cmake/CompilerOptions.cmake - - -mkdir build -cd build -cmake -DACADO_WITH_EXAMPLES=OFF -DACADO_BUILD_STATIC=ON -DCMAKE_INSTALL_PREFIX="$HOME/one/phonelibs/acado" .. -make -j4 -make install diff --git a/phonelibs/acado/include/acado/code_generation/templates/acado_forces_generator.m.in b/phonelibs/acado/include/acado/code_generation/templates/acado_forces_generator.m.in index 437ff6ff..507ca15a 100644 --- a/phonelibs/acado/include/acado/code_generation/templates/acado_forces_generator.m.in +++ b/phonelibs/acado/include/acado/code_generation/templates/acado_forces_generator.m.in @@ -1,221 +1,221 @@ -clear stages params outputs codeoptions - -%% Generator for a FORCES QP solver used by the ACADO OCP solver - -% -% User options -% - -% Number of states -nx = @NX@; -% Number of controls -nu = @NU@; -% Number of discretization (shooting) nodes -N = @N@; - -% Lower and upper bounds on stage variables, for all N stages -lbIdx = { ... - @LB_IDX@ ... -}; -ubIdx = { ... - @UB_IDX@ ... -}; - -% Polytopic constraints (A_i*z_i <= b_i) on stage variables, for all N stages -AbDim = { ... - @AB_DIM@ ... -}; - -% Constant Hessian -constHessian = @CONST_HESSIAN@; -% Diagonal Hessian -diagHessian = @DIAG_HESSIAN@; -diagHessianN = @DIAG_HESSIAN_N@; -% Fixed initial state -fixedInitialState = @FIXED_INITIAL_STATE@; - -%% Define FORCES multistage problem - -stages = MultistageProblem( N ); - -for i = 1: N - if (i == 1) - - % - % Initial stage - % - - % Dimension - stages(i).dims.n = nx + nu; % number of stage variables - stages(i).dims.r = nx; % number of equality constraints - stages(i).dims.l = length( lbIdx{ i } ); % number of lower bounds - stages(i).dims.u = length( ubIdx{ i } ); % number of upper bounds - stages(i).dims.p = AbDim{ i }; % number of polytopic constraints - stages(i).dims.q = 0; % number of quadratic constraints - - % Cost - if (constHessian == 1) - if (diagHessian == 1) - params( 1 ) = newParam('H1', 1: 1: N - 1, 'cost.H', 'diag'); - else - params( 1 ) = newParam('H1', 1: 1: N - 1, 'cost.H'); - end; - else - if (diagHessian == 1) - params( 1 ) = newParam('H1', 1, 'cost.H', 'diag'); - else - params( 1 ) = newParam('H1', 1, 'cost.H'); - end; - end; - params(end + 1) = newParam('f1', 1, 'cost.f'); - - % Lower bounds - stages(i).ineq.b.lbidx = cell2mat( lbIdx{ i } ); - if (~isempty( lbIdx{ i } )) - params(end + 1) = newParam(sprintf('%s%d', 'lb', i), i, 'ineq.b.lb'); - end; - - % Upper bounds - stages(i).ineq.b.ubidx = cell2mat( ubIdx{ i } ); - if (~isempty( ubIdx{ i } )) - params(end + 1) = newParam(sprintf('%s%d', 'ub', i), i, 'ineq.b.ub'); - end; - - % Polytopic constraints - % stages(i).ineq.p.bidx = cell2mat( AbIdx{ i } ); - if (AbDim{ i } ~= 0) - params(end + 1) = newParam(sprintf('%s%d', 'Ab', i), i, 'ineq.p.b'); - params(end + 1) = newParam(sprintf('%s%d', 'A', i), i, 'ineq.p.A'); - end - - % equality constraints - params(end + 1) = newParam('C1', 1, 'eq.C'); - params(end + 1) = newParam('d1', 1, 'eq.c'); - - if (fixedInitialState == 1) - stages(i).eq.D = [eye(nx), zeros(nx,nu)]; - end; - - elseif (i < N) - - % - % Stages along horizon - % - - % Dimension - stages(i).dims.n = nx + nu; % number of stage variables - stages(i).dims.r = nx; % number of equality constraints - stages(i).dims.l = length( lbIdx{ i } ); % number of lower bounds - stages(i).dims.u = length( ubIdx{ i } ); % number of upper bounds - stages(i).dims.p = AbDim{ i }; % number of polytopic constraints - stages(i).dims.q = 0; % number of quadratic constraints - - % Cost - if (constHessian == 0) - if (diagHessian == 1) - params(end + 1) = newParam(sprintf('%s%d', 'H', i), i, 'cost.H', 'diag'); - else - params(end + 1) = newParam(sprintf('%s%d', 'H', i), i, 'cost.H'); - end; - end; - params(end + 1) = newParam(sprintf('%s%d', 'f', i), i, 'cost.f'); - - % Lower bounds - stages(i).ineq.b.lbidx = cell2mat( lbIdx{ i } ); - if (~isempty( lbIdx{ i } )) - params(end + 1) = newParam(sprintf('%s%d', 'lb', i), i, 'ineq.b.lb'); - end; - - % Upper bounds - stages(i).ineq.b.ubidx = cell2mat( ubIdx{ i } ); - if (~isempty( ubIdx{ i } )) - params(end + 1) = newParam(sprintf('%s%d', 'ub', i), i, 'ineq.b.ub'); - end; - - % Polytopic constraints - % stages(i).ineq.p.bidx = cell2mat( AbIdx{ i } ); - if (AbDim{ i } ~= 0) - params(end + 1) = newParam(sprintf('%s%d', 'Ab', i), i, 'ineq.p.b'); - params(end + 1) = newParam(sprintf('%s%d', 'A', i), i, 'ineq.p.A'); - end - - % Equality constraints - params(end + 1) = newParam(sprintf('%s%d', 'C', i), i, 'eq.C'); - params(end + 1) = newParam(sprintf('%s%d', 'd', i), i, 'eq.c'); - - stages(i).eq.D = [-eye( nx ), zeros(nx, nu)]; - - else - - % - % Final stage - % - - % Dimension - stages(i).dims.n = nx; % number of stage variables - - if (fixedInitialState == 1) - stages(i).dims.r = nx; % number of equality constraints - else - stages(i).dims.r = 0; - end; - - stages(i).dims.l = length( lbIdx{ i } ); % number of lower bounds - stages(i).dims.u = length( ubIdx{ i } ); % number of upper bounds - stages(i).dims.p = AbDim{ i }; % number of polytopic constraints - stages(i).dims.q = 0; % number of quadratic constraints - - % Cost - if (diagHessianN == 1) - params(end + 1) = newParam(sprintf('%s%d', 'H', i), i, 'cost.H', 'diag'); - else - params(end + 1) = newParam(sprintf('%s%d', 'H', i), i, 'cost.H'); - end; - params(end + 1) = newParam(sprintf('%s%d', 'f', i), i, 'cost.f'); - - % Lower bounds - stages(i).ineq.b.lbidx = cell2mat( lbIdx{ i } ); - if (~isempty( lbIdx{ i } )) - params(end + 1) = newParam(sprintf('%s%d', 'lb', i), i, 'ineq.b.lb'); - end; - - % Upper bounds - stages(i).ineq.b.ubidx = cell2mat( ubIdx{ i } ); - if (~isempty( ubIdx{ i } )) - params(end + 1) = newParam(sprintf('%s%d', 'ub', i), i, 'ineq.b.ub'); - end; - - % Polytopic constraints - % stages(i).ineq.p.bidx = cell2mat( AbIdx{ i } ); - if (AbDim{ i } ~= 0) - params(end + 1) = newParam(sprintf('%s%d', 'Ab', i), i, 'ineq.p.b'); - params(end + 1) = newParam(sprintf('%s%d', 'A', i), i, 'ineq.p.A'); - end - - % Equality constraints - stages(i).eq.D = -eye(nx); - - if (fixedInitialState == 1) - params(end + 1) = newParam(sprintf('%s%d', 'd', i), i, 'eq.c'); - end; - - end; -end; - -%% Define outputs of the solver -for i = 1: N - outputs( i ) = newOutput(sprintf('%s%d', 'out', i), i, 1: 1: stages(i).dims.n); -end; - -%% Solver settings -codeoptions = getOptions('@SOLVER_NAME@'); - -codeoptions.overwrite = 1; % Always overwrite the existing code -codeoptions.printlevel = @PRINT_LEVEL@; -codeoptions.maxit = @MAX_ITERATIONS@; -codeoptions.parallel = @PARALLEL@; -codeoptions.init = @WARM_START@; - - -%% Generate code -generateCode(stages, params, codeoptions, outputs); +clear stages params outputs codeoptions + +%% Generator for a FORCES QP solver used by the ACADO OCP solver + +% +% User options +% + +% Number of states +nx = @NX@; +% Number of controls +nu = @NU@; +% Number of discretization (shooting) nodes +N = @N@; + +% Lower and upper bounds on stage variables, for all N stages +lbIdx = { ... + @LB_IDX@ ... +}; +ubIdx = { ... + @UB_IDX@ ... +}; + +% Polytopic constraints (A_i*z_i <= b_i) on stage variables, for all N stages +AbDim = { ... + @AB_DIM@ ... +}; + +% Constant Hessian +constHessian = @CONST_HESSIAN@; +% Diagonal Hessian +diagHessian = @DIAG_HESSIAN@; +diagHessianN = @DIAG_HESSIAN_N@; +% Fixed initial state +fixedInitialState = @FIXED_INITIAL_STATE@; + +%% Define FORCES multistage problem + +stages = MultistageProblem( N ); + +for i = 1: N + if (i == 1) + + % + % Initial stage + % + + % Dimension + stages(i).dims.n = nx + nu; % number of stage variables + stages(i).dims.r = nx; % number of equality constraints + stages(i).dims.l = length( lbIdx{ i } ); % number of lower bounds + stages(i).dims.u = length( ubIdx{ i } ); % number of upper bounds + stages(i).dims.p = AbDim{ i }; % number of polytopic constraints + stages(i).dims.q = 0; % number of quadratic constraints + + % Cost + if (constHessian == 1) + if (diagHessian == 1) + params( 1 ) = newParam('H1', 1: 1: N - 1, 'cost.H', 'diag'); + else + params( 1 ) = newParam('H1', 1: 1: N - 1, 'cost.H'); + end; + else + if (diagHessian == 1) + params( 1 ) = newParam('H1', 1, 'cost.H', 'diag'); + else + params( 1 ) = newParam('H1', 1, 'cost.H'); + end; + end; + params(end + 1) = newParam('f1', 1, 'cost.f'); + + % Lower bounds + stages(i).ineq.b.lbidx = cell2mat( lbIdx{ i } ); + if (~isempty( lbIdx{ i } )) + params(end + 1) = newParam(sprintf('%s%d', 'lb', i), i, 'ineq.b.lb'); + end; + + % Upper bounds + stages(i).ineq.b.ubidx = cell2mat( ubIdx{ i } ); + if (~isempty( ubIdx{ i } )) + params(end + 1) = newParam(sprintf('%s%d', 'ub', i), i, 'ineq.b.ub'); + end; + + % Polytopic constraints + % stages(i).ineq.p.bidx = cell2mat( AbIdx{ i } ); + if (AbDim{ i } ~= 0) + params(end + 1) = newParam(sprintf('%s%d', 'Ab', i), i, 'ineq.p.b'); + params(end + 1) = newParam(sprintf('%s%d', 'A', i), i, 'ineq.p.A'); + end + + % equality constraints + params(end + 1) = newParam('C1', 1, 'eq.C'); + params(end + 1) = newParam('d1', 1, 'eq.c'); + + if (fixedInitialState == 1) + stages(i).eq.D = [eye(nx), zeros(nx,nu)]; + end; + + elseif (i < N) + + % + % Stages along horizon + % + + % Dimension + stages(i).dims.n = nx + nu; % number of stage variables + stages(i).dims.r = nx; % number of equality constraints + stages(i).dims.l = length( lbIdx{ i } ); % number of lower bounds + stages(i).dims.u = length( ubIdx{ i } ); % number of upper bounds + stages(i).dims.p = AbDim{ i }; % number of polytopic constraints + stages(i).dims.q = 0; % number of quadratic constraints + + % Cost + if (constHessian == 0) + if (diagHessian == 1) + params(end + 1) = newParam(sprintf('%s%d', 'H', i), i, 'cost.H', 'diag'); + else + params(end + 1) = newParam(sprintf('%s%d', 'H', i), i, 'cost.H'); + end; + end; + params(end + 1) = newParam(sprintf('%s%d', 'f', i), i, 'cost.f'); + + % Lower bounds + stages(i).ineq.b.lbidx = cell2mat( lbIdx{ i } ); + if (~isempty( lbIdx{ i } )) + params(end + 1) = newParam(sprintf('%s%d', 'lb', i), i, 'ineq.b.lb'); + end; + + % Upper bounds + stages(i).ineq.b.ubidx = cell2mat( ubIdx{ i } ); + if (~isempty( ubIdx{ i } )) + params(end + 1) = newParam(sprintf('%s%d', 'ub', i), i, 'ineq.b.ub'); + end; + + % Polytopic constraints + % stages(i).ineq.p.bidx = cell2mat( AbIdx{ i } ); + if (AbDim{ i } ~= 0) + params(end + 1) = newParam(sprintf('%s%d', 'Ab', i), i, 'ineq.p.b'); + params(end + 1) = newParam(sprintf('%s%d', 'A', i), i, 'ineq.p.A'); + end + + % Equality constraints + params(end + 1) = newParam(sprintf('%s%d', 'C', i), i, 'eq.C'); + params(end + 1) = newParam(sprintf('%s%d', 'd', i), i, 'eq.c'); + + stages(i).eq.D = [-eye( nx ), zeros(nx, nu)]; + + else + + % + % Final stage + % + + % Dimension + stages(i).dims.n = nx; % number of stage variables + + if (fixedInitialState == 1) + stages(i).dims.r = nx; % number of equality constraints + else + stages(i).dims.r = 0; + end; + + stages(i).dims.l = length( lbIdx{ i } ); % number of lower bounds + stages(i).dims.u = length( ubIdx{ i } ); % number of upper bounds + stages(i).dims.p = AbDim{ i }; % number of polytopic constraints + stages(i).dims.q = 0; % number of quadratic constraints + + % Cost + if (diagHessianN == 1) + params(end + 1) = newParam(sprintf('%s%d', 'H', i), i, 'cost.H', 'diag'); + else + params(end + 1) = newParam(sprintf('%s%d', 'H', i), i, 'cost.H'); + end; + params(end + 1) = newParam(sprintf('%s%d', 'f', i), i, 'cost.f'); + + % Lower bounds + stages(i).ineq.b.lbidx = cell2mat( lbIdx{ i } ); + if (~isempty( lbIdx{ i } )) + params(end + 1) = newParam(sprintf('%s%d', 'lb', i), i, 'ineq.b.lb'); + end; + + % Upper bounds + stages(i).ineq.b.ubidx = cell2mat( ubIdx{ i } ); + if (~isempty( ubIdx{ i } )) + params(end + 1) = newParam(sprintf('%s%d', 'ub', i), i, 'ineq.b.ub'); + end; + + % Polytopic constraints + % stages(i).ineq.p.bidx = cell2mat( AbIdx{ i } ); + if (AbDim{ i } ~= 0) + params(end + 1) = newParam(sprintf('%s%d', 'Ab', i), i, 'ineq.p.b'); + params(end + 1) = newParam(sprintf('%s%d', 'A', i), i, 'ineq.p.A'); + end + + % Equality constraints + stages(i).eq.D = -eye(nx); + + if (fixedInitialState == 1) + params(end + 1) = newParam(sprintf('%s%d', 'd', i), i, 'eq.c'); + end; + + end; +end; + +%% Define outputs of the solver +for i = 1: N + outputs( i ) = newOutput(sprintf('%s%d', 'out', i), i, 1: 1: stages(i).dims.n); +end; + +%% Solver settings +codeoptions = getOptions('@SOLVER_NAME@'); + +codeoptions.overwrite = 1; % Always overwrite the existing code +codeoptions.printlevel = @PRINT_LEVEL@; +codeoptions.maxit = @MAX_ITERATIONS@; +codeoptions.parallel = @PARALLEL@; +codeoptions.init = @WARM_START@; + + +%% Generate code +generateCode(stages, params, codeoptions, outputs); diff --git a/phonelibs/acado/include/acado/code_generation/templates/qpdunes_interface.in b/phonelibs/acado/include/acado/code_generation/templates/qpdunes_interface.in index d55ad6d4..fc3b8895 100644 --- a/phonelibs/acado/include/acado/code_generation/templates/qpdunes_interface.in +++ b/phonelibs/acado/include/acado/code_generation/templates/qpdunes_interface.in @@ -1,77 +1,77 @@ -/******************************************************************************/ -/* */ -/* qpDUNES interface data and functions */ -/* */ -/******************************************************************************/ - -#include - -qpData_t qpData; -qpOptions_t qpOptions; - -#if @DIAG_H@ -#define QPDUNES_LS_HOMOTOPY_GRID_SEARCH 0 -#endif - -int initializeQpDunes( void ) -{ - return_t statusFlag; - int kk; - - @QP_ND_ARRAY@ - - qpOptions = qpDUNES_setupDefaultOptions(); - qpOptions.maxIter = @MAX_ITER@; - qpOptions.printLevel = @PRINT_LEVEL@; - qpOptions.stationarityTolerance = 1.e-6; - qpOptions.regParam = 1.e-6; - qpOptions.newtonHessDiagRegTolerance = 1.e-8; - qpOptions.lsType = @DIAG_H@ ? QPDUNES_LS_ACCELERATED_GRADIENT_BISECTION_LS : QPDUNES_LS_HOMOTOPY_GRID_SEARCH; -/* qpOptions.lsType = QPDUNES_LS_BACKTRACKING_LS; */ - qpOptions.lineSearchReductionFactor = 0.1; - qpOptions.lineSearchMaxStepSize = 1.; - qpOptions.maxNumLineSearchIterations = 25; - qpOptions.maxNumLineSearchRefinementIterations = 25; -/* qpOptions.regType = QPDUNES_REG_SINGULAR_DIRECTIONS; */ - qpOptions.regType = QPDUNES_REG_LEVENBERG_MARQUARDT; - - qpDUNES_setup(&qpData, @ACADO_N@, @ACADO_NX@, @ACADO_NU@, nD, &( qpOptions )); - - for (kk = 0; kk < @ACADO_N@; ++kk) - { - qpData.intervals[ kk ]->H.sparsityType = @DIAG_H@ ? QPDUNES_DIAGONAL : QPDUNES_DENSE; - } - qpData.intervals[ @ACADO_N@ ]->H.sparsityType = @DIAG_HN@ ? QPDUNES_DIAGONAL : QPDUNES_DENSE; - - statusFlag = qpDUNES_init(&qpData, @QP_H@, @QP_G@, @QP_C@, @QP_c@, @QP_LB@, @QP_UB@, @QP_D@, @QP_LBA@, @QP_UBA@); - - return (int)statusFlag; -} - -void cleanupQpDunes( void ) -{ - qpDUNES_cleanup( &qpData ); -} - -int solveQpDunes( void ) -{ - return_t statusFlag; - - statusFlag = qpDUNES_updateData(&qpData, @QP_H@, @QP_G@, @QP_C@, @QP_c@, @QP_LB@, @QP_UB@, @QP_D@, @QP_LBA@, @QP_UBA@); - if (statusFlag != QPDUNES_OK) - return (int)statusFlag; - - if ( @INITIAL_STATE_FIXED@ ) - statusFlag = qpDUNES_updateIntervalData(&qpData, qpData.intervals[ 0 ], 0, 0, 0, 0, @QP_LB_0@, @QP_UB_0@, 0, 0, 0, 0); - else - statusFlag = qpDUNES_updateIntervalData(&qpData, qpData.intervals[ @ACADO_N@ ], 0, @QP_G_N@, 0, 0, 0, 0, 0, 0, 0, 0); - if (statusFlag != QPDUNES_OK) - return (int)statusFlag; - - statusFlag = qpDUNES_solve( &qpData ); - - qpDUNES_getPrimalSol(&qpData, @QP_PRIMAL@); - qpDUNES_getDualSol(&qpData, @QP_LAMBDA@, @QP_MU@); - - return (int)statusFlag; -} +/******************************************************************************/ +/* */ +/* qpDUNES interface data and functions */ +/* */ +/******************************************************************************/ + +#include + +qpData_t qpData; +qpOptions_t qpOptions; + +#if @DIAG_H@ +#define QPDUNES_LS_HOMOTOPY_GRID_SEARCH 0 +#endif + +int initializeQpDunes( void ) +{ + return_t statusFlag; + int kk; + + @QP_ND_ARRAY@ + + qpOptions = qpDUNES_setupDefaultOptions(); + qpOptions.maxIter = @MAX_ITER@; + qpOptions.printLevel = @PRINT_LEVEL@; + qpOptions.stationarityTolerance = 1.e-6; + qpOptions.regParam = 1.e-6; + qpOptions.newtonHessDiagRegTolerance = 1.e-8; + qpOptions.lsType = @DIAG_H@ ? QPDUNES_LS_ACCELERATED_GRADIENT_BISECTION_LS : QPDUNES_LS_HOMOTOPY_GRID_SEARCH; +/* qpOptions.lsType = QPDUNES_LS_BACKTRACKING_LS; */ + qpOptions.lineSearchReductionFactor = 0.1; + qpOptions.lineSearchMaxStepSize = 1.; + qpOptions.maxNumLineSearchIterations = 25; + qpOptions.maxNumLineSearchRefinementIterations = 25; +/* qpOptions.regType = QPDUNES_REG_SINGULAR_DIRECTIONS; */ + qpOptions.regType = QPDUNES_REG_LEVENBERG_MARQUARDT; + + qpDUNES_setup(&qpData, @ACADO_N@, @ACADO_NX@, @ACADO_NU@, nD, &( qpOptions )); + + for (kk = 0; kk < @ACADO_N@; ++kk) + { + qpData.intervals[ kk ]->H.sparsityType = @DIAG_H@ ? QPDUNES_DIAGONAL : QPDUNES_DENSE; + } + qpData.intervals[ @ACADO_N@ ]->H.sparsityType = @DIAG_HN@ ? QPDUNES_DIAGONAL : QPDUNES_DENSE; + + statusFlag = qpDUNES_init(&qpData, @QP_H@, @QP_G@, @QP_C@, @QP_c@, @QP_LB@, @QP_UB@, @QP_D@, @QP_LBA@, @QP_UBA@); + + return (int)statusFlag; +} + +void cleanupQpDunes( void ) +{ + qpDUNES_cleanup( &qpData ); +} + +int solveQpDunes( void ) +{ + return_t statusFlag; + + statusFlag = qpDUNES_updateData(&qpData, @QP_H@, @QP_G@, @QP_C@, @QP_c@, @QP_LB@, @QP_UB@, @QP_D@, @QP_LBA@, @QP_UBA@); + if (statusFlag != QPDUNES_OK) + return (int)statusFlag; + + if ( @INITIAL_STATE_FIXED@ ) + statusFlag = qpDUNES_updateIntervalData(&qpData, qpData.intervals[ 0 ], 0, 0, 0, 0, @QP_LB_0@, @QP_UB_0@, 0, 0, 0, 0); + else + statusFlag = qpDUNES_updateIntervalData(&qpData, qpData.intervals[ @ACADO_N@ ], 0, @QP_G_N@, 0, 0, 0, 0, 0, 0, 0, 0); + if (statusFlag != QPDUNES_OK) + return (int)statusFlag; + + statusFlag = qpDUNES_solve( &qpData ); + + qpDUNES_getPrimalSol(&qpData, @QP_PRIMAL@); + qpDUNES_getDualSol(&qpData, @QP_LAMBDA@, @QP_MU@); + + return (int)statusFlag; +} diff --git a/phonelibs/acado/include/acado/code_generation/templates/qpdunes_split_interface.in b/phonelibs/acado/include/acado/code_generation/templates/qpdunes_split_interface.in index 5a0e653f..680fb8d3 100644 --- a/phonelibs/acado/include/acado/code_generation/templates/qpdunes_split_interface.in +++ b/phonelibs/acado/include/acado/code_generation/templates/qpdunes_split_interface.in @@ -1,96 +1,96 @@ -/******************************************************************************/ -/* */ -/* qpDUNES interface data and functions */ -/* */ -/******************************************************************************/ - -#include - -qpData_t qpData; -qpOptions_t qpOptions; - -#if @DIAG_H@ -#define QPDUNES_LS_HOMOTOPY_GRID_SEARCH 0 -#endif - -#include "@MODULE_NAME@_auxiliary_functions.h" -real_t prepareQPtime, updateQPtime, solveQPtime; - -int initializeQpDunes( void ) -{ - return_t statusFlag; - int kk; - - @QP_ND_ARRAY@ - - qpOptions = qpDUNES_setupDefaultOptions(); - qpOptions.maxIter = @MAX_ITER@; - qpOptions.printLevel = @PRINT_LEVEL@; - qpOptions.stationarityTolerance = 1.e-6; - qpOptions.regParam = 1.e-6; - qpOptions.newtonHessDiagRegTolerance = 1.e-8; - qpOptions.lsType = @DIAG_H@ ? QPDUNES_LS_ACCELERATED_GRADIENT_BISECTION_LS : QPDUNES_LS_HOMOTOPY_GRID_SEARCH; -/* qpOptions.lsType = QPDUNES_LS_BACKTRACKING_LS; */ - qpOptions.lineSearchReductionFactor = 0.1; - qpOptions.lineSearchMaxStepSize = 1.; - qpOptions.maxNumLineSearchIterations = 25; - qpOptions.maxNumLineSearchRefinementIterations = 25; -/* qpOptions.regType = QPDUNES_REG_SINGULAR_DIRECTIONS; */ - qpOptions.regType = QPDUNES_REG_LEVENBERG_MARQUARDT; - - qpDUNES_setup(&qpData, @ACADO_N@, @ACADO_NX@, @ACADO_NU@, nD, &( qpOptions )); - - for (kk = 0; kk < @ACADO_N@; ++kk) - { - qpData.intervals[ kk ]->H.sparsityType = @DIAG_H@ ? QPDUNES_DIAGONAL : QPDUNES_DENSE; - } - qpData.intervals[ @ACADO_N@ ]->H.sparsityType = @DIAG_HN@ ? QPDUNES_DIAGONAL : QPDUNES_DENSE; - - statusFlag = qpDUNES_init(&qpData, @QP_H@, @QP_G@, @QP_C@, @QP_c@, @QP_LB@, @QP_UB@, @QP_D@, @QP_LBA@, @QP_UBA@); - - return (int)statusFlag; -} - -void cleanupQpDunes( void ) -{ - qpDUNES_cleanup( &qpData ); -} - -int prepareQpDunes( void ) -{ - return_t statusFlag; - @MODULE_NAME@_timer tmr; - - @MODULE_NAME@_tic( &tmr ); - statusFlag = qpDUNES_updateData(&qpData, @QP_H@, @QP_G@, @QP_C@, @QP_c@, @QP_LB@, @QP_UB@, @QP_D@, @QP_LBA@, @QP_UBA@); - if (statusFlag != QPDUNES_OK) - return (int)statusFlag; - prepareQPtime = @MODULE_NAME@_toc( &tmr ); - - return (int)statusFlag; -} - -int solveQpDunes( void ) -{ - return_t statusFlag; - @MODULE_NAME@_timer tmr; - - @MODULE_NAME@_tic( &tmr ); - if ( @INITIAL_STATE_FIXED@ ) - statusFlag = qpDUNES_updateIntervalData(&qpData, qpData.intervals[ 0 ], 0, 0, 0, 0, @QP_LB_0@, @QP_UB_0@, 0, 0, 0, 0); - else - statusFlag = qpDUNES_updateIntervalData(&qpData, qpData.intervals[ @ACADO_N@ ], 0, @QP_G_N@, 0, 0, 0, 0, 0, 0, 0, 0); - if (statusFlag != QPDUNES_OK) - return (int)statusFlag; - updateQPtime = @MODULE_NAME@_toc( &tmr ); - - @MODULE_NAME@_tic( &tmr ); - statusFlag = qpDUNES_solve( &qpData ); - - qpDUNES_getPrimalSol(&qpData, @QP_PRIMAL@); - qpDUNES_getDualSol(&qpData, @QP_LAMBDA@, @QP_MU@); - solveQPtime = @MODULE_NAME@_toc( &tmr ); - - return (int)statusFlag; -} - +/******************************************************************************/ +/* */ +/* qpDUNES interface data and functions */ +/* */ +/******************************************************************************/ + +#include + +qpData_t qpData; +qpOptions_t qpOptions; + +#if @DIAG_H@ +#define QPDUNES_LS_HOMOTOPY_GRID_SEARCH 0 +#endif + +#include "@MODULE_NAME@_auxiliary_functions.h" +real_t prepareQPtime, updateQPtime, solveQPtime; + +int initializeQpDunes( void ) +{ + return_t statusFlag; + int kk; + + @QP_ND_ARRAY@ + + qpOptions = qpDUNES_setupDefaultOptions(); + qpOptions.maxIter = @MAX_ITER@; + qpOptions.printLevel = @PRINT_LEVEL@; + qpOptions.stationarityTolerance = 1.e-6; + qpOptions.regParam = 1.e-6; + qpOptions.newtonHessDiagRegTolerance = 1.e-8; + qpOptions.lsType = @DIAG_H@ ? QPDUNES_LS_ACCELERATED_GRADIENT_BISECTION_LS : QPDUNES_LS_HOMOTOPY_GRID_SEARCH; +/* qpOptions.lsType = QPDUNES_LS_BACKTRACKING_LS; */ + qpOptions.lineSearchReductionFactor = 0.1; + qpOptions.lineSearchMaxStepSize = 1.; + qpOptions.maxNumLineSearchIterations = 25; + qpOptions.maxNumLineSearchRefinementIterations = 25; +/* qpOptions.regType = QPDUNES_REG_SINGULAR_DIRECTIONS; */ + qpOptions.regType = QPDUNES_REG_LEVENBERG_MARQUARDT; + + qpDUNES_setup(&qpData, @ACADO_N@, @ACADO_NX@, @ACADO_NU@, nD, &( qpOptions )); + + for (kk = 0; kk < @ACADO_N@; ++kk) + { + qpData.intervals[ kk ]->H.sparsityType = @DIAG_H@ ? QPDUNES_DIAGONAL : QPDUNES_DENSE; + } + qpData.intervals[ @ACADO_N@ ]->H.sparsityType = @DIAG_HN@ ? QPDUNES_DIAGONAL : QPDUNES_DENSE; + + statusFlag = qpDUNES_init(&qpData, @QP_H@, @QP_G@, @QP_C@, @QP_c@, @QP_LB@, @QP_UB@, @QP_D@, @QP_LBA@, @QP_UBA@); + + return (int)statusFlag; +} + +void cleanupQpDunes( void ) +{ + qpDUNES_cleanup( &qpData ); +} + +int prepareQpDunes( void ) +{ + return_t statusFlag; + @MODULE_NAME@_timer tmr; + + @MODULE_NAME@_tic( &tmr ); + statusFlag = qpDUNES_updateData(&qpData, @QP_H@, @QP_G@, @QP_C@, @QP_c@, @QP_LB@, @QP_UB@, @QP_D@, @QP_LBA@, @QP_UBA@); + if (statusFlag != QPDUNES_OK) + return (int)statusFlag; + prepareQPtime = @MODULE_NAME@_toc( &tmr ); + + return (int)statusFlag; +} + +int solveQpDunes( void ) +{ + return_t statusFlag; + @MODULE_NAME@_timer tmr; + + @MODULE_NAME@_tic( &tmr ); + if ( @INITIAL_STATE_FIXED@ ) + statusFlag = qpDUNES_updateIntervalData(&qpData, qpData.intervals[ 0 ], 0, 0, 0, 0, @QP_LB_0@, @QP_UB_0@, 0, 0, 0, 0); + else + statusFlag = qpDUNES_updateIntervalData(&qpData, qpData.intervals[ @ACADO_N@ ], 0, @QP_G_N@, 0, 0, 0, 0, 0, 0, 0, 0); + if (statusFlag != QPDUNES_OK) + return (int)statusFlag; + updateQPtime = @MODULE_NAME@_toc( &tmr ); + + @MODULE_NAME@_tic( &tmr ); + statusFlag = qpDUNES_solve( &qpData ); + + qpDUNES_getPrimalSol(&qpData, @QP_PRIMAL@); + qpDUNES_getDualSol(&qpData, @QP_LAMBDA@, @QP_MU@); + solveQPtime = @MODULE_NAME@_toc( &tmr ); + + return (int)statusFlag; +} + diff --git a/phonelibs/acado/include/acado/code_generation/templates/qpoases3_interface.c.in b/phonelibs/acado/include/acado/code_generation/templates/qpoases3_interface.c.in index d3ddba7d..52673ed1 100644 --- a/phonelibs/acado/include/acado/code_generation/templates/qpoases3_interface.c.in +++ b/phonelibs/acado/include/acado/code_generation/templates/qpoases3_interface.c.in @@ -1,60 +1,60 @@ -#include <@ACADO_COMMON_HEADER@> - -#include "qpOASES_e/@SOLVER_NAME@.h" - -#if @MODULE_PREFIX@_COMPUTE_COVARIANCE_MATRIX == 1 -#include "qpOASES_e/extras/SolutionAnalysis.h" -#endif /* @MODULE_PREFIX@_COMPUTE_COVARIANCE_MATRIX */ - -static int @MODULE_NAME@_@PREFIX@nWSR; - -@USE_NAMESPACE@ - -#if @MODULE_PREFIX@_COMPUTE_COVARIANCE_MATRIX == 1 -static SolutionAnalysis @MODULE_NAME@_sa; -#endif /* @MODULE_PREFIX@_COMPUTE_COVARIANCE_MATRIX */ - -int @MODULE_NAME@_@PREFIX@solve( void ) -{ - - returnValue retVal; - @SOLVER_NAME@ qp; - Options options; - - @MODULE_NAME@_@PREFIX@nWSR = QPOASES_NWSRMAX; - - @CTOR@; - Options_setToMPC( &options ); - @SOLVER_NAME@_setOptions( &qp,options ); - - retVal = @SOLVER_NAME@_@CALL_SOLVER@ - retVal = qpOASES_getSimpleStatus( retVal,0 ); - - /* only use solution on success, if iteration limit has been reached or if QP is infeasible! */ - if ( ( retVal == 0 ) || ( retVal == 1 ) || ( retVal == -2 ) ) - { - @SOLVER_NAME@_getPrimalSolution( &qp,@PRIMAL_SOLUTION@ ); - @SOLVER_NAME@_getDualSolution( &qp,@DUAL_SOLUTION@ ); - } - -#if @MODULE_PREFIX@_COMPUTE_COVARIANCE_MATRIX == 1 - - if (retVal != SUCCESSFUL_RETURN) - return (int)retVal; - - retVal = @MODULE_NAME@_sa.getHessianInverse( &qp,@SIGMA@ ); - -#endif /* @MODULE_PREFIX@_COMPUTE_COVARIANCE_MATRIX */ - - return (int)retVal; -} - -int @MODULE_NAME@_@PREFIX@getNWSR( void ) -{ - return @MODULE_NAME@_@PREFIX@nWSR; -} - -const char* @MODULE_NAME@_@PREFIX@getErrorString( int error ) -{ - return MessageHandling_getErrorCodeMessage( qpOASES_getGlobalMessageHandler(),error ); -} +#include <@ACADO_COMMON_HEADER@> + +#include "qpOASES_e/@SOLVER_NAME@.h" + +#if @MODULE_PREFIX@_COMPUTE_COVARIANCE_MATRIX == 1 +#include "qpOASES_e/extras/SolutionAnalysis.h" +#endif /* @MODULE_PREFIX@_COMPUTE_COVARIANCE_MATRIX */ + +static int @MODULE_NAME@_@PREFIX@nWSR; + +@USE_NAMESPACE@ + +#if @MODULE_PREFIX@_COMPUTE_COVARIANCE_MATRIX == 1 +static SolutionAnalysis @MODULE_NAME@_sa; +#endif /* @MODULE_PREFIX@_COMPUTE_COVARIANCE_MATRIX */ + +int @MODULE_NAME@_@PREFIX@solve( void ) +{ + + returnValue retVal; + @SOLVER_NAME@ qp; + Options options; + + @MODULE_NAME@_@PREFIX@nWSR = QPOASES_NWSRMAX; + + @CTOR@; + Options_setToMPC( &options ); + @SOLVER_NAME@_setOptions( &qp,options ); + + retVal = @SOLVER_NAME@_@CALL_SOLVER@ + retVal = qpOASES_getSimpleStatus( retVal,0 ); + + /* only use solution on success, if iteration limit has been reached or if QP is infeasible! */ + if ( ( retVal == 0 ) || ( retVal == 1 ) || ( retVal == -2 ) ) + { + @SOLVER_NAME@_getPrimalSolution( &qp,@PRIMAL_SOLUTION@ ); + @SOLVER_NAME@_getDualSolution( &qp,@DUAL_SOLUTION@ ); + } + +#if @MODULE_PREFIX@_COMPUTE_COVARIANCE_MATRIX == 1 + + if (retVal != SUCCESSFUL_RETURN) + return (int)retVal; + + retVal = @MODULE_NAME@_sa.getHessianInverse( &qp,@SIGMA@ ); + +#endif /* @MODULE_PREFIX@_COMPUTE_COVARIANCE_MATRIX */ + + return (int)retVal; +} + +int @MODULE_NAME@_@PREFIX@getNWSR( void ) +{ + return @MODULE_NAME@_@PREFIX@nWSR; +} + +const char* @MODULE_NAME@_@PREFIX@getErrorString( int error ) +{ + return MessageHandling_getErrorCodeMessage( qpOASES_getGlobalMessageHandler(),error ); +} diff --git a/phonelibs/acado/include/acado/code_generation/templates/templates.hpp b/phonelibs/acado/include/acado/code_generation/templates/templates.hpp index a12b030b..dee8104b 100644 --- a/phonelibs/acado/include/acado/code_generation/templates/templates.hpp +++ b/phonelibs/acado/include/acado/code_generation/templates/templates.hpp @@ -1,7 +1,7 @@ #ifndef ACADO_TOOLKIT_TEMPLATES_HPP #define ACADO_TOOLKIT_TEMPLATES_HPP -#define TEMPLATE_PATHS "/home/batman/tmpbld/acado/acado/code_generation/templates;/home/batman/one/phonelibs/acado/include/acado/code_generation/templates;/data/data/com.termux/files/home/one/phonelibs/acado/include/acado/code_generation/templates" +#define TEMPLATE_PATHS "/home/batman/openpilot/phonelibs/acado/acado/acado/code_generation/templates;/home/batman/openpilot/phonelibs/acado/include/acado/code_generation/templates" #define INTEGRATOR_MEX_TEMPLATE "integrator_mex.c.in" #define RHS_MEX_TEMPLATE "rhs_mex.c.in" diff --git a/phonelibs/acado/include/acado/external_packages/eigen3/COPYING.MINPACK b/phonelibs/acado/include/acado/external_packages/eigen3/COPYING.MINPACK index ae7984da..11d8a9a6 100644 --- a/phonelibs/acado/include/acado/external_packages/eigen3/COPYING.MINPACK +++ b/phonelibs/acado/include/acado/external_packages/eigen3/COPYING.MINPACK @@ -1,52 +1,52 @@ -Minpack Copyright Notice (1999) University of Chicago. All rights reserved - -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. - -3. The end-user documentation included with the -redistribution, if any, must include the following -acknowledgment: - - "This product includes software developed by the - University of Chicago, as Operator of Argonne National - Laboratory. - -Alternately, this acknowledgment may appear in the software -itself, if and wherever such third-party acknowledgments -normally appear. - -4. WARRANTY DISCLAIMER. THE SOFTWARE IS SUPPLIED "AS IS" -WITHOUT WARRANTY OF ANY KIND. THE COPYRIGHT HOLDER, THE -UNITED STATES, THE UNITED STATES DEPARTMENT OF ENERGY, AND -THEIR EMPLOYEES: (1) DISCLAIM ANY WARRANTIES, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO ANY IMPLIED WARRANTIES -OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, TITLE -OR NON-INFRINGEMENT, (2) DO NOT ASSUME ANY LEGAL LIABILITY -OR RESPONSIBILITY FOR THE ACCURACY, COMPLETENESS, OR -USEFULNESS OF THE SOFTWARE, (3) DO NOT REPRESENT THAT USE OF -THE SOFTWARE WOULD NOT INFRINGE PRIVATELY OWNED RIGHTS, (4) -DO NOT WARRANT THAT THE SOFTWARE WILL FUNCTION -UNINTERRUPTED, THAT IT IS ERROR-FREE OR THAT ANY ERRORS WILL -BE CORRECTED. - -5. LIMITATION OF LIABILITY. IN NO EVENT WILL THE COPYRIGHT -HOLDER, THE UNITED STATES, THE UNITED STATES DEPARTMENT OF -ENERGY, OR THEIR EMPLOYEES: BE LIABLE FOR ANY INDIRECT, -INCIDENTAL, CONSEQUENTIAL, SPECIAL OR PUNITIVE DAMAGES OF -ANY KIND OR NATURE, INCLUDING BUT NOT LIMITED TO LOSS OF -PROFITS OR LOSS OF DATA, FOR ANY REASON WHATSOEVER, WHETHER -SUCH LIABILITY IS ASSERTED ON THE BASIS OF CONTRACT, TORT -(INCLUDING NEGLIGENCE OR STRICT LIABILITY), OR OTHERWISE, -EVEN IF ANY OF SAID PARTIES HAS BEEN WARNED OF THE -POSSIBILITY OF SUCH LOSS OR DAMAGES. - +Minpack Copyright Notice (1999) University of Chicago. All rights reserved + +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. + +3. The end-user documentation included with the +redistribution, if any, must include the following +acknowledgment: + + "This product includes software developed by the + University of Chicago, as Operator of Argonne National + Laboratory. + +Alternately, this acknowledgment may appear in the software +itself, if and wherever such third-party acknowledgments +normally appear. + +4. WARRANTY DISCLAIMER. THE SOFTWARE IS SUPPLIED "AS IS" +WITHOUT WARRANTY OF ANY KIND. THE COPYRIGHT HOLDER, THE +UNITED STATES, THE UNITED STATES DEPARTMENT OF ENERGY, AND +THEIR EMPLOYEES: (1) DISCLAIM ANY WARRANTIES, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO ANY IMPLIED WARRANTIES +OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, TITLE +OR NON-INFRINGEMENT, (2) DO NOT ASSUME ANY LEGAL LIABILITY +OR RESPONSIBILITY FOR THE ACCURACY, COMPLETENESS, OR +USEFULNESS OF THE SOFTWARE, (3) DO NOT REPRESENT THAT USE OF +THE SOFTWARE WOULD NOT INFRINGE PRIVATELY OWNED RIGHTS, (4) +DO NOT WARRANT THAT THE SOFTWARE WILL FUNCTION +UNINTERRUPTED, THAT IT IS ERROR-FREE OR THAT ANY ERRORS WILL +BE CORRECTED. + +5. LIMITATION OF LIABILITY. IN NO EVENT WILL THE COPYRIGHT +HOLDER, THE UNITED STATES, THE UNITED STATES DEPARTMENT OF +ENERGY, OR THEIR EMPLOYEES: BE LIABLE FOR ANY INDIRECT, +INCIDENTAL, CONSEQUENTIAL, SPECIAL OR PUNITIVE DAMAGES OF +ANY KIND OR NATURE, INCLUDING BUT NOT LIMITED TO LOSS OF +PROFITS OR LOSS OF DATA, FOR ANY REASON WHATSOEVER, WHETHER +SUCH LIABILITY IS ASSERTED ON THE BASIS OF CONTRACT, TORT +(INCLUDING NEGLIGENCE OR STRICT LIABILITY), OR OTHERWISE, +EVEN IF ANY OF SAID PARTIES HAS BEEN WARNED OF THE +POSSIBILITY OF SUCH LOSS OR DAMAGES. + diff --git a/phonelibs/acado/x64/lib/libacado_casadi.a b/phonelibs/acado/x64/lib/libacado_casadi.a deleted file mode 100644 index 231bc7d5..00000000 --- a/phonelibs/acado/x64/lib/libacado_casadi.a +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:9b4709c7fd3a4e6bdddfaee0d4482087ba1ea1a90c71d4a16652f1b363e010e8 -size 49744 diff --git a/phonelibs/acado/x64/lib/libacado_csparse.a b/phonelibs/acado/x64/lib/libacado_csparse.a deleted file mode 100644 index 0e146419..00000000 --- a/phonelibs/acado/x64/lib/libacado_csparse.a +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:ebe06e3c6876190aae26d48b1dfbfb5a50d1887e5d6e6482669e48caabc1a80e -size 141978 diff --git a/phonelibs/acado/x64/lib/libacado_qpoases.a b/phonelibs/acado/x64/lib/libacado_qpoases.a deleted file mode 100644 index 461e1e21..00000000 --- a/phonelibs/acado/x64/lib/libacado_qpoases.a +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:4cf01f68ba2dca1054334af49c77b443267b33551efe188f3176ac176b93fd7e -size 754948 diff --git a/phonelibs/acado/x64/lib/libacado_toolkit.a b/phonelibs/acado/x64/lib/libacado_toolkit.a deleted file mode 100644 index 8726af14..00000000 --- a/phonelibs/acado/x64/lib/libacado_toolkit.a +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:469251cfd2abb76655132f3de1da86f91fe1f581701fe1780cb1c77074d7fa3b -size 26405540 diff --git a/phonelibs/acado/x64/lib/libacado_toolkit_s.so b/phonelibs/acado/x64/lib/libacado_toolkit_s.so deleted file mode 120000 index 08cbf4e4..00000000 --- a/phonelibs/acado/x64/lib/libacado_toolkit_s.so +++ /dev/null @@ -1 +0,0 @@ -libacado_toolkit_s.so.1 \ No newline at end of file diff --git a/phonelibs/acado/x64/lib/libacado_toolkit_s.so.1 b/phonelibs/acado/x64/lib/libacado_toolkit_s.so.1 deleted file mode 120000 index 228d0129..00000000 --- a/phonelibs/acado/x64/lib/libacado_toolkit_s.so.1 +++ /dev/null @@ -1 +0,0 @@ -libacado_toolkit_s.so.1.2.2beta \ No newline at end of file diff --git a/phonelibs/acado/x64/lib/libacado_toolkit_s.so.1.2.2beta b/phonelibs/acado/x64/lib/libacado_toolkit_s.so.1.2.2beta deleted file mode 100755 index 1faef381..00000000 --- a/phonelibs/acado/x64/lib/libacado_toolkit_s.so.1.2.2beta +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:72239a2333dadef2e351e0a627c68e43fef7d8d6c9ea674f07b6c81d91f4db5e -size 10249488 diff --git a/phonelibs/acado/aarch64/lib/libacado_toolkit_s.so.1 b/phonelibs/acado/x86_64/lib/libacado_toolkit_s.so.1 similarity index 100% rename from phonelibs/acado/aarch64/lib/libacado_toolkit_s.so.1 rename to phonelibs/acado/x86_64/lib/libacado_toolkit_s.so.1 diff --git a/phonelibs/acado/x86_64/lib/libacado_toolkit_s.so.1.2.2beta b/phonelibs/acado/x86_64/lib/libacado_toolkit_s.so.1.2.2beta new file mode 100644 index 00000000..b6da672c --- /dev/null +++ b/phonelibs/acado/x86_64/lib/libacado_toolkit_s.so.1.2.2beta @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:178937c40ec3326eab01c62f3239a885cbfb1308af65436d831dd4fa6a7d619e +size 10304768 diff --git a/selfdrive/controls/lib/lateral_mpc/SConscript b/selfdrive/controls/lib/lateral_mpc/SConscript index 6cdc22d1..e609c914 100644 --- a/selfdrive/controls/lib/lateral_mpc/SConscript +++ b/selfdrive/controls/lib/lateral_mpc/SConscript @@ -7,23 +7,40 @@ cpp_path = [ "#phonelibs/qpoases/INCLUDE/EXTRAS", "#phonelibs/qpoases/SRC/", "#phonelibs/qpoases", - "lib_mpc_export" - + "lib_mpc_export", ] -mpc_files = [ - "lateral_mpc.c", - Glob("lib_mpc_export/*.c"), - Glob("lib_mpc_export/*.cpp"), +generated_c = [ + 'lib_mpc_export/acado_auxiliary_functions.c', + 'lib_mpc_export/acado_qpoases_interface.cpp', + 'lib_mpc_export/acado_integrator.c', + 'lib_mpc_export/acado_solver.c', +] + +generated_h = [ + 'lib_mpc_export/acado_common.h', + 'lib_mpc_export/acado_auxiliary_functions.h', + 'lib_mpc_export/acado_qpoases_interface.hpp', ] interface_dir = Dir('lib_mpc_export') SConscript(['#phonelibs/qpoases/SConscript'], variant_dir='lib_qp', exports=['interface_dir']) +if GetOption('mpc_generate'): + generator_cpp = File('generator.cpp') + + acado_libs = [File(f"#phonelibs/acado/{arch}/lib/libacado_toolkit.a"), + File(f"#phonelibs/acado/{arch}/lib/libacado_casadi.a"), + File(f"#phonelibs/acado/{arch}/lib/libacado_csparse.a")] + + generator = env.Program('generator', generator_cpp, LIBS=acado_libs, CPPPATH=cpp_path, + CCFLAGS=env['CCFLAGS'] + ["-Wno-deprecated", "-Wno-overloaded-shift-op-parentheses"]) + + cmd = f"cd {Dir('.').get_abspath()} && {generator[0].get_abspath()}" + env.Command(generated_c + generated_h, generator, cmd) + + + +mpc_files = ["lateral_mpc.c"] + generated_c env.SharedLibrary('mpc', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path) -# if arch != "aarch64": -# acado_libs = [File("#phonelibs/acado/x64/lib/libacado_toolkit.a"), -# File("#phonelibs/acado/x64/lib/libacado_casadi.a"), -# File("#phonelibs/acado/x64/lib/libacado_csparse.a")] -# env.Program('generator', 'generator.cpp', LIBS=acado_libs, CPPPATH=cpp_path) diff --git a/selfdrive/controls/lib/longitudinal_mpc/SConscript b/selfdrive/controls/lib/longitudinal_mpc/SConscript index 072ff646..584d5103 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/SConscript +++ b/selfdrive/controls/lib/longitudinal_mpc/SConscript @@ -8,25 +8,41 @@ cpp_path = [ "#phonelibs/qpoases/INCLUDE/EXTRAS", "#phonelibs/qpoases/SRC/", "#phonelibs/qpoases", - "lib_mpc_export" - + "lib_mpc_export", ] -mpc_files = [ - "longitudinal_mpc.c", - Glob("lib_mpc_export/*.c"), - Glob("lib_mpc_export/*.cpp"), +generated_c = [ + 'lib_mpc_export/acado_auxiliary_functions.c', + 'lib_mpc_export/acado_qpoases_interface.cpp', + 'lib_mpc_export/acado_integrator.c', + 'lib_mpc_export/acado_solver.c', ] +generated_h = [ + 'lib_mpc_export/acado_common.h', + 'lib_mpc_export/acado_auxiliary_functions.h', + 'lib_mpc_export/acado_qpoases_interface.hpp', +] + + interface_dir = Dir('lib_mpc_export') SConscript(['#phonelibs/qpoases/SConscript'], variant_dir='lib_qp', exports=['interface_dir']) +if GetOption('mpc_generate'): + generator_cpp = File('generator.cpp') + + acado_libs = [File(f"#phonelibs/acado/{arch}/lib/libacado_toolkit.a"), + File(f"#phonelibs/acado/{arch}/lib/libacado_casadi.a"), + File(f"#phonelibs/acado/{arch}/lib/libacado_csparse.a")] + + generator = env.Program('generator', generator_cpp, LIBS=acado_libs, CPPPATH=cpp_path, + CCFLAGS=env['CCFLAGS'] + ["-Wno-deprecated", "-Wno-overloaded-shift-op-parentheses"]) + + cmd = f"cd {Dir('.').get_abspath()} && {generator[0].get_abspath()}" + env.Command(generated_c + generated_h, generator, cmd) + + +mpc_files = ["longitudinal_mpc.c"] + generated_c env.SharedLibrary('mpc1', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path) env.SharedLibrary('mpc2', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path) - -# if arch != "aarch64": -# acado_libs = [File("#phonelibs/acado/x64/lib/libacado_toolkit.a"), -# File("#phonelibs/acado/x64/lib/libacado_casadi.a"), -# File("#phonelibs/acado/x64/lib/libacado_csparse.a")] -# env.Program('generator', 'generator.cpp', LIBS=acado_libs, CPPPATH=cpp_path)