Use acados 0.3.6 (#16)

* bump acados_vendor_ros2 version * update README.md * regenerate test acados C-code CMD: python3 external/acados_solver_ros2/acados_solver_plugins_example/script/demo_export_plugin_to_custom_pkg.py Then copy mock_acados_solver code from acados_solver_plugins/src * fix namespace tests * Update CHANGELOG * remove ament_copyright from pre-commit * Update README.md * fix EOL lint error --------- Co-authored-by: Thibault Poignonec <[email protected]>
ICube-Robotics · Jan 19, 2025 · d5e50f4 · d5e50f4
1 parent 4034ff4
commit d5e50f4
Show file tree

Hide file tree

Showing 32 changed files with 385 additions and 440 deletions.
diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
@@ -113,14 +113,14 @@ repos:
         files: CMakeLists\.txt$
 
   # Copyright
-  - repo: local
-    hooks:
-      - id: ament_copyright
-        name: ament_copyright
-        description: Check if copyright notice is available in all files.
-        stages: [commit]
-        entry: ament_copyright
-        language: system
+  # - repo: local
+  #   hooks:
+  #     - id: ament_copyright
+  #       name: ament_copyright
+  #       description: Check if copyright notice is available in all files.
+  #       stages: [commit]
+  #       entry: ament_copyright
+  #       language: system
 
   # Docs - RestructuredText hooks
   - repo: https://github.com/PyCQA/doc8

diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -1,6 +1,15 @@
 
 # Change Log
 
+## [0.2.2] - 2025-01-19
+
+### Added
+
+### Changed
+
+- Acados vendor ros2 v0.3.2 -> v0.3.6
+- Update tests
+
 ## [0.2.1] - 2024-04-24
 
 ### Added

diff --git a/README.md b/README.md
@@ -34,6 +34,20 @@ colcon build && colcon build
 source install/setup.bash
 ```
 
+> [!NOTE]
+> If you are using ros jazzy or later, you might have to use a virtual environment or the `PIP_BREAK_SYSTEM_PACKAGES` flag.
+>
+> Otherwise, you might get the following error:
+> ```bash
+> ERROR: the following rosdeps failed to install
+>   pip:
+> rosdep installation of pip packages requires installing packages globally as root.
+> When using Python >= 3.11, PEP 668 compliance requires you to allow pip to install alongside
+> externally managed packages using the 'break-system-packages' option.
+> The recommended way to set this option when using rosdep is to set the environment variable
+> PIP_BREAK_SYSTEM_PACKAGES=1 in your environment.
+> ```
+
 ## Contacts ##
 ![icube](https://icube.unistra.fr/fileadmin/templates/DUN/icube/images/logo.png)
 

diff --git a/acados_solver_base/test/mock_acados_solver/acados_ocp_mock_acados_solver.json b/acados_solver_base/test/mock_acados_solver/acados_ocp_mock_acados_solver.json
@@ -1,7 +1,7 @@
 {
-    "acados_include_path": "/home/tpoignonec/dev/ros2_workspaces/ws_passive_nmpic_ros2/install/acados_vendor_ros2/opt/acados_vendor_ros2/include",
-    "acados_lib_path": "/home/tpoignonec/dev/ros2_workspaces/ws_passive_nmpic_ros2/install/acados_vendor_ros2/opt/acados_vendor_ros2/lib",
-    "code_export_directory": "/home/tpoignonec/dev/ros2_workspaces/ws_passive_nmpic_ros2/src/external/acados_solver_ros2/acados_solver_plugins/src/plugins/mock_acados_solver/generated_c_code",
+    "acados_include_path": "/home/tpo/dev/ros2_workspaces/ws_nmpic_icube/install/acados_vendor_ros2/opt/acados_vendor_ros2/include",
+    "acados_lib_path": "/home/tpo/dev/ros2_workspaces/ws_nmpic_icube/install/acados_vendor_ros2/opt/acados_vendor_ros2/lib",
+    "code_export_directory": "/home/tpo/dev/ros2_workspaces/ws_nmpic_icube/external/acados_solver_ros2/acados_solver_plugins_example/src/plugins/mock_acados_solver/generated_c_code",
     "constraints": {
         "C": [],
         "C_e": [],
@@ -131,6 +131,12 @@
         "cost_ext_fun_type": "casadi",
         "cost_ext_fun_type_0": "casadi",
         "cost_ext_fun_type_e": "casadi",
+        "cost_function_ext_cost": null,
+        "cost_function_ext_cost_0": null,
+        "cost_function_ext_cost_e": null,
+        "cost_source_ext_cost": null,
+        "cost_source_ext_cost_0": null,
+        "cost_source_ext_cost_e": null,
         "cost_type": "EXTERNAL",
         "cost_type_0": "EXTERNAL",
         "cost_type_e": "EXTERNAL",
@@ -145,8 +151,8 @@
         "zu_e": []
     },
     "cython_include_dirs": [
-        "/home/tpoignonec/dev/ros2_workspaces/ws_passive_nmpic_ros2/.venv/lib/python3.10/site-packages/numpy/core/include",
-        "/usr/include/python3.10"
+        "/usr/lib/python3/dist-packages/numpy/core/include",
+        "/usr/include/python3.12"
     ],
     "dims": {
         "N": 20,
@@ -188,7 +194,7 @@
         "ny_e": 0,
         "nz": 0
     },
-    "json_file": "/home/tpoignonec/dev/ros2_workspaces/ws_passive_nmpic_ros2/src/external/acados_solver_ros2/acados_solver_plugins/src/plugins/mock_acados_solver/acados_ocp_mock_acados_solver.json",
+    "json_file": "/home/tpo/dev/ros2_workspaces/ws_nmpic_icube/external/acados_solver_ros2/acados_solver_plugins_example/src/plugins/mock_acados_solver/acados_ocp_mock_acados_solver.json",
     "model": {
         "con_h_expr": null,
         "con_h_expr_0": null,
@@ -226,6 +232,9 @@
         "dyn_disc_fun_jac_hess": null,
         "dyn_ext_fun_type": "casadi",
         "dyn_generic_source": null,
+        "dyn_impl_dae_fun": null,
+        "dyn_impl_dae_fun_jac": null,
+        "dyn_impl_dae_jac": null,
         "f_expl_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegiaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaacaaaaaaaghbdegpcaaaaaaaaaaaaaagaaaaaaaegehigfgehbgegeaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaachpbfilobfilojdcaeegpcaaaaaaaaaaaaaabaaaaaaangegoaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaehigfgehbgegnaaaaaaaaaaaaaaachgaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaachkjjjjjjjjjjjjopdcheaaaaaaaaaaaaaaachjaaaaaaaaaaaaaaachcaaaaaaaaaaaaaaachcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaabaaaaaaageegcaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaabaaaaaaanecheaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaachhaaaaaaaaaaaaaaachhaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaachbbaaaaaaaaaaaaaachhaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaachlaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaachhaaaaaaaaaaaaaaachjaaaaaaaaaaaaaaachcaaaaaaaaaaaaaaachcaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaachdaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaachdbaaaaaaaaaaaaaacheaaaaaaaaaaaaaaachjaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaachlaaaaaaaaaaaaaaachhbaaaaaaaaaaaaaa",
         "f_impl_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegiaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaagaaaaaaaihbdpfegpgehegpcaaaaaaaaaaaaaacaaaaaaaghbdegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaajaaaaaaaehigfgehbgpfegpgehegpcaaaaaaaaaaaaaagaaaaaaaegehigfgehbgegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaagaaaaaaaghbdpfegpgehegeaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaachpbfilobfilojdcaeegpcaaaaaaaaaaaaaabaaaaaaangegoaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaehigfgehbgegnaaaaaaaaaaaaaaachlaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaachkjjjjjjjjjjjjopdchjaaaaaaaaaaaaaaachoaaaaaaaaaaaaaaachfaaaaaaaaaaaaaaachfaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaabaaaaaaageegcaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaabaaaaaaanechjaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaachjaaaaaaaaaaaaaaachmaaaaaaaaaaaaaaachmaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaegehigfgehbgpfegpgehegeaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaachgbaaaaaaaaaaaaaachmaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaachabaaaaaaaaaaaaaachjaaaaaaaaaaaaaaachmaaaaaaaaaaaaaaachoaaaaaaaaaaaaaaachfaaaaaaaaaaaaaaachfaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaachiaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaachibaaaaaaaaaaaaaachjaaaaaaaaaaaaaaachoaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaachabaaaaaaaaaaaaaachmbaaaaaaaaaaaaaa",
         "gnsf": {
@@ -236,6 +245,7 @@
         "nu_original": null,
         "p": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaeggaaaaaaaaaaaaaaacaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaacaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaabaaaaaaaneegpcaaaaaaaaaaaaaabaaaaaaang",
         "t": [],
+        "t0": null,
         "t_label": "t",
         "u": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegfaaaaaaaaaaaaaaabaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaabaaaaaaage",
         "u_labels": null,
@@ -254,6 +264,9 @@
     "simulink_opts": null,
     "solver_options": {
         "Tsim": 0.05,
+        "adaptive_levenberg_marquardt_lam": 5.0,
+        "adaptive_levenberg_marquardt_mu0": 0.001,
+        "adaptive_levenberg_marquardt_mu_min": 1e-16,
         "alpha_min": 0.05,
         "alpha_reduction": 0.7,
         "as_rti_iter": 1,
@@ -265,21 +278,28 @@
         "custom_update_filename": "",
         "custom_update_header_filename": "",
         "eps_sufficient_descent": 0.0001,
+        "eval_residual_at_max_iter": false,
         "exact_hess_constr": 1,
         "exact_hess_cost": 1,
         "exact_hess_dyn": 1,
         "ext_cost_num_hess": 0,
         "ext_fun_compile_flags": "-O2",
         "fixed_hess": 0,
         "full_step_dual": 0,
+        "funnel_fraction_switching_condition": 0.001,
+        "funnel_initial_penalty_parameter": 1.0,
+        "funnel_initialization_increase_factor": 15.0,
+        "funnel_initialization_upper_bound": 1.0,
+        "funnel_kappa": 0.9,
+        "funnel_sufficient_decrease_factor": 0.9,
         "globalization": "FIXED_STEP",
         "globalization_use_SOC": 0,
         "hessian_approx": "EXACT",
         "hpipm_mode": "BALANCE",
-        "initialize_t_slacks": 0,
         "integrator_type": "IRK",
         "levenberg_marquardt": 0.0,
         "line_search_use_sufficient_descent": 0,
+        "log_primal_step_norm": false,
         "model_external_shared_lib_dir": null,
         "model_external_shared_lib_name": null,
         "nlp_solver_ext_qp_res": 0,
@@ -288,11 +308,15 @@
         "nlp_solver_tol_comp": 1e-06,
         "nlp_solver_tol_eq": 1e-06,
         "nlp_solver_tol_ineq": 1e-06,
+        "nlp_solver_tol_min_step_norm": 0.0,
         "nlp_solver_tol_stat": 1e-06,
         "nlp_solver_type": "SQP",
+        "nlp_solver_warm_start_first_qp": false,
+        "num_threads_in_batch_solve": 1,
         "print_level": 0,
         "qp_solver": "PARTIAL_CONDENSING_HPIPM",
         "qp_solver_cond_N": 20,
+        "qp_solver_cond_block_size": null,
         "qp_solver_cond_ric_alg": 1,
         "qp_solver_iter_max": 50,
         "qp_solver_ric_alg": 1,
@@ -418,6 +442,7 @@
             0.05,
             0.05
         ],
+        "with_adaptive_levenberg_marquardt": false,
         "with_solution_sens_wrt_params": false,
         "with_value_sens_wrt_params": false
     },

diff --git a/acados_solver_base/test/mock_acados_solver/export_plugin.xml b/acados_solver_base/test/mock_acados_solver/export_plugin.xml
@@ -0,0 +1,9 @@
+<library path="acados_solver_plugins_example">
+  <class name="acados_solver_plugins_example/MockAcadosSolver"
+         type="acados_solver_plugins_example::MockAcadosSolver"
+         base_class_type="acados::AcadosSolver">
+    <description>
+      This is an example of Acados solver plugin. The underlying model is a pendulum on a cart.
+    </description>
+  </class>
+</library>
diff --git a/...lver_base/test/mock_acados_solver/generated_c_code/acados_sim_solver_mock_acados_solver.c b/...lver_base/test/mock_acados_solver/generated_c_code/acados_sim_solver_mock_acados_solver.c
@@ -138,7 +138,7 @@ int mock_acados_solver_acados_sim_create(mock_acados_solver_sim_solver_capsule *
     capsule->acados_sim_opts = mock_acados_solver_sim_opts;
     int tmp_int = 3;
     sim_opts_set(mock_acados_solver_sim_config, mock_acados_solver_sim_opts, "newton_iter", &tmp_int);
-    double tmp_double = 0;
+    double tmp_double = 0.0;
     sim_opts_set(mock_acados_solver_sim_config, mock_acados_solver_sim_opts, "newton_tol", &tmp_double);
     sim_collocation_type collocation_type = GAUSS_LEGENDRE;
     sim_opts_set(mock_acados_solver_sim_config, mock_acados_solver_sim_opts, "collocation_type", &collocation_type);
@@ -180,7 +180,7 @@ int mock_acados_solver_acados_sim_create(mock_acados_solver_sim_solver_capsule *
     /* initialize parameter values */
     double* p = calloc(np, sizeof(double));
 
-    p[0] = 1;
+    p[0] = 1.0;
     p[1] = 0.1;
 
     mock_acados_solver_acados_sim_update_params(capsule, p, np);
@@ -234,6 +234,19 @@ int mock_acados_solver_acados_sim_solve(mock_acados_solver_sim_solver_capsule *c
 }
 
 
+void mock_acados_solver_acados_sim_batch_solve(mock_acados_solver_sim_solver_capsule ** capsules, int N_batch)
+{
+
+    for (int i = 0; i < N_batch; i++)
+    {
+        sim_solve(capsules[i]->acados_sim_solver, capsules[i]->acados_sim_in, capsules[i]->acados_sim_out);
+    }
+
+
+    return;
+}
+
+
 int mock_acados_solver_acados_sim_free(mock_acados_solver_sim_solver_capsule *capsule)
 {
     // free memory

diff --git a/...lver_base/test/mock_acados_solver/generated_c_code/acados_sim_solver_mock_acados_solver.h b/...lver_base/test/mock_acados_solver/generated_c_code/acados_sim_solver_mock_acados_solver.h
@@ -80,6 +80,7 @@ typedef struct mock_acados_solver_sim_solver_capsule
 
 ACADOS_SYMBOL_EXPORT int mock_acados_solver_acados_sim_create(mock_acados_solver_sim_solver_capsule *capsule);
 ACADOS_SYMBOL_EXPORT int mock_acados_solver_acados_sim_solve(mock_acados_solver_sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT void mock_acados_solver_acados_sim_batch_solve(mock_acados_solver_sim_solver_capsule **capsules, int N_batch);
 ACADOS_SYMBOL_EXPORT int mock_acados_solver_acados_sim_free(mock_acados_solver_sim_solver_capsule *capsule);
 ACADOS_SYMBOL_EXPORT int mock_acados_solver_acados_sim_update_params(mock_acados_solver_sim_solver_capsule *capsule, double *value, int np);