Upstreamed and update acados (#23927)

* change acados version to finalized PR https://github.com/acados/acados/pull/791

* acados: update include and x86_64 build

* acados: update pyextra

* adapt mpc files to updated acados

* acados buid: clear acados_template and include dir before updating

* update acados again

* acados MPC: only generate solver, building is done via scons

* update acados to latest acados/master

* c3 libs

* c2 libs

* acados scons files: add acados build script as a dependency

* acados scons files: add libacados.so and ocp_nlp_interface.h, build script not in files_common

* update acados

* adapts long & lat MPC file

* update acados reference commit to acados/master

Co-authored-by: Comma Device <device@comma.ai>
old-commit-hash: d653dd843a

pyextra/acados_template/acados_layout.json

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:81426176ea04cb7ed7b4b17c2941f49dcc8c2d576c2fc78ff158a6e9ec2839e5
+oid sha256:5c727abe27e9d89f04baca65f02c77ad8dcb918cd9f782df0575c01350dc8a5f
-size 13723
+size 14205

pyextra/acados_template/acados_model.py

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:a27c58335c2ffae67468f2cbac2ecfd59300d8af5a97b15cc29273dff20a0aef
+oid sha256:af19df99d9669672bfdc72d959bd444919a16c800f764873c923db425c26e70d
-size 8020
+size 8289

pyextra/acados_template/acados_ocp.py

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:7bbb8ea1f67d4b2247be1eb7e4e6c2d5cc89fac823d0ca4337804a8c0f8de387
+oid sha256:d85c58ec2b66a50cb1008cc134e43278afc38f9ba261aee1a4c02495895e237c
-size 95897
+size 99987

pyextra/acados_template/acados_ocp_solver.py

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:4463ca9b30ad759498497085bfb94e4d11994dfb88e915aa209c124da13254c1
+oid sha256:c8cfc4b83e74e6841aa4572535e6e42bc82e76c62f7294b3a93ec438995d694a
-size 70093
+size 78430

pyextra/acados_template/acados_ocp_solver_fast.py

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:00bc74d60d2727be6c6898eb6e034fc1c626dc1361e1a5ae2e4910b844d9dbb5
-size 20493

pyextra/acados_template/acados_ocp_solver_pyx.pyx

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:da810ba4a48d1be64fbad09946672779f5d5401de56aa38ea9da8fdbe719e9e4
+oid sha256:22f6a541bce66f0d2d050295c1c7dae17aeaff2daf7aeb3e5dcc7194ae869cac
-size 22704
+size 28607

pyextra/acados_template/acados_sim.py

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:534e0fcc2b75f6217fb748e9c8a1c365ad2a3b201bcba895484995fb5ed78133
+oid sha256:3c74e04339b39aa20917d85d4a7743f18bca6a2d0a3d9524aead6dfb5843aec9
-size 11803
+size 11857

pyextra/acados_template/acados_sim_solver.py

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:8d118afbf4993842e202870ff43675d6837befa3a552ddace49b334e6715bb72
+oid sha256:33e2008b08a828e396f79169c888934a5a42a1acbae5e504222a4de345a3128e
-size 16342
+size 17417

pyextra/acados_template/acados_solver_common.pxd

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:dd53981de52f81cccaf7b35054bde06dba358c3425f792877fa837e9e74ecf1a
+oid sha256:9166b8d29e88c53739da15d79f1a39ef007bac88b3219af171f738f57937286f
-size 4216
+size 4335

pyextra/acados_template/c_templates_tera/Makefile.in

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:ae30257d087cf8a0e41a9c9d6c41bb9bc1700059678bc7e82cf899f32f26c71a
+oid sha256:a5cfdabde7dd957124c1dcac1ad41c5144f7359b95498bbdc833d6e50bcbfea9
-size 19209
+size 13858

pyextra/acados_template/c_templates_tera/acados_mex_create.in.c

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:b69ab4513e92ed696e708028a1862fae9f6f38f6c0c3de284f198e5e189fb9b0
+oid sha256:5a837cf6930efa41eb33fe32bd126ac5acea7afcac8ff3f6aee65e1ec5f974a3
-size 17381
+size 17505

pyextra/acados_template/c_templates_tera/acados_mex_set.in.c

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:7cfae123bb078e0637621b339a14d0b776f9c451ba50939ed9ec341a8daa030e
+oid sha256:cac8dd3c940a2b07d7104aeae846f4238a711a973da1667fa7bfdf91dea65cd4
-size 19912
+size 19922

pyextra/acados_template/c_templates_tera/acados_sim_solver.in.c

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:d1ed4a97c47e85468b4a19bb0b0dae365f5b13bff7054a58e547a2e530570ee0
+oid sha256:f2a7be5c4f4c3afdcb7e252b72c90a8c0f92992c63bd97977f0470745feb2b6e
-size 25463
+size 26035

pyextra/acados_template/c_templates_tera/acados_sim_solver_sfun.in.c

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:862c27f90f69e5de2d8d6e6e9b8b365eacd9cb9f593b30fb05c9f8ef5bfe3b46
+oid sha256:b1ee39cbcb027288fcaae01deaafba0f7ccfa863f3cf30a19a2e19860ab32e1f
-size 7223
+size 7226

pyextra/acados_template/c_templates_tera/acados_solver.in.c

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:46068c324d8a79f92eb0b758067e68708829fcd31b6f6106fdd57ddaa3ba4e00
+oid sha256:6755f5a6a891a5ac691b5c916679a266cddfd0e1ea2cf2b66700197e03668ab6
-size 115538
+size 98679

pyextra/acados_template/c_templates_tera/acados_solver.in.h

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:4d0202112aabfba5b642e7bab59c4608d3227981ba22509166f9ce3c55a3a39b
+oid sha256:eb3e1902761787e0d910b826baf6c4a9fb968a525e4463949244d53ec2430c6b
-size 9834
+size 10191

pyextra/acados_template/c_templates_tera/acados_solver.in.pxd

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:2d92fa9835ac4149aa8ad7a4a29a8b4c1a6234c22d43c31df313f94e2602f9e9
+oid sha256:51a0b126e0f9fda18acc137766af4f5c669ec8d60e847b254aeb175388776548
-size 1708
+size 3917

pyextra/acados_template/c_templates_tera/acados_solver_sfun.in.c

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:6395761be4c3e202197dd6d1953d52299260f2ff6a9fd13bf122a752fdc2467b
+oid sha256:f5b7c14eed81ec50f8cbbba42c74c71558df8d29651b682a90519b0918d9f5ae
-size 28556
+size 28559

pyextra/acados_template/c_templates_tera/main.in.c

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:c30036c8497a66d000b05c7dcf8e7f37a3fed477885cf5d9d4600eacbc6cf96d
+oid sha256:b0476231619e9b58e9d9f1ac38b23d7b07267af02b9fc059b3e96a881f692cbb
-size 7983
+size 8044

pyextra/acados_template/c_templates_tera/make_sfun.in.m

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:0b9bcfda52a1857bfc34ab6c91aeeda13c887c11340ca030cb9c19757a80df9f
+oid sha256:3ba6545310bbb8c2cf6864278a3129d05c11b84ff9de2caa464397740b9ed8ca
-size 15205
+size 15359

pyextra/acados_template/c_templates_tera/make_sfun_sim.in.m

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:6f92ead3b7f21387003b5d67de12484aba20b8c2ca0c415c197eb715304d8f84
+oid sha256:eeb1550ed63d4d9d561a133db36c4d11c9d03634f466f2ab80ba54ac69dac83a
-size 4326
+size 4496

pyextra/acados_template/c_templates_tera/mex_solver.in.m

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:04dab04856cf83d6235ef93aaa1e7d848e54fc0fc3d4bfe9ab0e91ae3a51e245
+oid sha256:4ac71216686385bcf4dedd49a2e4b85e510f36b9d1e1748245d0a7abefad0d9d
-size 6160
+size 6185

pyextra/acados_template/c_templates_tera/model.in.h

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:d13a880b927a698e088b2fed7231e76e58bb1e3a394ca62d6703404f4bdb83bd
+oid sha256:64753b01879d27e824a86c26ca15b29956efa3b2a5f536dbe78220df4f01fbdc
-size 10312
+size 10420

pyextra/acados_template/utils.py

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:3172aa8e2130b7d411bc330e0b7b3038ee0896db22bcdee9ef045392f03ea6ce
+oid sha256:4e4cd82cf613ac671f5b5dde04063c8e2e556edfebdc34a0c971f9456a33a5cd
-size 15924
+size 15987

selfdrive/controls/lib/lateral_mpc_lib/SConscript

@@ -43,11 +43,19 @@ generated_files = [
   f'{gen}/lat_cost/lat_cost_y_0_fun.h',
 ] + build_files
 
+acados_dir = '#third_party/acados'
+source_list = ['lat_mpc.py',
+  f'{acados_dir}/include/acados_c/ocp_nlp_interface.h',
+  f'{acados_dir}/aarch64/lib/libacados.so',
+  f'{acados_dir}/x86_64/lib/libacados.so',
+  f'{acados_dir}/larch64/lib/libacados.so',
+]
+
 lenv = env.Clone()
 lenv.Clean(generated_files, Dir(gen))
 
 lenv.Command(generated_files,
-             ["lat_mpc.py"],
+             source_list,
              f"cd {Dir('.').abspath} && python3 lat_mpc.py")
 
 lenv["CFLAGS"].append("-DACADOS_WITH_QPOASES")

selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py

@@ -11,18 +11,19 @@ from selfdrive.modeld.constants import T_IDXS
 if __name__ == '__main__':  # generating code
   from pyextra.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
 else:
-  # from pyextra.acados_template import AcadosOcpSolverFast
+  from selfdrive.controls.lib.lateral_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython  # pylint: disable=no-name-in-module, import-error
-  from selfdrive.controls.lib.lateral_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverFast  # pylint: disable=no-name-in-module, import-error
 
 LAT_MPC_DIR = os.path.dirname(os.path.abspath(__file__))
 EXPORT_DIR = os.path.join(LAT_MPC_DIR, "c_generated_code")
-JSON_FILE = "acados_ocp_lat.json"
+JSON_FILE = os.path.join(LAT_MPC_DIR, "acados_ocp_lat.json")
 X_DIM = 4
 P_DIM = 2
+MODEL_NAME = 'lat'
+ACADOS_SOLVER_TYPE = 'SQP_RTI'
 
 def gen_lat_model():
   model = AcadosModel()
-  model.name = 'lat'
+  model.name = MODEL_NAME
 
   # set up states & controls
   x_ego = SX.sym('x_ego')
@@ -58,7 +59,7 @@ def gen_lat_model():
   return model
 
 
-def gen_lat_mpc_solver():
+def gen_lat_ocp():
   ocp = AcadosOcp()
   ocp.model = gen_lat_model()
 
@@ -103,7 +104,7 @@ def gen_lat_mpc_solver():
   ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
   ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
   ocp.solver_options.integrator_type = 'ERK'
-  ocp.solver_options.nlp_solver_type = 'SQP_RTI'
+  ocp.solver_options.nlp_solver_type = ACADOS_SOLVER_TYPE
   ocp.solver_options.qp_solver_iter_max = 1
   ocp.solver_options.qp_solver_cond_N = 1
 
@@ -117,7 +118,7 @@ def gen_lat_mpc_solver():
 
 class LateralMpc():
   def __init__(self, x0=np.zeros(X_DIM)):
-    self.solver = AcadosOcpSolverFast('lat', N)
+    self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
     self.reset(x0)
 
   def reset(self, x0=np.zeros(X_DIM)):
@@ -173,5 +174,6 @@ class LateralMpc():
 
 
 if __name__ == "__main__":
-  ocp = gen_lat_mpc_solver()
+  ocp = gen_lat_ocp()
-  AcadosOcpSolver.generate(ocp, json_file=JSON_FILE, build=False)
+  AcadosOcpSolver.generate(ocp, json_file=JSON_FILE)
+  # AcadosOcpSolver.build(ocp.code_export_directory, with_cython=True)

selfdrive/controls/lib/longitudinal_mpc_lib/SConscript

@@ -50,11 +50,19 @@ generated_files = [
   f'{gen}/long_cost/long_cost_y_0_fun.h',
 ] + build_files
 
+acados_dir = '#third_party/acados'
+source_list = ['long_mpc.py',
+  f'{acados_dir}/include/acados_c/ocp_nlp_interface.h',
+  f'{acados_dir}/aarch64/lib/libacados.so',
+  f'{acados_dir}/x86_64/lib/libacados.so',
+  f'{acados_dir}/larch64/lib/libacados.so',
+]
+
 lenv = env.Clone()
 lenv.Clean(generated_files, Dir(gen))
 
 lenv.Command(generated_files,
-             ["long_mpc.py"],
+             source_list,
              f"cd {Dir('.').abspath} && python3 long_mpc.py")
 
 lenv["CFLAGS"].append("-DACADOS_WITH_QPOASES")

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -11,14 +11,14 @@ from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU
 if __name__ == '__main__':  # generating code
   from pyextra.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
 else:
-  # from pyextra.acados_template import AcadosOcpSolver as AcadosOcpSolverFast
+  from selfdrive.controls.lib.longitudinal_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython  # pylint: disable=no-name-in-module, import-error
-  from selfdrive.controls.lib.longitudinal_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverFast  # pylint: disable=no-name-in-module, import-error
 
 from casadi import SX, vertcat
 
+MODEL_NAME = 'long'
 LONG_MPC_DIR = os.path.dirname(os.path.abspath(__file__))
 EXPORT_DIR = os.path.join(LONG_MPC_DIR, "c_generated_code")
-JSON_FILE = "acados_ocp_long.json"
+JSON_FILE = os.path.join(LONG_MPC_DIR, "acados_ocp_long.json")
 
 SOURCES = ['lead0', 'lead1', 'cruise']
 
@@ -38,6 +38,7 @@ A_CHANGE_COST = 200.
 DANGER_ZONE_COST = 100.
 CRASH_DISTANCE = .5
 LIMIT_COST = 1e6
+ACADOS_SOLVER_TYPE = 'SQP_RTI'
 
 
 # Fewer timestamps don't hurt performance and lead to
@@ -65,7 +66,7 @@ def desired_follow_distance(v_ego, v_lead):
 
 def gen_long_model():
   model = AcadosModel()
-  model.name = 'long'
+  model.name = MODEL_NAME
 
   # set up states & controls
   x_ego = SX.sym('x_ego')
@@ -97,7 +98,7 @@ def gen_long_model():
   return model
 
 
-def gen_long_mpc_solver():
+def gen_long_ocp():
   ocp = AcadosOcp()
   ocp.model = gen_long_model()
 
@@ -172,7 +173,7 @@ def gen_long_mpc_solver():
   ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
   ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
   ocp.solver_options.integrator_type = 'ERK'
-  ocp.solver_options.nlp_solver_type = 'SQP_RTI'
+  ocp.solver_options.nlp_solver_type = ACADOS_SOLVER_TYPE
   ocp.solver_options.qp_solver_cond_N = 1
 
   # More iterations take too much time and less lead to inaccurate convergence in
@@ -195,7 +196,7 @@ class LongitudinalMpc:
     self.source = SOURCES[2]
 
   def reset(self):
-    self.solver = AcadosOcpSolverFast('long', N)
+    self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
     self.v_solution = np.zeros(N+1)
     self.a_solution = np.zeros(N+1)
     self.prev_a = np.array(self.a_solution)
@@ -388,5 +389,6 @@ class LongitudinalMpc:
 
 
 if __name__ == "__main__":
-  ocp = gen_long_mpc_solver()
+  ocp = gen_long_ocp()
-  AcadosOcpSolver.generate(ocp, json_file=JSON_FILE, build=False)
+  AcadosOcpSolver.generate(ocp, json_file=JSON_FILE)
+  # AcadosOcpSolver.build(ocp.code_export_directory, with_cython=True)

third_party/acados/aarch64/lib/libacados.so

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:a42119de38b56c672e473edd8860e2e75c3e2793961b631634f8dc6f6f6618e5
+oid sha256:a4d51bdef2544ba6c3b7cf119f464b29b48f4759f96490ec3561ae5fc1200cf9
-size 575769
+size 571865

third_party/acados/aarch64/lib/libblasfeo.so

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:567bfcb693ce73c8faa847ddd3798037f611c8ebfcecddce4da2140f22b191af
+oid sha256:802d85593bac49cbf325a7a05b3c2dd909f09843e462e4eea526faaad7cc33a8
-size 694193
+size 694257

third_party/acados/build.sh

@@ -13,19 +13,19 @@ elif [ -f /EON ]; then
 fi
 
 if [ ! -d acados_repo/ ]; then
-  #git clone https://github.com/acados/acados.git $DIR/acados
+  git clone https://github.com/acados/acados.git $DIR/acados_repo
-  git clone https://github.com/commaai/acados.git $DIR/acados_repo
+  # git clone https://github.com/commaai/acados.git $DIR/acados_repo
 fi
 cd acados_repo
 git fetch
-git checkout 92b85c61f7358a1b08b7cd30aeb9d32ad15942e8
+git checkout 105e06df87f06ea02df4af825867c946b31defdd
 git submodule update --recursive --init
 
 # build
 mkdir -p build
 cd build
 cmake -DACADOS_WITH_QPOASES=ON -UBLASFEO_TARGET -DBLASFEO_TARGET=$BLAS_TARGET ..
-make -j4 install
+make -j20 install
 
 INSTALL_DIR="$DIR/$ARCHNAME"
 rm -rf $INSTALL_DIR
@@ -33,8 +33,10 @@ mkdir -p $INSTALL_DIR
 
 rm $DIR/acados_repo/lib/*.json
 
+rm -rf $DIR/include
 cp -r $DIR/acados_repo/include $DIR
 cp -r $DIR/acados_repo/lib $INSTALL_DIR
+rm -rf $DIR/../../pyextra/acados_template
 cp -r $DIR/acados_repo/interfaces/acados_template/acados_template $DIR/../../pyextra
 #pip3 install -e $DIR/acados/interfaces/acados_template
 

third_party/acados/include/acados/ocp_nlp/ocp_nlp_common.h

@@ -234,11 +234,7 @@ typedef struct ocp_nlp_out
 
     // NOTE: the inequalities are internally organized in the following order:
     // [ lbu lbx lg lh lphi ubu ubx ug uh uphi; lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi]
-
-    int sqp_iter;
-    int qp_iter;
     double inf_norm_res;
-    double total_time;
 
     void *raw_memory; // Pointer to allocated memory, to be used for freeing
 
@@ -274,11 +270,16 @@ typedef struct ocp_nlp_opts
     double levenberg_marquardt;  // LM factor to be added to the hessian before regularization
     int reuse_workspace;
     int num_threads;
+    int print_level;
 
     // TODO: move to separate struct?
     ocp_nlp_globalization_t globalization;
+    int full_step_dual;
+    int line_search_use_sufficient_descent;
+    int globalization_use_SOC;
     double alpha_min;
     double alpha_reduction;
+    double eps_sufficient_descent;
 } ocp_nlp_opts;
 
 //
@@ -316,6 +317,8 @@ typedef struct ocp_nlp_res
 acados_size_t ocp_nlp_res_calculate_size(ocp_nlp_dims *dims);
 //
 ocp_nlp_res *ocp_nlp_res_assign(ocp_nlp_dims *dims, void *raw_memory);
+//
+void ocp_nlp_res_get_inf_norm(ocp_nlp_res *res, double *out);
 
 /************************************************
  * memory
@@ -351,7 +354,7 @@ typedef struct ocp_nlp_memory
     bool *set_sim_guess; // indicate if there is new explicitly provided guess for integration variables
     struct blasfeo_dvec *sim_guess;
 
-	int *sqp_iter; // pointer to iteration number
+    int *sqp_iter; // pointer to iteration number
 
 } ocp_nlp_memory;
 
@@ -375,8 +378,12 @@ typedef struct ocp_nlp_workspace
     void **cost;         // cost_workspace
     void **constraints;  // constraints_workspace
 
-	ocp_nlp_out *tmp_nlp_out;
+    // for globalization: -> move to module?!
-	ocp_nlp_out *weight_merit_fun;
+    ocp_nlp_out *tmp_nlp_out;
+    ocp_nlp_out *weight_merit_fun;
+    struct blasfeo_dvec tmp_nxu;
+    struct blasfeo_dvec tmp_ni;
+    struct blasfeo_dvec dxnext_dy;
 
 } ocp_nlp_workspace;
 
@@ -392,6 +399,8 @@ ocp_nlp_workspace *ocp_nlp_workspace_assign(ocp_nlp_config *config, ocp_nlp_dims
  * function
  ************************************************/
 
+void ocp_nlp_alias_memory_to_submodules(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
+            ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
 //
 void ocp_nlp_initialize_qp(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
             ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
@@ -409,7 +418,8 @@ void ocp_nlp_update_variables_sqp(ocp_nlp_config *config, ocp_nlp_dims *dims, oc
            ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work, double alpha);
 //
 double ocp_nlp_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
-            ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
+            ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work,
+            int check_early_termination);
 //
 double ocp_nlp_evaluate_merit_fun(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
           ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);

third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_common.h

@@ -84,6 +84,7 @@ typedef struct
     /* memory */
     acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts);
     void *(*memory_assign)(void *config, void *dims, void *opts, void *raw_memory);
+    // get shooting node gap x_next(x_n, u_n) - x_{n+1}
     struct blasfeo_dvec *(*memory_get_fun_ptr)(void *memory_);
     struct blasfeo_dvec *(*memory_get_adj_ptr)(void *memory_);
     void (*memory_set_ux_ptr)(struct blasfeo_dvec *ux, void *memory_);

third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_noreg.h

@@ -65,11 +65,6 @@ extern "C" {
  * options
  ************************************************/
 
-typedef struct
-{
-    int dummy;
-} ocp_nlp_reg_noreg_opts;
-
 //
 acados_size_t ocp_nlp_reg_noreg_opts_calculate_size(void);
 //
@@ -84,12 +79,6 @@ void ocp_nlp_reg_noreg_opts_set(void *config_, ocp_nlp_reg_dims *dims, void *opt
 /************************************************
  * memory
  ************************************************/
-
-typedef struct
-{
-    int dummy;
-} ocp_nlp_reg_noreg_memory;
-
 //
 acados_size_t ocp_nlp_reg_noreg_memory_calculate_size(void *config, ocp_nlp_reg_dims *dims, void *opts);
 //

third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp.h

@@ -68,7 +68,6 @@ typedef struct
     int qp_warm_start;   // qp_warm_start in all but the first sqp iterations
     bool warm_start_first_qp; // to set qp_warm_start in first iteration
     int rti_phase;       // only phase 0 at the moment 
-    int print_level;     // verbosity
     int initialize_t_slacks;  // 0-false or 1-true
 
 } ocp_nlp_sqp_opts;
@@ -107,6 +106,7 @@ typedef struct
     double time_sim;
     double time_sim_la;
     double time_sim_ad;
+    double time_solution_sensitivities;
 
     // statistics
     double *stat;

third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp_rti.h

@@ -64,7 +64,6 @@ typedef struct
     int qp_warm_start;        // NOTE: this is not actually setting the warm_start! Just for compatibility with sqp.
     bool warm_start_first_qp; // to set qp_warm_start in first iteration
     int rti_phase;            // phase of RTI. Possible values 1 (preparation), 2 (feedback) 0 (both)
-    int print_level;     // verbosity
 
 } ocp_nlp_sqp_rti_opts;
 
@@ -100,6 +99,7 @@ typedef struct
     double time_reg;
     double time_tot;
     double time_glob;
+    double time_solution_sensitivities;
 
     // statistics
     double *stat;

third_party/acados/include/acados/utils/print.h

@@ -69,28 +69,32 @@ void ocp_nlp_out_print(ocp_nlp_dims *dims, ocp_nlp_out *nlp_out);
 void ocp_nlp_res_print(ocp_nlp_dims *dims, ocp_nlp_res *nlp_res);
 
 // ocp qp
+// TODO: move printing routines below that print qp structures to HPIPM!
 void print_ocp_qp_dims(ocp_qp_dims *dims);
 
 // void print_dense_qp_dims(dense_qp_dims *dims);
 
 void print_ocp_qp_in(ocp_qp_in *qp_in);
 
+void print_ocp_qp_in_to_file(FILE *file, ocp_qp_in *qp_in);
+
 void print_ocp_qp_out(ocp_qp_out *qp_out);
 
+void print_ocp_qp_out_to_file(FILE *file, ocp_qp_out *qp_out);
+
+void print_ocp_qp_res(ocp_qp_res *qp_res);
+
+void print_dense_qp_in(dense_qp_in *qp_in);
 // void print_ocp_qp_in_to_string(char string_out[], ocp_qp_in *qp_in);
 
 // void print_ocp_qp_out_to_string(char string_out[], ocp_qp_out *qp_out);
 
-void print_ocp_qp_res(ocp_qp_res *qp_res);
-
 // void print_colmaj_ocp_qp_in(colmaj_ocp_qp_in *qp);
 
 // void print_colmaj_ocp_qp_in_to_file(colmaj_ocp_qp_in *qp);
 
 // void print_colmaj_ocp_qp_out(char *filename, colmaj_ocp_qp_in *qp, colmaj_ocp_qp_out *out);
 
-void print_dense_qp_in(dense_qp_in *qp_in);
-
 void print_qp_info(qp_info *info);
 
 // void acados_warning(char warning_string[]);

third_party/acados/include/acados_c/ocp_nlp_interface.h

@@ -107,13 +107,13 @@ typedef enum
 
 
 /// Structure to store the configuration of a non-linear program
-typedef struct ocp_nlp_plan
+typedef struct ocp_nlp_plan_t
 {
     /// QP solver configuration.
-    ocp_qp_solver_plan ocp_qp_solver_plan;
+    ocp_qp_solver_plan_t ocp_qp_solver_plan;
 
     /// Simulation solver configuration for each stage.
-    sim_solver_plan *sim_solver_plan;
+    sim_solver_plan_t *sim_solver_plan;
 
     /// Nlp solver type.
     ocp_nlp_solver_t nlp_solver;
@@ -133,7 +133,7 @@ typedef struct ocp_nlp_plan
     /// Horizon length.
     int N;
 
-} ocp_nlp_plan;
+} ocp_nlp_plan_t;
 
 
 /// Structure to store the state/configuration for the non-linear programming solver
@@ -151,7 +151,7 @@ typedef struct ocp_nlp_solver
 /// default/invalid state.
 ///
 /// \param N Horizon length
-ocp_nlp_plan *ocp_nlp_plan_create(int N);
+ocp_nlp_plan_t *ocp_nlp_plan_create(int N);
 
 /// Destructor for plan struct, frees memory.
 ///
@@ -162,7 +162,7 @@ void ocp_nlp_plan_destroy(void* plan_);
 /// Constructs an nlp configuration struct from a plan.
 ///
 /// \param plan The plan (user nlp configuration).
-ocp_nlp_config *ocp_nlp_config_create(ocp_nlp_plan plan);
+ocp_nlp_config *ocp_nlp_config_create(ocp_nlp_plan_t plan);
 
 /// Desctructor of the nlp configuration.
 ///
@@ -372,6 +372,14 @@ void ocp_nlp_eval_cost(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *
 //
 void ocp_nlp_eval_residuals(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out);
 
+/// Computes the residuals.
+///
+/// \param solver The solver struct.
+/// \param nlp_in The inputs struct.
+/// \param nlp_out The output struct.
+void ocp_nlp_eval_residuals(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out);
+
+
 //
 void ocp_nlp_eval_param_sens(ocp_nlp_solver *solver, char *field, int stage, int index, ocp_nlp_out *sens_nlp_out);
 

third_party/acados/include/acados_c/ocp_qp_interface.h

@@ -105,7 +105,7 @@ typedef enum {
 typedef struct
 {
     ocp_qp_solver_t qp_solver;
-} ocp_qp_solver_plan;
+} ocp_qp_solver_plan_t;
 
 
 /// Linear ocp configuration.
@@ -127,7 +127,7 @@ void ocp_qp_xcond_solver_config_initialize_from_plan(
 /// Constructs a qp solver config and Initializes with default values.
 ///
 /// \param plan The qp solver plan struct.
-ocp_qp_xcond_solver_config *ocp_qp_xcond_solver_config_create(ocp_qp_solver_plan plan);
+ocp_qp_xcond_solver_config *ocp_qp_xcond_solver_config_create(ocp_qp_solver_plan_t plan);
 
 /// Destructor for config struct, frees memory.
 ///

third_party/acados/include/acados_c/sim_interface.h

@@ -57,7 +57,7 @@ typedef enum
 typedef struct
 {
     sim_solver_t sim_solver;
-} sim_solver_plan;
+} sim_solver_plan_t;
 
 
 
@@ -74,7 +74,7 @@ typedef struct
 
 /* config */
 //
-sim_config *sim_config_create(sim_solver_plan plan);
+sim_config *sim_config_create(sim_solver_plan_t plan);
 //
 void sim_config_destroy(void *config);
 

third_party/acados/include/blasfeo/include/blasfeo_d_aux.h

@@ -185,9 +185,14 @@ void blasfeo_dveccp(int m, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec
 void blasfeo_dvecsc(int m, double alpha, struct blasfeo_dvec *sx, int xi);
 // y <= alpha*x
 void blasfeo_dveccpsc(int m, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi);
+// z[idx] += alpha * x
 void blasfeo_dvecad_sp(int m, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dvec *sz, int zi);
+// z[idx] <= alpha * x
 void blasfeo_dvecin_sp(int m, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dvec *sz, int zi);
-void blasfeo_dvecex_sp(int m, double alpha, int *idx, struct blasfeo_dvec *sx, int x, struct blasfeo_dvec *sz, int zi);
+// z <= alpha * x[idx]
+void blasfeo_dvecex_sp(int m, double alpha, int *idx, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
+// z += alpha * x[idx]
+void blasfeo_dvecexad_sp(int m, double alpha, int *idx, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
 
 void blasfeo_dveccl(int m,
 	struct blasfeo_dvec *sxm, int xim, struct blasfeo_dvec *sx, int xi,

third_party/acados/include/blasfeo/include/blasfeo_d_aux_ref.h

@@ -182,6 +182,8 @@ void blasfeo_ref_dveccpsc(int m, double alpha, struct blasfeo_dvec *sx, int xi,
 void blasfeo_ref_dvecad_sp(int m, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dvec *sz, int zi);
 void blasfeo_ref_dvecin_sp(int m, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dvec *sz, int zi);
 void blasfeo_ref_dvecex_sp(int m, double alpha, int *idx, struct blasfeo_dvec *sx, int x, struct blasfeo_dvec *sz, int zi);
+// z += alpha * x[idx]
+void blasfeo_ref_dvecexad_sp(int m, double alpha, int *idx, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
 
 void blasfeo_ref_dveccl(int m,
 	struct blasfeo_dvec *sxm, int xim, struct blasfeo_dvec *sx, int xi,

third_party/acados/include/blasfeo/include/blasfeo_s_aux.h

@@ -127,6 +127,8 @@ float blasfeo_svecex1(struct blasfeo_svec *sx, int xi);
 void blasfeo_svecad_sp(int m, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_svec *sz, int zi);
 void blasfeo_svecin_sp(int m, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_svec *sz, int zi);
 void blasfeo_svecex_sp(int m, float alpha, int *idx, struct blasfeo_svec *sx, int x, struct blasfeo_svec *sz, int zi);
+// z += alpha * x[idx]
+void blasfeo_svecexad_sp(int m, double alpha, int *idx, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
 void blasfeo_sveccl(int m, struct blasfeo_svec *sxm, int xim, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sxp, int xip, struct blasfeo_svec *sz, int zi);
 void blasfeo_sveccl_mask(int m, struct blasfeo_svec *sxm, int xim, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sxp, int xip, struct blasfeo_svec *sz, int zi, struct blasfeo_svec *sm, int mi);
 void blasfeo_svecze(int m, struct blasfeo_svec *sm, int mi, struct blasfeo_svec *sv, int vi, struct blasfeo_svec *se, int ei);

third_party/acados/include/blasfeo/include/blasfeo_s_aux_ref.h

@@ -128,6 +128,8 @@ float blasfeo_ref_svecex1(struct blasfeo_svec *sx, int xi);
 void blasfeo_ref_svecad_sp(int m, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_svec *sz, int zi);
 void blasfeo_ref_svecin_sp(int m, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_svec *sz, int zi);
 void blasfeo_ref_svecex_sp(int m, float alpha, int *idx, struct blasfeo_svec *sx, int x, struct blasfeo_svec *sz, int zi);
+// z += alpha * x[idx]
+void blasfeo_ref_svecexad_sp(int m, double alpha, int *idx, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
 void blasfeo_ref_sveccl(int m, struct blasfeo_svec *sxm, int xim, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sxp, int xip, struct blasfeo_svec *sz, int zi);
 void blasfeo_ref_sveccl_mask(int m, struct blasfeo_svec *sxm, int xim, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sxp, int xip, struct blasfeo_svec *sz, int zi, struct blasfeo_svec *sm, int mi);
 void blasfeo_ref_svecze(int m, struct blasfeo_svec *sm, int mi, struct blasfeo_svec *sv, int vi, struct blasfeo_svec *se, int ei);

third_party/acados/larch64/lib/libacados.so

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:9303c7ff72999cc1e799faf24b715112326eefefb842240d4da19beb8caba1d7
+oid sha256:08d73672735c9bf05babda079627e3b93c593954a8b164f529c422cf9867d791
-size 484904
+size 489256

third_party/acados/larch64/lib/libblasfeo.so

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:654deb9f5c9cca7c6c049f8cffc9eae40052d7971489690929b5efded4b9d9da
+oid sha256:1e0cbc45c14340834991e617cd28045b8cac34bd094c73f3c4a0b3506ce3cdbe
-size 730608
+size 726592

third_party/acados/x86_64/lib/libacados.so

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:304b272189540640455e2003b6b124ce134edbc537d9f7e266939a6163e3965c
+oid sha256:5b84ad639a00b9ad35e502588a96145b9914ef0fc24262b3e9bfcdbcd655d9ae
-size 521320
+size 534128

third_party/acados/x86_64/lib/libblasfeo.so

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:7eed704b395c5ab6eeff9e4188ee4f883889c666adcea7743cf2bdcef535f533
+oid sha256:2361e3efae8404b728e631bcaaa1f686158e0b62e19b6237d01c2efa17147c21
-size 1265064
+size 1265168

third_party/acados/x86_64/lib/libhpipm.so

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:58abc1d388507fbab4c92483b58a4a2dd985877cd0b568c3135436cadc8f616e
+oid sha256:559bb84f78448803ef2b455cfb53aff95e283eb452c5e132afc5e663510802a0
 size 1572648

third_party/acados/x86_64/lib/libqpOASES_e.so.3.1

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:f8b684fe7a70461124d492e99ffe8e7858c9d6708ffeefec121653ac3aaa945c
+oid sha256:6452b56b86528616695f44752d975d26c93a0692246a8ca313fc969effec75ca
 size 262824

third_party/acados/x86_64/t_renderer

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:2545ad0a8dc9c92e60ac21d3209d62b94fdd3d91c5634ed81189844059b1d4df
+oid sha256:703e96a35b87eeace4b4558266a4d715742e28f17be0c02a6fcc3ea975d050f6
-size 7377848
+size 15186136