From b3e47c691b29022c2b6fd0927fd6c655481f00d3 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Sat, 31 Dec 2022 21:00:50 -0800 Subject: [PATCH] move all third party stuff into third_party/ (#26853) * mv fastcluster * move msm_kgsl.h * camerad include * update path * mv pyextra * fix tici build * add acados_template to release build Co-authored-by: Comma Device old-commit-hash: cd8e03d53ed210aec46a2ff728cb4a830314a21a --- .pre-commit-config.yaml | 12 +- Dockerfile.openpilot | 1 - README.md | 1 - SConstruct | 5 +- docs/Makefile | 2 +- docs/docker/Dockerfile | 1 - launch_chffrplus.sh | 2 +- mypy.ini | 2 +- pyextra/.gitignore | 3 - pyextra/acados_template/.gitignore | 3 - pyextra/acados_template/__init__.py | 3 - pyextra/acados_template/acados_layout.json | 3 - pyextra/acados_template/acados_model.py | 3 - pyextra/acados_template/acados_ocp.py | 3 - pyextra/acados_template/acados_ocp_solver.py | 3 - .../acados_template/acados_ocp_solver_pyx.pyx | 3 - pyextra/acados_template/acados_sim.py | 3 - .../acados_template/acados_sim_layout.json | 3 - pyextra/acados_template/acados_sim_solver.py | 3 - .../acados_template/acados_solver_common.pxd | 3 - pyextra/acados_template/builders.py | 3 - .../c_templates_tera/CMakeLists.in.txt | 3 - .../c_templates_tera/CPPLINT.cfg | 3 - .../c_templates_tera/Makefile.in | 3 - .../c_templates_tera/acados_mex_create.in.c | 3 - .../c_templates_tera/acados_mex_free.in.c | 3 - .../c_templates_tera/acados_mex_set.in.c | 3 - .../c_templates_tera/acados_mex_solve.in.c | 3 - .../c_templates_tera/acados_sim_solver.in.c | 3 - .../c_templates_tera/acados_sim_solver.in.h | 3 - .../acados_sim_solver_sfun.in.c | 3 - .../c_templates_tera/acados_solver.in.c | 3 - .../c_templates_tera/acados_solver.in.h | 3 - .../c_templates_tera/acados_solver.in.pxd | 3 - .../c_templates_tera/acados_solver_sfun.in.c | 3 - .../c_templates_tera/cost_y_0_fun.in.h | 3 - .../c_templates_tera/cost_y_e_fun.in.h | 3 - .../c_templates_tera/cost_y_fun.in.h | 3 - .../c_templates_tera/external_cost.in.h | 3 - .../c_templates_tera/external_cost_0.in.h | 3 - .../c_templates_tera/external_cost_e.in.h | 3 - .../c_templates_tera/h_constraint.in.h | 3 - .../c_templates_tera/h_e_constraint.in.h | 3 - .../c_templates_tera/main.in.c | 3 - .../c_templates_tera/main_mex.in.c | 3 - .../c_templates_tera/main_sim.in.c | 3 - .../c_templates_tera/make_main_mex.in.m | 3 - .../c_templates_tera/make_mex.in.m | 3 - .../c_templates_tera/make_sfun.in.m | 3 - .../c_templates_tera/make_sfun_sim.in.m | 3 - .../c_templates_tera/mex_solver.in.m | 3 - .../c_templates_tera/model.in.h | 3 - .../c_templates_tera/phi_constraint.in.h | 3 - .../c_templates_tera/phi_e_constraint.in.h | 3 - .../generate_c_code_constraint.py | 3 - .../generate_c_code_discrete_dynamics.py | 3 - .../generate_c_code_explicit_ode.py | 3 - .../generate_c_code_external_cost.py | 3 - .../acados_template/generate_c_code_gnsf.py | 3 - .../generate_c_code_implicit_ode.py | 3 - .../generate_c_code_nls_cost.py | 3 - .../simulink_default_opts.json | 3 - pyextra/acados_template/utils.py | 3 - release/files_common | 11 +- .../controls/lib/lateral_mpc_lib/SConscript | 6 +- .../controls/lib/lateral_mpc_lib/lat_mpc.py | 2 +- .../lib/longitudinal_mpc_lib/SConscript | 6 +- .../lib/longitudinal_mpc_lib/long_mpc.py | 2 +- selfdrive/controls/radard.py | 2 +- selfdrive/controls/tests/test_clustering.py | 4 +- selfdrive/manager/manager.py | 2 - selfdrive/modeld/thneed/include/msm_kgsl.h | 3 - selfdrive/modeld/thneed/thneed.h | 2 +- system/camerad/SConscript | 14 +- third_party/.gitignore | 1 + third_party/SConscript | 2 + third_party/acados/acados_template/.gitignore | 6 + .../acados/acados_template/__init__.py | 43 + .../acados/acados_template/acados_layout.json | 804 +++++ .../acados/acados_template/acados_model.py | 167 + .../acados/acados_template/acados_ocp.py | 2955 +++++++++++++++++ .../acados_template/acados_ocp_solver.py | 1818 ++++++++++ .../acados_template/acados_ocp_solver_pyx.pyx | 707 ++++ .../acados/acados_template/acados_sim.py | 331 ++ .../acados_template/acados_sim_layout.json | 47 + .../acados_template/acados_sim_solver.py | 454 +++ .../acados_template/acados_solver_common.pxd | 103 + .../acados/acados_template/builders.py | 116 + .../c_templates_tera/CMakeLists.in.txt | 374 +++ .../c_templates_tera/CPPLINT.cfg | 1 + .../c_templates_tera/Makefile.in | 406 +++ .../c_templates_tera/acados_mex_create.in.c | 387 +++ .../c_templates_tera/acados_mex_free.in.c | 70 + .../c_templates_tera/acados_mex_set.in.c | 570 ++++ .../c_templates_tera/acados_mex_solve.in.c | 59 + .../c_templates_tera/acados_sim_solver.in.c | 508 +++ .../c_templates_tera/acados_sim_solver.in.h | 103 + .../acados_sim_solver_sfun.in.c | 233 ++ .../c_templates_tera/acados_solver.in.c | 2495 ++++++++++++++ .../c_templates_tera/acados_solver.in.h | 217 ++ .../c_templates_tera/acados_solver.in.pxd | 62 + .../c_templates_tera/acados_solver_sfun.in.c | 782 +++++ .../c_templates_tera/cost_y_0_fun.in.h | 69 + .../c_templates_tera/cost_y_e_fun.in.h | 69 + .../c_templates_tera/cost_y_fun.in.h | 69 + .../c_templates_tera/external_cost.in.h | 74 + .../c_templates_tera/external_cost_0.in.h | 75 + .../c_templates_tera/external_cost_e.in.h | 74 + .../c_templates_tera/h_constraint.in.h | 70 + .../c_templates_tera/h_e_constraint.in.h | 71 + .../c_templates_tera/main.in.c | 216 ++ .../c_templates_tera/main_mex.in.c | 184 + .../c_templates_tera/main_sim.in.c | 135 + .../c_templates_tera/make_main_mex.in.m | 105 + .../c_templates_tera/make_mex.in.m | 110 + .../c_templates_tera/make_sfun.in.m | 344 ++ .../c_templates_tera/make_sfun_sim.in.m | 103 + .../c_templates_tera/mex_solver.in.m | 166 + .../c_templates_tera/model.in.h | 211 ++ .../c_templates_tera/phi_constraint.in.h | 55 + .../c_templates_tera/phi_e_constraint.in.h | 21 + .../generate_c_code_constraint.py | 180 + .../generate_c_code_discrete_dynamics.py | 98 + .../generate_c_code_explicit_ode.py | 124 + .../generate_c_code_external_cost.py | 116 + .../acados_template/generate_c_code_gnsf.py | 131 + .../generate_c_code_implicit_ode.py | 139 + .../generate_c_code_nls_cost.py | 113 + .../simulink_default_opts.json | 40 + third_party/acados/acados_template/utils.py | 440 +++ third_party/acados/build.sh | 5 +- .../lib => third_party}/cluster/.gitignore | 0 .../lib => third_party}/cluster/LICENSE | 0 .../lib => third_party}/cluster/README | 0 .../lib => third_party}/cluster/SConscript | 0 .../lib => third_party}/cluster/__init__.py | 0 .../cluster/fastcluster.cpp | 0 .../lib => third_party}/cluster/fastcluster.h | 0 .../cluster/fastcluster_R_dm.cpp | 0 .../cluster/fastcluster_dm.cpp | 0 .../cluster/fastcluster_py.py | 0 .../lib => third_party}/cluster/test.cpp | 0 .../linux}/include/media/cam_cpas.h | 0 .../linux}/include/media/cam_defs.h | 0 .../linux}/include/media/cam_fd.h | 0 .../linux}/include/media/cam_icp.h | 0 .../linux}/include/media/cam_isp.h | 0 .../linux}/include/media/cam_isp_ife.h | 0 .../linux}/include/media/cam_isp_vfe.h | 0 .../linux}/include/media/cam_jpeg.h | 0 .../linux}/include/media/cam_lrme.h | 0 .../linux}/include/media/cam_req_mgr.h | 0 .../linux}/include/media/cam_sensor.h | 0 .../include/media/cam_sensor_cmn_header.h | 0 .../linux}/include/media/cam_sync.h | 0 .../linux}/include/msm_cam_sensor.h | 0 .../linux}/include/msm_camsensor_sdk.h | 0 third_party/linux/include/msm_kgsl.h | 1449 ++++++++ .../linux}/include/msmb_camera.h | 0 .../linux}/include/msmb_isp.h | 0 .../linux}/include/msmb_ispif.h | 0 tools/gpstest/fuzzy_testing.py | 2 +- tools/sim/Dockerfile.sim | 1 - 163 files changed, 18907 insertions(+), 218 deletions(-) delete mode 100644 pyextra/.gitignore delete mode 100644 pyextra/acados_template/.gitignore delete mode 100644 pyextra/acados_template/__init__.py delete mode 100644 pyextra/acados_template/acados_layout.json delete mode 100644 pyextra/acados_template/acados_model.py delete mode 100644 pyextra/acados_template/acados_ocp.py delete mode 100644 pyextra/acados_template/acados_ocp_solver.py delete mode 100644 pyextra/acados_template/acados_ocp_solver_pyx.pyx delete mode 100644 pyextra/acados_template/acados_sim.py delete mode 100644 pyextra/acados_template/acados_sim_layout.json delete mode 100644 pyextra/acados_template/acados_sim_solver.py delete mode 100644 pyextra/acados_template/acados_solver_common.pxd delete mode 100644 pyextra/acados_template/builders.py delete mode 100644 pyextra/acados_template/c_templates_tera/CMakeLists.in.txt delete mode 100644 pyextra/acados_template/c_templates_tera/CPPLINT.cfg delete mode 100644 pyextra/acados_template/c_templates_tera/Makefile.in delete mode 100644 pyextra/acados_template/c_templates_tera/acados_mex_create.in.c delete mode 100644 pyextra/acados_template/c_templates_tera/acados_mex_free.in.c delete mode 100644 pyextra/acados_template/c_templates_tera/acados_mex_set.in.c delete mode 100644 pyextra/acados_template/c_templates_tera/acados_mex_solve.in.c delete mode 100644 pyextra/acados_template/c_templates_tera/acados_sim_solver.in.c delete mode 100644 pyextra/acados_template/c_templates_tera/acados_sim_solver.in.h delete mode 100644 pyextra/acados_template/c_templates_tera/acados_sim_solver_sfun.in.c delete mode 100644 pyextra/acados_template/c_templates_tera/acados_solver.in.c delete mode 100644 pyextra/acados_template/c_templates_tera/acados_solver.in.h delete mode 100644 pyextra/acados_template/c_templates_tera/acados_solver.in.pxd delete mode 100644 pyextra/acados_template/c_templates_tera/acados_solver_sfun.in.c delete mode 100644 pyextra/acados_template/c_templates_tera/cost_y_0_fun.in.h delete mode 100644 pyextra/acados_template/c_templates_tera/cost_y_e_fun.in.h delete mode 100644 pyextra/acados_template/c_templates_tera/cost_y_fun.in.h delete mode 100644 pyextra/acados_template/c_templates_tera/external_cost.in.h delete mode 100644 pyextra/acados_template/c_templates_tera/external_cost_0.in.h delete mode 100644 pyextra/acados_template/c_templates_tera/external_cost_e.in.h delete mode 100644 pyextra/acados_template/c_templates_tera/h_constraint.in.h delete mode 100644 pyextra/acados_template/c_templates_tera/h_e_constraint.in.h delete mode 100644 pyextra/acados_template/c_templates_tera/main.in.c delete mode 100644 pyextra/acados_template/c_templates_tera/main_mex.in.c delete mode 100644 pyextra/acados_template/c_templates_tera/main_sim.in.c delete mode 100644 pyextra/acados_template/c_templates_tera/make_main_mex.in.m delete mode 100644 pyextra/acados_template/c_templates_tera/make_mex.in.m delete mode 100644 pyextra/acados_template/c_templates_tera/make_sfun.in.m delete mode 100644 pyextra/acados_template/c_templates_tera/make_sfun_sim.in.m delete mode 100644 pyextra/acados_template/c_templates_tera/mex_solver.in.m delete mode 100644 pyextra/acados_template/c_templates_tera/model.in.h delete mode 100644 pyextra/acados_template/c_templates_tera/phi_constraint.in.h delete mode 100644 pyextra/acados_template/c_templates_tera/phi_e_constraint.in.h delete mode 100644 pyextra/acados_template/generate_c_code_constraint.py delete mode 100644 pyextra/acados_template/generate_c_code_discrete_dynamics.py delete mode 100644 pyextra/acados_template/generate_c_code_explicit_ode.py delete mode 100644 pyextra/acados_template/generate_c_code_external_cost.py delete mode 100644 pyextra/acados_template/generate_c_code_gnsf.py delete mode 100644 pyextra/acados_template/generate_c_code_implicit_ode.py delete mode 100644 pyextra/acados_template/generate_c_code_nls_cost.py delete mode 100644 pyextra/acados_template/simulink_default_opts.json delete mode 100644 pyextra/acados_template/utils.py delete mode 100644 selfdrive/modeld/thneed/include/msm_kgsl.h create mode 100644 third_party/.gitignore create mode 100644 third_party/acados/acados_template/.gitignore create mode 100644 third_party/acados/acados_template/__init__.py create mode 100644 third_party/acados/acados_template/acados_layout.json create mode 100644 third_party/acados/acados_template/acados_model.py create mode 100644 third_party/acados/acados_template/acados_ocp.py create mode 100644 third_party/acados/acados_template/acados_ocp_solver.py create mode 100644 third_party/acados/acados_template/acados_ocp_solver_pyx.pyx create mode 100644 third_party/acados/acados_template/acados_sim.py create mode 100644 third_party/acados/acados_template/acados_sim_layout.json create mode 100644 third_party/acados/acados_template/acados_sim_solver.py create mode 100644 third_party/acados/acados_template/acados_solver_common.pxd create mode 100644 third_party/acados/acados_template/builders.py create mode 100644 third_party/acados/acados_template/c_templates_tera/CMakeLists.in.txt create mode 100644 third_party/acados/acados_template/c_templates_tera/CPPLINT.cfg create mode 100644 third_party/acados/acados_template/c_templates_tera/Makefile.in create mode 100644 third_party/acados/acados_template/c_templates_tera/acados_mex_create.in.c create mode 100644 third_party/acados/acados_template/c_templates_tera/acados_mex_free.in.c create mode 100644 third_party/acados/acados_template/c_templates_tera/acados_mex_set.in.c create mode 100644 third_party/acados/acados_template/c_templates_tera/acados_mex_solve.in.c create mode 100644 third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.c create mode 100644 third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.h create mode 100644 third_party/acados/acados_template/c_templates_tera/acados_sim_solver_sfun.in.c create mode 100644 third_party/acados/acados_template/c_templates_tera/acados_solver.in.c create mode 100644 third_party/acados/acados_template/c_templates_tera/acados_solver.in.h create mode 100644 third_party/acados/acados_template/c_templates_tera/acados_solver.in.pxd create mode 100644 third_party/acados/acados_template/c_templates_tera/acados_solver_sfun.in.c create mode 100644 third_party/acados/acados_template/c_templates_tera/cost_y_0_fun.in.h create mode 100644 third_party/acados/acados_template/c_templates_tera/cost_y_e_fun.in.h create mode 100644 third_party/acados/acados_template/c_templates_tera/cost_y_fun.in.h create mode 100644 third_party/acados/acados_template/c_templates_tera/external_cost.in.h create mode 100644 third_party/acados/acados_template/c_templates_tera/external_cost_0.in.h create mode 100644 third_party/acados/acados_template/c_templates_tera/external_cost_e.in.h create mode 100644 third_party/acados/acados_template/c_templates_tera/h_constraint.in.h create mode 100644 third_party/acados/acados_template/c_templates_tera/h_e_constraint.in.h create mode 100644 third_party/acados/acados_template/c_templates_tera/main.in.c create mode 100644 third_party/acados/acados_template/c_templates_tera/main_mex.in.c create mode 100644 third_party/acados/acados_template/c_templates_tera/main_sim.in.c create mode 100644 third_party/acados/acados_template/c_templates_tera/make_main_mex.in.m create mode 100644 third_party/acados/acados_template/c_templates_tera/make_mex.in.m create mode 100644 third_party/acados/acados_template/c_templates_tera/make_sfun.in.m create mode 100644 third_party/acados/acados_template/c_templates_tera/make_sfun_sim.in.m create mode 100644 third_party/acados/acados_template/c_templates_tera/mex_solver.in.m create mode 100644 third_party/acados/acados_template/c_templates_tera/model.in.h create mode 100644 third_party/acados/acados_template/c_templates_tera/phi_constraint.in.h create mode 100644 third_party/acados/acados_template/c_templates_tera/phi_e_constraint.in.h create mode 100644 third_party/acados/acados_template/generate_c_code_constraint.py create mode 100644 third_party/acados/acados_template/generate_c_code_discrete_dynamics.py create mode 100644 third_party/acados/acados_template/generate_c_code_explicit_ode.py create mode 100644 third_party/acados/acados_template/generate_c_code_external_cost.py create mode 100644 third_party/acados/acados_template/generate_c_code_gnsf.py create mode 100644 third_party/acados/acados_template/generate_c_code_implicit_ode.py create mode 100644 third_party/acados/acados_template/generate_c_code_nls_cost.py create mode 100644 third_party/acados/acados_template/simulink_default_opts.json create mode 100644 third_party/acados/acados_template/utils.py rename {selfdrive/controls/lib => third_party}/cluster/.gitignore (100%) rename {selfdrive/controls/lib => third_party}/cluster/LICENSE (100%) rename {selfdrive/controls/lib => third_party}/cluster/README (100%) rename {selfdrive/controls/lib => third_party}/cluster/SConscript (100%) rename {selfdrive/controls/lib => third_party}/cluster/__init__.py (100%) rename {selfdrive/controls/lib => third_party}/cluster/fastcluster.cpp (100%) rename {selfdrive/controls/lib => third_party}/cluster/fastcluster.h (100%) rename {selfdrive/controls/lib => third_party}/cluster/fastcluster_R_dm.cpp (100%) rename {selfdrive/controls/lib => third_party}/cluster/fastcluster_dm.cpp (100%) rename {selfdrive/controls/lib => third_party}/cluster/fastcluster_py.py (100%) rename {selfdrive/controls/lib => third_party}/cluster/test.cpp (100%) rename {system/camerad => third_party/linux}/include/media/cam_cpas.h (100%) rename {system/camerad => third_party/linux}/include/media/cam_defs.h (100%) rename {system/camerad => third_party/linux}/include/media/cam_fd.h (100%) rename {system/camerad => third_party/linux}/include/media/cam_icp.h (100%) rename {system/camerad => third_party/linux}/include/media/cam_isp.h (100%) rename {system/camerad => third_party/linux}/include/media/cam_isp_ife.h (100%) rename {system/camerad => third_party/linux}/include/media/cam_isp_vfe.h (100%) rename {system/camerad => third_party/linux}/include/media/cam_jpeg.h (100%) rename {system/camerad => third_party/linux}/include/media/cam_lrme.h (100%) rename {system/camerad => third_party/linux}/include/media/cam_req_mgr.h (100%) rename {system/camerad => third_party/linux}/include/media/cam_sensor.h (100%) rename {system/camerad => third_party/linux}/include/media/cam_sensor_cmn_header.h (100%) rename {system/camerad => third_party/linux}/include/media/cam_sync.h (100%) rename {system/camerad => third_party/linux}/include/msm_cam_sensor.h (100%) rename {system/camerad => third_party/linux}/include/msm_camsensor_sdk.h (100%) create mode 100644 third_party/linux/include/msm_kgsl.h rename {system/camerad => third_party/linux}/include/msmb_camera.h (100%) rename {system/camerad => third_party/linux}/include/msmb_isp.h (100%) rename {system/camerad => third_party/linux}/include/msmb_ispif.h (100%) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 91b9c61628..edbca52fed 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -7,7 +7,7 @@ repos: rev: v4.1.0 hooks: - id: check-ast - exclude: '^(pyextra)/' + exclude: '^(third_party)/' - id: check-json - id: check-xml - id: check-yaml @@ -19,7 +19,7 @@ repos: rev: v2.2.1 hooks: - id: codespell - exclude: '^(pyextra/)|(third_party/)|(body/)|(cereal/)|(rednose/)|(panda/)|(laika/)|(opendbc/)|(laika_repo/)|(rednose_repo/)|(include/)|(selfdrive/ui/translations/.*.ts)|(selfdrive/controls/lib/cluster)' + exclude: '^(third_party/)|(body/)|(cereal/)|(rednose/)|(panda/)|(laika/)|(opendbc/)|(laika_repo/)|(rednose_repo/)|(selfdrive/ui/translations/.*.ts)' args: # if you've got a short variable name that's getting flagged, add it here - -L bu,ro,te,ue,alo,hda,ois,nam,nams,ned,som,parm,setts,inout,warmup @@ -31,12 +31,12 @@ repos: entry: mypy language: system types: [python] - exclude: '^(pyextra/)|(cereal/)|(opendbc/)|(panda/)|(laika/)|(laika_repo/)|(rednose/)|(rednose_repo/)|(tinygrad/)|(tinygrad_repo/)|(xx/)' + exclude: '^(third_party/)|(cereal/)|(opendbc/)|(panda/)|(laika/)|(laika_repo/)|(rednose/)|(rednose_repo/)|(tinygrad/)|(tinygrad_repo/)|(xx/)' - repo: https://github.com/PyCQA/flake8 rev: 4.0.1 hooks: - id: flake8 - exclude: '^(pyextra/)|(cereal/)|(rednose/)|(panda/)|(laika/)|(opendbc/)|(laika_repo/)|(rednose_repo/)|(tinygrad/)|(tinygrad_repo/)|(selfdrive/debug/)/' + exclude: '^(third_party/)|(cereal/)|(rednose/)|(panda/)|(laika/)|(opendbc/)|(laika_repo/)|(rednose_repo/)|(tinygrad/)|(tinygrad_repo/)|(selfdrive/debug/)/' additional_dependencies: ['flake8-no-implicit-concat'] args: - --indent-size=2 @@ -51,7 +51,7 @@ repos: entry: pylint language: system types: [python] - exclude: '^(pyextra/)|(cereal/)|(rednose/)|(panda/)|(laika/)|(laika_repo/)|(rednose_repo/)|(tinygrad/)|(tinygrad_repo/)' + exclude: '^(third_party/)|(cereal/)|(rednose/)|(panda/)|(laika/)|(laika_repo/)|(rednose_repo/)|(tinygrad/)|(tinygrad_repo/)' args: - -j0 - -rn @@ -64,7 +64,7 @@ repos: entry: cppcheck language: system types: [c++] - exclude: '^(third_party/)|(pyextra/)|(cereal/)|(body/)|(rednose/)|(rednose_repo/)|(opendbc/)|(panda/)|(tools/)|(selfdrive/modeld/thneed/debug/)|(selfdrive/modeld/test/)|(selfdrive/camerad/test/)|(installer/)' + exclude: '^(third_party/)|(cereal/)|(body/)|(rednose/)|(rednose_repo/)|(opendbc/)|(panda/)|(tools/)|(selfdrive/modeld/thneed/debug/)|(selfdrive/modeld/test/)|(selfdrive/camerad/test/)|(installer/)' args: - --error-exitcode=1 - --language=c++ diff --git a/Dockerfile.openpilot b/Dockerfile.openpilot index 102da78d7d..acc0fcc784 100644 --- a/Dockerfile.openpilot +++ b/Dockerfile.openpilot @@ -10,7 +10,6 @@ WORKDIR ${OPENPILOT_PATH} COPY SConstruct ${OPENPILOT_PATH} -COPY ./pyextra ${OPENPILOT_PATH}/pyextra COPY ./third_party ${OPENPILOT_PATH}/third_party COPY ./site_scons ${OPENPILOT_PATH}/site_scons COPY ./laika ${OPENPILOT_PATH}/laika diff --git a/README.md b/README.md index 88b7aeb2bb..4896d9ff72 100755 --- a/README.md +++ b/README.md @@ -103,7 +103,6 @@ Directory Structure ├── opendbc # Files showing how to interpret data from cars ├── panda # Code used to communicate on CAN ├── third_party # External libraries - ├── pyextra # Extra python packages └── system # Generic services ├── camerad # Driver to capture images from the camera sensors ├── clocksd # Broadcasts current time diff --git a/SConstruct b/SConstruct index 54b008004e..b148a9116a 100644 --- a/SConstruct +++ b/SConstruct @@ -71,10 +71,10 @@ if arch == "aarch64" and AGNOS: lenv = { "PATH": os.environ['PATH'], "LD_LIBRARY_PATH": [Dir(f"#third_party/acados/{arch}/lib").abspath], - "PYTHONPATH": Dir("#").abspath + ":" + Dir("#pyextra/").abspath, + "PYTHONPATH": Dir("#").abspath, "ACADOS_SOURCE_DIR": Dir("#third_party/acados/include/acados").abspath, - "ACADOS_PYTHON_INTERFACE_PATH": Dir("#pyextra/acados_template").abspath, + "ACADOS_PYTHON_INTERFACE_PATH": Dir("#third_party/acados/acados_template").abspath, "TERA_PATH": Dir("#").abspath + f"/third_party/acados/{arch}/t_renderer" } @@ -422,7 +422,6 @@ SConscript(['common/transformations/SConscript']) SConscript(['selfdrive/modeld/SConscript']) -SConscript(['selfdrive/controls/lib/cluster/SConscript']) SConscript(['selfdrive/controls/lib/lateral_mpc_lib/SConscript']) SConscript(['selfdrive/controls/lib/longitudinal_mpc_lib/SConscript']) diff --git a/docs/Makefile b/docs/Makefile index d0aa841c4d..dee660f770 100644 --- a/docs/Makefile +++ b/docs/Makefile @@ -41,7 +41,7 @@ clean: @echo "Building rst files..." sphinx-apidoc -o "$(DOCSBUILDDIR)" ../ \ - ../xx ../laika_repo ../rednose_repo ../pyextra ../notebooks ../panda_jungle \ + ../xx ../laika_repo ../rednose_repo ../notebooks ../panda_jungle \ ../third_party \ ../panda/examples \ ../scripts \ diff --git a/docs/docker/Dockerfile b/docs/docker/Dockerfile index 0d5883f566..a1cdbbeaf0 100644 --- a/docs/docker/Dockerfile +++ b/docs/docker/Dockerfile @@ -11,7 +11,6 @@ WORKDIR ${OPENPILOT_PATH} COPY SConstruct ${OPENPILOT_PATH} -COPY ./pyextra ${OPENPILOT_PATH}/pyextra COPY ./third_party ${OPENPILOT_PATH}/third_party COPY ./site_scons ${OPENPILOT_PATH}/site_scons COPY ./laika ${OPENPILOT_PATH}/laika diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh index 911774a4eb..9fe9b1bd15 100755 --- a/launch_chffrplus.sh +++ b/launch_chffrplus.sh @@ -74,7 +74,7 @@ function launch { # handle pythonpath ln -sfn $(pwd) /data/pythonpath - export PYTHONPATH="$PWD:$PWD/pyextra" + export PYTHONPATH="$PWD" # hardware specific init agnos_init diff --git a/mypy.ini b/mypy.ini index 39b1b007a7..9b6d00d2c4 100644 --- a/mypy.ini +++ b/mypy.ini @@ -2,7 +2,7 @@ python_version = 3.8 plugins = numpy.typing.mypy_plugin files = body, common, docs, scripts, selfdrive, site_scons, system, tools -exclude = ^(pyextra/)|(cereal/)|(opendbc/)|(panda/)|(laika/)|(laika_repo/)|(rednose/)|(rednose_repo/)|(tinygrad/)|(tinygrad_repo/)|(xx/) +exclude = ^(cereal/)|(opendbc/)|(panda/)|(laika/)|(laika_repo/)|(rednose/)|(rednose_repo/)|(tinygrad/)|(tinygrad_repo/)|(xx/) ; third-party packages ignore_missing_imports = True diff --git a/pyextra/.gitignore b/pyextra/.gitignore deleted file mode 100644 index 6835cb7b63..0000000000 --- a/pyextra/.gitignore +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:094c25031dcfe5c288666671bc2012866293b3797ea53c796af798bda4c32f01 -size 6 diff --git a/pyextra/acados_template/.gitignore b/pyextra/acados_template/.gitignore deleted file mode 100644 index c14a7dfe70..0000000000 --- a/pyextra/acados_template/.gitignore +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:477f73573a50b1ae2740849e1aed4f8d353ead59116f95b897e8f620e8dbf31b -size 62 diff --git a/pyextra/acados_template/__init__.py b/pyextra/acados_template/__init__.py deleted file mode 100644 index 05bfc99bf6..0000000000 --- a/pyextra/acados_template/__init__.py +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:dc3561a615e9baefc436c341280cac6634008a3b4a1773227604db4bf15b00d8 -size 1971 diff --git a/pyextra/acados_template/acados_layout.json b/pyextra/acados_template/acados_layout.json deleted file mode 100644 index dfeae71d28..0000000000 --- a/pyextra/acados_template/acados_layout.json +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:c116b7b6dae6a24ed2e3f55604edce722d6b86c9c25451e3ebe96c278f700cc9 -size 14258 diff --git a/pyextra/acados_template/acados_model.py b/pyextra/acados_template/acados_model.py deleted file mode 100644 index 2dcf15d088..0000000000 --- a/pyextra/acados_template/acados_model.py +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:af19df99d9669672bfdc72d959bd444919a16c800f764873c923db425c26e70d -size 8289 diff --git a/pyextra/acados_template/acados_ocp.py b/pyextra/acados_template/acados_ocp.py deleted file mode 100644 index 96db25b00c..0000000000 --- a/pyextra/acados_template/acados_ocp.py +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:00a5921254ea36f5cb0c80838bbc18bd7544990f4366979152c9a81cd0f302b8 -size 101018 diff --git a/pyextra/acados_template/acados_ocp_solver.py b/pyextra/acados_template/acados_ocp_solver.py deleted file mode 100644 index 5dba555791..0000000000 --- a/pyextra/acados_template/acados_ocp_solver.py +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:88265ec648408897af39ffe030823b009dde3cf56e588d3e717c60898432461c -size 84016 diff --git a/pyextra/acados_template/acados_ocp_solver_pyx.pyx b/pyextra/acados_template/acados_ocp_solver_pyx.pyx deleted file mode 100644 index a491778bf8..0000000000 --- a/pyextra/acados_template/acados_ocp_solver_pyx.pyx +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:74e00646a8d51f0f4e8d0aa3d0e31160949f9a69df05b3d6b200ec3d60c560fb -size 29596 diff --git a/pyextra/acados_template/acados_sim.py b/pyextra/acados_template/acados_sim.py deleted file mode 100644 index db81efb969..0000000000 --- a/pyextra/acados_template/acados_sim.py +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:ded40649250d2bb047cadb87584e5f2bbbd1c8b88276ca8d7909acd11454cfa9 -size 12029 diff --git a/pyextra/acados_template/acados_sim_layout.json b/pyextra/acados_template/acados_sim_layout.json deleted file mode 100644 index 145927689a..0000000000 --- a/pyextra/acados_template/acados_sim_layout.json +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:7e21d44ed88590aaa52541839e2e8a8d008ceb5d82c7e2af9736df7b0f4eb5e9 -size 759 diff --git a/pyextra/acados_template/acados_sim_solver.py b/pyextra/acados_template/acados_sim_solver.py deleted file mode 100644 index e84516f0ed..0000000000 --- a/pyextra/acados_template/acados_sim_solver.py +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:5969249d08301184bdfb0f90adeb23e518a892d38005c3b34ab1d32114f7820f -size 18884 diff --git a/pyextra/acados_template/acados_solver_common.pxd b/pyextra/acados_template/acados_solver_common.pxd deleted file mode 100644 index 5d83ef2c4d..0000000000 --- a/pyextra/acados_template/acados_solver_common.pxd +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:9166b8d29e88c53739da15d79f1a39ef007bac88b3219af171f738f57937286f -size 4335 diff --git a/pyextra/acados_template/builders.py b/pyextra/acados_template/builders.py deleted file mode 100644 index 092bf0b19b..0000000000 --- a/pyextra/acados_template/builders.py +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:6e365ee08adc63a7d366dd283171bcd4b7f8e25825ea1d4fff09b36ba1ab754a -size 5454 diff --git a/pyextra/acados_template/c_templates_tera/CMakeLists.in.txt b/pyextra/acados_template/c_templates_tera/CMakeLists.in.txt deleted file mode 100644 index 5b73d40bcb..0000000000 --- a/pyextra/acados_template/c_templates_tera/CMakeLists.in.txt +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:259e9620ab3810be89004b58838f7a72edd4729928a899c3938d49c05bd10359 -size 14199 diff --git a/pyextra/acados_template/c_templates_tera/CPPLINT.cfg b/pyextra/acados_template/c_templates_tera/CPPLINT.cfg deleted file mode 100644 index 784fc2f61c..0000000000 --- a/pyextra/acados_template/c_templates_tera/CPPLINT.cfg +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:082e141c424fbec023b900efb4f8d5d4e9f13634eb9e92bfe3b076d298585e68 -size 78 diff --git a/pyextra/acados_template/c_templates_tera/Makefile.in b/pyextra/acados_template/c_templates_tera/Makefile.in deleted file mode 100644 index 52d0a96618..0000000000 --- a/pyextra/acados_template/c_templates_tera/Makefile.in +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:a5cfdabde7dd957124c1dcac1ad41c5144f7359b95498bbdc833d6e50bcbfea9 -size 13858 diff --git a/pyextra/acados_template/c_templates_tera/acados_mex_create.in.c b/pyextra/acados_template/c_templates_tera/acados_mex_create.in.c deleted file mode 100644 index cb1b20e90c..0000000000 --- a/pyextra/acados_template/c_templates_tera/acados_mex_create.in.c +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:5a837cf6930efa41eb33fe32bd126ac5acea7afcac8ff3f6aee65e1ec5f974a3 -size 17505 diff --git a/pyextra/acados_template/c_templates_tera/acados_mex_free.in.c b/pyextra/acados_template/c_templates_tera/acados_mex_free.in.c deleted file mode 100644 index a0483d0c86..0000000000 --- a/pyextra/acados_template/c_templates_tera/acados_mex_free.in.c +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:cfab0ed0fac52bea3bb4dae81dd45f18764330f56d08dfed5c80d11923ad6b44 -size 2555 diff --git a/pyextra/acados_template/c_templates_tera/acados_mex_set.in.c b/pyextra/acados_template/c_templates_tera/acados_mex_set.in.c deleted file mode 100644 index 5144152464..0000000000 --- a/pyextra/acados_template/c_templates_tera/acados_mex_set.in.c +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:cac8dd3c940a2b07d7104aeae846f4238a711a973da1667fa7bfdf91dea65cd4 -size 19922 diff --git a/pyextra/acados_template/c_templates_tera/acados_mex_solve.in.c b/pyextra/acados_template/c_templates_tera/acados_mex_solve.in.c deleted file mode 100644 index 7ed976336c..0000000000 --- a/pyextra/acados_template/c_templates_tera/acados_mex_solve.in.c +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:1b5a8e5e2f2bf2456962c53842de00d236d67000b98cb61e48c968fe760eb6a5 -size 2206 diff --git a/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.c b/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.c deleted file mode 100644 index 24c0d8a399..0000000000 --- a/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.c +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:f2a7be5c4f4c3afdcb7e252b72c90a8c0f92992c63bd97977f0470745feb2b6e -size 26035 diff --git a/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.h b/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.h deleted file mode 100644 index 7959f83e14..0000000000 --- a/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.h +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:9299ceb39f13dfc8adac44ca5965713b64c76626f216de0d95505204229daed6 -size 4445 diff --git a/pyextra/acados_template/c_templates_tera/acados_sim_solver_sfun.in.c b/pyextra/acados_template/c_templates_tera/acados_sim_solver_sfun.in.c deleted file mode 100644 index e11faa4751..0000000000 --- a/pyextra/acados_template/c_templates_tera/acados_sim_solver_sfun.in.c +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:b1ee39cbcb027288fcaae01deaafba0f7ccfa863f3cf30a19a2e19860ab32e1f -size 7226 diff --git a/pyextra/acados_template/c_templates_tera/acados_solver.in.c b/pyextra/acados_template/c_templates_tera/acados_solver.in.c deleted file mode 100644 index bb644643c6..0000000000 --- a/pyextra/acados_template/c_templates_tera/acados_solver.in.c +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:933ccc120f3eb064619c1b1b7c69dee3d48492d7bbcbede5480f5e146aae103c -size 101177 diff --git a/pyextra/acados_template/c_templates_tera/acados_solver.in.h b/pyextra/acados_template/c_templates_tera/acados_solver.in.h deleted file mode 100644 index fb0e37616e..0000000000 --- a/pyextra/acados_template/c_templates_tera/acados_solver.in.h +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:17274e2daf731a43a8e28cb8da55e767cffe907a16d9d1338271494649a77715 -size 10723 diff --git a/pyextra/acados_template/c_templates_tera/acados_solver.in.pxd b/pyextra/acados_template/c_templates_tera/acados_solver.in.pxd deleted file mode 100644 index a31279545e..0000000000 --- a/pyextra/acados_template/c_templates_tera/acados_solver.in.pxd +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:8c2eb33568b8c3a7fdc13bab1c922329b6e7c55b0e99eb93becc3853ab9c1ae8 -size 4000 diff --git a/pyextra/acados_template/c_templates_tera/acados_solver_sfun.in.c b/pyextra/acados_template/c_templates_tera/acados_solver_sfun.in.c deleted file mode 100644 index ce16af0f61..0000000000 --- a/pyextra/acados_template/c_templates_tera/acados_solver_sfun.in.c +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:6dad085983af45f46d30dd3ac45615fe0224c4821992de7b8d20b0fff625878a -size 29202 diff --git a/pyextra/acados_template/c_templates_tera/cost_y_0_fun.in.h b/pyextra/acados_template/c_templates_tera/cost_y_0_fun.in.h deleted file mode 100644 index 61085b2478..0000000000 --- a/pyextra/acados_template/c_templates_tera/cost_y_0_fun.in.h +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:40972ebe4461c432b5802c7f4a0d2644b1c2a4727fbc629695cb30c5b71c4d68 -size 3125 diff --git a/pyextra/acados_template/c_templates_tera/cost_y_e_fun.in.h b/pyextra/acados_template/c_templates_tera/cost_y_e_fun.in.h deleted file mode 100644 index af5545f5c6..0000000000 --- a/pyextra/acados_template/c_templates_tera/cost_y_e_fun.in.h +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:1f26921d2f60b054e9c2a5ee3cef9663540e885131c66efba6be3b36857798c5 -size 3125 diff --git a/pyextra/acados_template/c_templates_tera/cost_y_fun.in.h b/pyextra/acados_template/c_templates_tera/cost_y_fun.in.h deleted file mode 100644 index 649a7f4362..0000000000 --- a/pyextra/acados_template/c_templates_tera/cost_y_fun.in.h +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:0baa31610dd9fcf653e4b2289911cb5251787606125ec101739570d28e818bf3 -size 3081 diff --git a/pyextra/acados_template/c_templates_tera/external_cost.in.h b/pyextra/acados_template/c_templates_tera/external_cost.in.h deleted file mode 100644 index 71aaf4d3ed..0000000000 --- a/pyextra/acados_template/c_templates_tera/external_cost.in.h +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:ba8e07cf179230f65ec8c46905ad969f82c41ed8580fb6d45c3376b416f1e52e -size 3354 diff --git a/pyextra/acados_template/c_templates_tera/external_cost_0.in.h b/pyextra/acados_template/c_templates_tera/external_cost_0.in.h deleted file mode 100644 index 276153873b..0000000000 --- a/pyextra/acados_template/c_templates_tera/external_cost_0.in.h +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:eb7fff7be0adbc647ec0af0e8bd68a2b10cbe7eacfc38d8c5665d856c66abb4e -size 3403 diff --git a/pyextra/acados_template/c_templates_tera/external_cost_e.in.h b/pyextra/acados_template/c_templates_tera/external_cost_e.in.h deleted file mode 100644 index 340e2d6218..0000000000 --- a/pyextra/acados_template/c_templates_tera/external_cost_e.in.h +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:7d7754b835a25342af3e1bb53887469daacd53675b1cc86c0562e7f46e5819bd -size 3402 diff --git a/pyextra/acados_template/c_templates_tera/h_constraint.in.h b/pyextra/acados_template/c_templates_tera/h_constraint.in.h deleted file mode 100644 index fa4698bdd6..0000000000 --- a/pyextra/acados_template/c_templates_tera/h_constraint.in.h +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:7b411cbfab08ca9ecbe1d75c80ab71f0551db4050cbcce7dae330f92c7473a4f -size 3272 diff --git a/pyextra/acados_template/c_templates_tera/h_e_constraint.in.h b/pyextra/acados_template/c_templates_tera/h_e_constraint.in.h deleted file mode 100644 index 48339bc82f..0000000000 --- a/pyextra/acados_template/c_templates_tera/h_e_constraint.in.h +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:d1e42330752c441b92bc0f7cc4ef73cac67f8dba664e050cb88cdc682dd545f4 -size 3317 diff --git a/pyextra/acados_template/c_templates_tera/main.in.c b/pyextra/acados_template/c_templates_tera/main.in.c deleted file mode 100644 index 3211c43f23..0000000000 --- a/pyextra/acados_template/c_templates_tera/main.in.c +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:b0476231619e9b58e9d9f1ac38b23d7b07267af02b9fc059b3e96a881f692cbb -size 8044 diff --git a/pyextra/acados_template/c_templates_tera/main_mex.in.c b/pyextra/acados_template/c_templates_tera/main_mex.in.c deleted file mode 100644 index 7605d5f4b3..0000000000 --- a/pyextra/acados_template/c_templates_tera/main_mex.in.c +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:2a6d9e4529cb352a13c65ea30dec561c5423cd191c32510099d9a6e0c2948966 -size 6326 diff --git a/pyextra/acados_template/c_templates_tera/main_sim.in.c b/pyextra/acados_template/c_templates_tera/main_sim.in.c deleted file mode 100644 index 0b1f617791..0000000000 --- a/pyextra/acados_template/c_templates_tera/main_sim.in.c +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:659efc1476d5b27bb6ade88223762fe65d2aa1e96bae0c1fdd7dec6e167fe02a -size 4677 diff --git a/pyextra/acados_template/c_templates_tera/make_main_mex.in.m b/pyextra/acados_template/c_templates_tera/make_main_mex.in.m deleted file mode 100644 index 2a7e73494f..0000000000 --- a/pyextra/acados_template/c_templates_tera/make_main_mex.in.m +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:148d43fb0fa9fa42fc571648c94a344d69e7c8e26b47d023b323773adddbb6f7 -size 4293 diff --git a/pyextra/acados_template/c_templates_tera/make_mex.in.m b/pyextra/acados_template/c_templates_tera/make_mex.in.m deleted file mode 100644 index 41ca87688a..0000000000 --- a/pyextra/acados_template/c_templates_tera/make_mex.in.m +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:0df5010cd2c049661bb45da3ab20cfc78adeea15612bc710259e36d855a1800e -size 4558 diff --git a/pyextra/acados_template/c_templates_tera/make_sfun.in.m b/pyextra/acados_template/c_templates_tera/make_sfun.in.m deleted file mode 100644 index 6645485292..0000000000 --- a/pyextra/acados_template/c_templates_tera/make_sfun.in.m +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:b8d2619181da8938064c8e867ae7b511e476002f6c33e73324f60ac88bea7f22 -size 15658 diff --git a/pyextra/acados_template/c_templates_tera/make_sfun_sim.in.m b/pyextra/acados_template/c_templates_tera/make_sfun_sim.in.m deleted file mode 100644 index 672b08fa10..0000000000 --- a/pyextra/acados_template/c_templates_tera/make_sfun_sim.in.m +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:eeb1550ed63d4d9d561a133db36c4d11c9d03634f466f2ab80ba54ac69dac83a -size 4496 diff --git a/pyextra/acados_template/c_templates_tera/mex_solver.in.m b/pyextra/acados_template/c_templates_tera/mex_solver.in.m deleted file mode 100644 index caf3ec0f5d..0000000000 --- a/pyextra/acados_template/c_templates_tera/mex_solver.in.m +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:4ac71216686385bcf4dedd49a2e4b85e510f36b9d1e1748245d0a7abefad0d9d -size 6185 diff --git a/pyextra/acados_template/c_templates_tera/model.in.h b/pyextra/acados_template/c_templates_tera/model.in.h deleted file mode 100644 index 55bff301c5..0000000000 --- a/pyextra/acados_template/c_templates_tera/model.in.h +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:64753b01879d27e824a86c26ca15b29956efa3b2a5f536dbe78220df4f01fbdc -size 10420 diff --git a/pyextra/acados_template/c_templates_tera/phi_constraint.in.h b/pyextra/acados_template/c_templates_tera/phi_constraint.in.h deleted file mode 100644 index 509e6b7556..0000000000 --- a/pyextra/acados_template/c_templates_tera/phi_constraint.in.h +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:16ef8e23d19800aaf0bc433571c4b2da5a117a1128a3011c3208192880e80d3b -size 2321 diff --git a/pyextra/acados_template/c_templates_tera/phi_e_constraint.in.h b/pyextra/acados_template/c_templates_tera/phi_e_constraint.in.h deleted file mode 100644 index c8a3c596b4..0000000000 --- a/pyextra/acados_template/c_templates_tera/phi_e_constraint.in.h +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:da836e689945ebed0a730f6bbcd3a6922770a07216469cc4898bfcb4bb330c14 -size 658 diff --git a/pyextra/acados_template/generate_c_code_constraint.py b/pyextra/acados_template/generate_c_code_constraint.py deleted file mode 100644 index 673139a871..0000000000 --- a/pyextra/acados_template/generate_c_code_constraint.py +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:e3ca280cc474c3fa50496ca7e21d3b7c918843c6c4f3737627538eb3a8847148 -size 6728 diff --git a/pyextra/acados_template/generate_c_code_discrete_dynamics.py b/pyextra/acados_template/generate_c_code_discrete_dynamics.py deleted file mode 100644 index 1d11e196f5..0000000000 --- a/pyextra/acados_template/generate_c_code_discrete_dynamics.py +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:90b41224841a3bc26fef52eed4f88b4a2c7e6c4ab08ebc3b943f45dde83847db -size 3702 diff --git a/pyextra/acados_template/generate_c_code_explicit_ode.py b/pyextra/acados_template/generate_c_code_explicit_ode.py deleted file mode 100644 index 624645dbea..0000000000 --- a/pyextra/acados_template/generate_c_code_explicit_ode.py +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:148abc2a03f578abaea318edd1042c01d3bb04fc6d9e33b7cab0bab48d9ca53f -size 4418 diff --git a/pyextra/acados_template/generate_c_code_external_cost.py b/pyextra/acados_template/generate_c_code_external_cost.py deleted file mode 100644 index d2989262d1..0000000000 --- a/pyextra/acados_template/generate_c_code_external_cost.py +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:df61d4a3b7c0e04bb02dab5408be15db88e293b481301febb129aedd12cd0cab -size 4337 diff --git a/pyextra/acados_template/generate_c_code_gnsf.py b/pyextra/acados_template/generate_c_code_gnsf.py deleted file mode 100644 index 83140efc14..0000000000 --- a/pyextra/acados_template/generate_c_code_gnsf.py +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:8ad9558c20c7bf4a526874c51204a86cc41b884d071ff55fab9c778a6f2ef999 -size 4935 diff --git a/pyextra/acados_template/generate_c_code_implicit_ode.py b/pyextra/acados_template/generate_c_code_implicit_ode.py deleted file mode 100644 index a80aeebb7b..0000000000 --- a/pyextra/acados_template/generate_c_code_implicit_ode.py +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:14938e8888f7fc72cbf0d1c8614d39952be8909aeb707975afb9b03e2aa817eb -size 5201 diff --git a/pyextra/acados_template/generate_c_code_nls_cost.py b/pyextra/acados_template/generate_c_code_nls_cost.py deleted file mode 100644 index f708e01aa6..0000000000 --- a/pyextra/acados_template/generate_c_code_nls_cost.py +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:b76eeb34b5ff74e072d53cbd08e464ebab8361e805aa6063b23de821ea400287 -size 3864 diff --git a/pyextra/acados_template/simulink_default_opts.json b/pyextra/acados_template/simulink_default_opts.json deleted file mode 100644 index 582e619ca5..0000000000 --- a/pyextra/acados_template/simulink_default_opts.json +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:947332713be607beb2599a93f0d999b4019d096322766564fa1bcaed01b0eebd -size 778 diff --git a/pyextra/acados_template/utils.py b/pyextra/acados_template/utils.py deleted file mode 100644 index c664e9bf0c..0000000000 --- a/pyextra/acados_template/utils.py +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:122176a24fd4a96f0a14201ddea9883bf4ca279ba62aaec78c97bcf1eb3a3174 -size 15953 diff --git a/release/files_common b/release/files_common index a06543abbf..d0afa30dea 100644 --- a/release/files_common +++ b/release/files_common @@ -192,8 +192,6 @@ selfdrive/controls/lib/pid.py selfdrive/controls/lib/radar_helpers.py selfdrive/controls/lib/vehicle_model.py -selfdrive/controls/lib/cluster/* - selfdrive/controls/lib/lateral_mpc_lib/.gitignore selfdrive/controls/lib/longitudinal_mpc_lib/.gitignore selfdrive/controls/lib/lateral_mpc_lib/* @@ -328,7 +326,6 @@ system/camerad/SConscript system/camerad/main.cc system/camerad/snapshot/* -system/camerad/include/* system/camerad/cameras/camera_common.h system/camerad/cameras/camera_common.cc system/camerad/cameras/sensor2_i2c.h @@ -384,7 +381,6 @@ selfdrive/modeld/thneed/thneed.h selfdrive/modeld/thneed/thneed_common.cc selfdrive/modeld/thneed/thneed_qcom2.cc selfdrive/modeld/thneed/serialize.cc -selfdrive/modeld/thneed/include/* selfdrive/modeld/runners/snpemodel.cc selfdrive/modeld/runners/snpemodel.h @@ -412,8 +408,11 @@ selfdrive/assets/offroad/* selfdrive/assets/sounds/* selfdrive/assets/training/* +third_party/.gitignore third_party/SConscript +third_party/cluster/* + third_party/linux/** third_party/opencl/** @@ -436,15 +435,13 @@ third_party/snpe/dsp** third_party/acados/x86_64/** third_party/acados/larch64/** third_party/acados/include/** +third_party/acados/acados_template/** third_party/qt5/larch64/bin/** scripts/update_now.sh scripts/stop_updater.sh -pyextra/.gitignore -pyextra/acados_template/** - rednose/.gitignore rednose/** laika/** diff --git a/selfdrive/controls/lib/lateral_mpc_lib/SConscript b/selfdrive/controls/lib/lateral_mpc_lib/SConscript index df1e2a2a1a..868b5a873c 100644 --- a/selfdrive/controls/lib/lateral_mpc_lib/SConscript +++ b/selfdrive/controls/lib/lateral_mpc_lib/SConscript @@ -44,7 +44,7 @@ generated_files = [ ] + build_files acados_dir = '#third_party/acados' -acados_templates_dir = '#pyextra/acados_template/c_templates_tera' +acados_templates_dir = '#third_party/acados/acados_template/c_templates_tera' source_list = ['lat_mpc.py', f'{acados_dir}/include/acados_c/ocp_nlp_interface.h', @@ -70,8 +70,8 @@ lib_solver = lenv.SharedLibrary(f"{gen}/acados_ocp_solver_lat", LIBS=['m', 'acados', 'hpipm', 'blasfeo', 'qpOASES_e']) # generate cython stuff -acados_ocp_solver_pyx = File("#pyextra/acados_template/acados_ocp_solver_pyx.pyx") -acados_ocp_solver_common = File("#pyextra/acados_template/acados_solver_common.pxd") +acados_ocp_solver_pyx = File("#third_party/acados/acados_template/acados_ocp_solver_pyx.pyx") +acados_ocp_solver_common = File("#third_party/acados/acados_template/acados_solver_common.pxd") libacados_ocp_solver_pxd = File(f'{gen}/acados_solver.pxd') libacados_ocp_solver_c = File(f'{gen}/acados_ocp_solver_pyx.c') diff --git a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py index 9607532ace..536f436fce 100755 --- a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py +++ b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py @@ -8,7 +8,7 @@ from common.realtime import sec_since_boot from selfdrive.modeld.constants import T_IDXS if __name__ == '__main__': # generating code - from pyextra.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver + from third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver else: from selfdrive.controls.lib.lateral_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython # pylint: disable=no-name-in-module, import-error diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript b/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript index 5a9e69c297..e5b2360607 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript @@ -51,7 +51,7 @@ generated_files = [ ] + build_files acados_dir = '#third_party/acados' -acados_templates_dir = '#pyextra/acados_template/c_templates_tera' +acados_templates_dir = '#third_party/acados/acados_template/c_templates_tera' source_list = ['long_mpc.py', f'{acados_dir}/include/acados_c/ocp_nlp_interface.h', @@ -77,8 +77,8 @@ lib_solver = lenv.SharedLibrary(f"{gen}/acados_ocp_solver_long", LIBS=['m', 'acados', 'hpipm', 'blasfeo', 'qpOASES_e']) # generate cython stuff -acados_ocp_solver_pyx = File("#pyextra/acados_template/acados_ocp_solver_pyx.pyx") -acados_ocp_solver_common = File("#pyextra/acados_template/acados_solver_common.pxd") +acados_ocp_solver_pyx = File("#third_party/acados/acados_template/acados_ocp_solver_pyx.pyx") +acados_ocp_solver_common = File("#third_party/acados/acados_template/acados_solver_common.pxd") libacados_ocp_solver_pxd = File(f'{gen}/acados_solver.pxd') libacados_ocp_solver_c = File(f'{gen}/acados_ocp_solver_pyx.c') diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 79a9ec4f0c..dc27fd27a9 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -9,7 +9,7 @@ from selfdrive.modeld.constants import index_function from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU if __name__ == '__main__': # generating code - from pyextra.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver + from third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver else: from selfdrive.controls.lib.longitudinal_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython # pylint: disable=no-name-in-module, import-error diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 3d958139d6..db87604e98 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -8,9 +8,9 @@ from cereal import car from common.numpy_fast import interp from common.params import Params from common.realtime import Ratekeeper, Priority, config_realtime_process -from selfdrive.controls.lib.cluster.fastcluster_py import cluster_points_centroid from selfdrive.controls.lib.radar_helpers import Cluster, Track, RADAR_TO_CAMERA from system.swaglog import cloudlog +from third_party.cluster.fastcluster_py import cluster_points_centroid class KalmanParams(): diff --git a/selfdrive/controls/tests/test_clustering.py b/selfdrive/controls/tests/test_clustering.py index 290b267029..99c24d5938 100644 --- a/selfdrive/controls/tests/test_clustering.py +++ b/selfdrive/controls/tests/test_clustering.py @@ -7,8 +7,8 @@ from fastcluster import linkage_vector from scipy.cluster import _hierarchy from scipy.spatial.distance import pdist -from selfdrive.controls.lib.cluster.fastcluster_py import hclust, ffi -from selfdrive.controls.lib.cluster.fastcluster_py import cluster_points_centroid +from third_party.cluster.fastcluster_py import hclust, ffi +from third_party.cluster.fastcluster_py import cluster_points_centroid def fcluster(Z, t, criterion='inconsistent', depth=2, R=None, monocrit=None): diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index 928507f65b..369c529626 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -23,8 +23,6 @@ from system.version import is_dirty, get_commit, get_version, get_origin, get_sh terms_version, training_version, is_tested_branch -sys.path.append(os.path.join(BASEDIR, "pyextra")) - def manager_init() -> None: # update system time from panda diff --git a/selfdrive/modeld/thneed/include/msm_kgsl.h b/selfdrive/modeld/thneed/include/msm_kgsl.h deleted file mode 100644 index 6653b07a59..0000000000 --- a/selfdrive/modeld/thneed/include/msm_kgsl.h +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:089b21b377325c0b0f04b96b6ed4a8e5975b1c050191598cd64dc0a3a3565a71 -size 45343 diff --git a/selfdrive/modeld/thneed/thneed.h b/selfdrive/modeld/thneed/thneed.h index 65475ccf7f..6475577734 100644 --- a/selfdrive/modeld/thneed/thneed.h +++ b/selfdrive/modeld/thneed/thneed.h @@ -12,7 +12,7 @@ #include -#include "selfdrive/modeld/thneed/include/msm_kgsl.h" +#include "msm_kgsl.h" using namespace std; diff --git a/system/camerad/SConscript b/system/camerad/SConscript index ddc763b53d..3ecc3f6d72 100644 --- a/system/camerad/SConscript +++ b/system/camerad/SConscript @@ -2,17 +2,13 @@ Import('env', 'arch', 'cereal', 'messaging', 'common', 'gpucommon', 'visionipc') libs = ['m', 'pthread', common, 'jpeg', 'OpenCL', 'yuv', cereal, messaging, 'zmq', 'capnp', 'kj', visionipc, gpucommon, 'atomic'] -cenv = env.Clone() -cenv['CPPPATH'].append('include/') - -camera_obj = cenv.Object(['cameras/camera_qcom2.cc', 'cameras/camera_common.cc', 'cameras/camera_util.cc']) -cenv.Program('camerad', [ +camera_obj = env.Object(['cameras/camera_qcom2.cc', 'cameras/camera_common.cc', 'cameras/camera_util.cc']) +env.Program('camerad', [ 'main.cc', camera_obj, ], LIBS=libs) if GetOption("test") and arch == "x86_64": - cenv.Program('test/ae_gray_test', [ - 'test/ae_gray_test.cc', - camera_obj, - ], LIBS=libs) + env.Program('test/ae_gray_test', + ['test/ae_gray_test.cc', camera_obj], + LIBS=libs) diff --git a/third_party/.gitignore b/third_party/.gitignore new file mode 100644 index 0000000000..0d20b6487c --- /dev/null +++ b/third_party/.gitignore @@ -0,0 +1 @@ +*.pyc diff --git a/third_party/SConscript b/third_party/SConscript index e5bbfaa07a..e8d1789ee0 100644 --- a/third_party/SConscript +++ b/third_party/SConscript @@ -4,3 +4,5 @@ env.Library('json11', ['json11/json11.cpp'], CCFLAGS=env['CCFLAGS'] + ['-Wno-unq env.Append(CPPPATH=[Dir('json11')]) env.Library('kaitai', ['kaitai/kaitaistream.cpp'], CPPDEFINES=['KS_STR_ENCODING_NONE']) + +SConscript(['cluster/SConscript']) diff --git a/third_party/acados/acados_template/.gitignore b/third_party/acados/acados_template/.gitignore new file mode 100644 index 0000000000..63b741bb9d --- /dev/null +++ b/third_party/acados/acados_template/.gitignore @@ -0,0 +1,6 @@ +__pycache__/ + +# Cython intermediates +*_pyx.c +*_pyx.o +*_pyx.so diff --git a/third_party/acados/acados_template/__init__.py b/third_party/acados/acados_template/__init__.py new file mode 100644 index 0000000000..f33b75bb7b --- /dev/null +++ b/third_party/acados/acados_template/__init__.py @@ -0,0 +1,43 @@ +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + +from .acados_model import * +from .generate_c_code_explicit_ode import * +from .generate_c_code_implicit_ode import * +from .generate_c_code_constraint import * +from .generate_c_code_nls_cost import * +from .acados_ocp import * +from .acados_sim import * +from .acados_ocp_solver import * +from .acados_sim_solver import * +from .utils import * diff --git a/third_party/acados/acados_template/acados_layout.json b/third_party/acados/acados_template/acados_layout.json new file mode 100644 index 0000000000..c9f0b90c73 --- /dev/null +++ b/third_party/acados/acados_template/acados_layout.json @@ -0,0 +1,804 @@ +{ + "code_export_directory": [ + "str" + ], + "acados_include_path": [ + "str" + ], + "cython_include_dirs": [ + "str" + ], + "model": { + "name" : [ + "str" + ], + "dyn_ext_fun_type" : [ + "str" + ], + "dyn_source_discrete" : [ + "str" + ], + "dyn_disc_fun_jac_hess" : [ + "str" + ], + "dyn_disc_fun_jac" : [ + "str" + ], + "dyn_disc_fun" : [ + "str" + ], + "gnsf" : { + "nontrivial_f_LO": [ + "int" + ], + "purely_linear": [ + "int" + ] + } + }, + "parameter_values": [ + "ndarray", + [ + "np" + ] + ], + "acados_lib_path": [ + "str" + ], + "problem_class": [ + "str" + ], + "constraints": { + "constr_type": [ + "str" + ], + "constr_type_e": [ + "str" + ], + "lbx": [ + "ndarray", + [ + "nbx" + ] + ], + "lbu": [ + "ndarray", + [ + "nbu" + ] + ], + "ubx": [ + "ndarray", + [ + "nbx" + ] + ], + "ubu": [ + "ndarray", + [ + "nbu" + ] + ], + "idxbx": [ + "ndarray", + [ + "nbx" + ] + ], + "idxbu": [ + "ndarray", + [ + "nbu" + ] + ], + "lbx_e": [ + "ndarray", + [ + "nbx_e" + ] + ], + "ubx_e": [ + "ndarray", + [ + "nbx_e" + ] + ], + "idxbx_e": [ + "ndarray", + [ + "nbx_e" + ] + ], + "lbx_0": [ + "ndarray", + [ + "nbx_0" + ] + ], + "ubx_0": [ + "ndarray", + [ + "nbx_0" + ] + ], + "idxbx_0": [ + "ndarray", + [ + "nbx_0" + ] + ], + "idxbxe_0": [ + "ndarray", + [ + "nbxe_0" + ] + ], + "lg": [ + "ndarray", + [ + "ng" + ] + ], + "ug": [ + "ndarray", + [ + "ng" + ] + ], + "D": [ + "ndarray", + [ + "ng", + "nu" + ] + ], + "C": [ + "ndarray", + [ + "ng", + "nx" + ] + ], + "C_e": [ + "ndarray", + [ + "ng_e", + "nx" + ] + ], + "lg_e": [ + "ndarray", + [ + "ng_e" + ] + ], + "ug_e": [ + "ndarray", + [ + "ng_e" + ] + ], + "lh": [ + "ndarray", + [ + "nh" + ] + ], + "uh": [ + "ndarray", + [ + "nh" + ] + ], + "lh_e": [ + "ndarray", + [ + "nh_e" + ] + ], + "uh_e": [ + "ndarray", + [ + "nh_e" + ] + ], + "lphi": [ + "ndarray", + [ + "nphi" + ] + ], + "uphi": [ + "ndarray", + [ + "nphi" + ] + ], + "lphi_e": [ + "ndarray", + [ + "nphi_e" + ] + ], + "uphi_e": [ + "ndarray", + [ + "nphi_e" + ] + ], + "lsbx": [ + "ndarray", + [ + "nsbx" + ] + ], + "usbx": [ + "ndarray", + [ + "nsbx" + ] + ], + "lsbu": [ + "ndarray", + [ + "nsbu" + ] + ], + "usbu": [ + "ndarray", + [ + "nsbu" + ] + ], + "idxsbx": [ + "ndarray", + [ + "nsbx" + ] + ], + "idxsbu": [ + "ndarray", + [ + "nsbu" + ] + ], + "lsbx_e": [ + "ndarray", + [ + "nsbx_e" + ] + ], + "usbx_e": [ + "ndarray", + [ + "nsbx_e" + ] + ], + "idxsbx_e": [ + "ndarray", + [ + "nsbx_e" + ] + ], + "lsg": [ + "ndarray", + [ + "nsg" + ] + ], + "usg": [ + "ndarray", + [ + "nsg" + ] + ], + "idxsg": [ + "ndarray", + [ + "nsg" + ] + ], + "lsg_e": [ + "ndarray", + [ + "nsg_e" + ] + ], + "usg_e": [ + "ndarray", + [ + "nsg_e" + ] + ], + "idxsg_e": [ + "ndarray", + [ + "nsg_e" + ] + ], + "lsh": [ + "ndarray", + [ + "nsh" + ] + ], + "ush": [ + "ndarray", + [ + "nsh" + ] + ], + "idxsh": [ + "ndarray", + [ + "nsh" + ] + ], + "lsh_e": [ + "ndarray", + [ + "nsh_e" + ] + ], + "ush_e": [ + "ndarray", + [ + "nsh_e" + ] + ], + "idxsh_e": [ + "ndarray", + [ + "nsh_e" + ] + ], + "lsphi": [ + "ndarray", + [ + "nsphi" + ] + ], + "usphi": [ + "ndarray", + [ + "nsphi" + ] + ], + "idxsphi": [ + "ndarray", + [ + "nsphi" + ] + ], + "lsphi_e": [ + "ndarray", + [ + "nsphi_e" + ] + ], + "usphi_e": [ + "ndarray", + [ + "nsphi_e" + ] + ], + "idxsphi_e": [ + "ndarray", + [ + "nsphi_e" + ] + ] + }, + "cost": { + "cost_type_0": [ + "str" + ], + "cost_type": [ + "str" + ], + "cost_type_e": [ + "str" + ], + "cost_ext_fun_type_0": [ + "str" + ], + "cost_ext_fun_type": [ + "str" + ], + "cost_ext_fun_type_e": [ + "str" + ], + "Vu_0": [ + "ndarray", + [ + "ny_0", + "nu" + ] + ], + "Vu": [ + "ndarray", + [ + "ny", + "nu" + ] + ], + "Vx_0": [ + "ndarray", + [ + "ny_0", + "nx" + ] + ], + "Vx": [ + "ndarray", + [ + "ny", + "nx" + ] + ], + "Vx_e": [ + "ndarray", + [ + "ny_e", + "nx" + ] + ], + "Vz_0": [ + "ndarray", + [ + "ny_0", + "nz" + ] + ], + "Vz": [ + "ndarray", + [ + "ny", + "nz" + ] + ], + "W_0": [ + "ndarray", + [ + "ny_0", + "ny_0" + ] + ], + "W": [ + "ndarray", + [ + "ny", + "ny" + ] + ], + "Zl": [ + "ndarray", + [ + "ns" + ] + ], + "Zu": [ + "ndarray", + [ + "ns" + ] + ], + "zl": [ + "ndarray", + [ + "ns" + ] + ], + "zu": [ + "ndarray", + [ + "ns" + ] + ], + "W_e": [ + "ndarray", + [ + "ny_e", + "ny_e" + ] + ], + "yref_0": [ + "ndarray", + [ + "ny_0" + ] + ], + "yref": [ + "ndarray", + [ + "ny" + ] + ], + "yref_e": [ + "ndarray", + [ + "ny_e" + ] + ], + "Zl_e": [ + "ndarray", + [ + "ns_e" + ] + ], + "Zu_e": [ + "ndarray", + [ + "ns_e" + ] + ], + "zl_e": [ + "ndarray", + [ + "ns_e" + ] + ], + "zu_e": [ + "ndarray", + [ + "ns_e" + ] + ] + }, + "dims": { + "N": [ + "int" + ], + "nbu": [ + "int" + ], + "nbx": [ + "int" + ], + "nsbu": [ + "int" + ], + "nsbx": [ + "int" + ], + "nsbx_e": [ + "int" + ], + "nbx_0": [ + "int" + ], + "nbx_e": [ + "int" + ], + "nbxe_0": [ + "int" + ], + "nsg": [ + "int" + ], + "nsg_e": [ + "int" + ], + "nsh": [ + "int" + ], + "nsh_e": [ + "int" + ], + "nsphi": [ + "int" + ], + "nsphi_e": [ + "int" + ], + "ns": [ + "int" + ], + "ns_e": [ + "int" + ], + "ng": [ + "int" + ], + "ng_e": [ + "int" + ], + "np": [ + "int" + ], + "nr": [ + "int" + ], + "nr_e": [ + "int" + ], + "nh": [ + "int" + ], + "nh_e": [ + "int" + ], + "nphi": [ + "int" + ], + "nphi_e": [ + "int" + ], + "nu": [ + "int" + ], + "nx": [ + "int" + ], + "ny": [ + "int" + ], + "ny_0": [ + "int" + ], + "ny_e": [ + "int" + ], + "nz": [ + "int" + ], + "gnsf_nx1": [ + "int" + ], + "gnsf_nz1": [ + "int" + ], + "gnsf_nuhat": [ + "int" + ], + "gnsf_ny": [ + "int" + ], + "gnsf_nout": [ + "int" + ] + }, + "solver_options": { + "time_steps": [ + "ndarray", + [ + "N" + ] + ], + "hessian_approx": [ + "str" + ], + "hpipm_mode": [ + "str" + ], + "regularize_method": [ + "str" + ], + "integrator_type": [ + "str" + ], + "nlp_solver_type": [ + "str" + ], + "collocation_type": [ + "str" + ], + "globalization": [ + "str" + ], + "nlp_solver_step_length": [ + "float" + ], + "levenberg_marquardt": [ + "float" + ], + "qp_solver": [ + "str" + ], + "tf": [ + "float" + ], + "Tsim": [ + "float" + ], + "alpha_min": [ + "float" + ], + "alpha_reduction": [ + "float" + ], + "line_search_use_sufficient_descent": [ + "int" + ], + "globalization_use_SOC": [ + "int" + ], + "full_step_dual": [ + "int" + ], + "eps_sufficient_descent": [ + "float" + ], + "sim_method_num_stages": [ + "ndarray", + [ + "N" + ] + ], + "sim_method_num_steps": [ + "ndarray", + [ + "N" + ] + ], + "sim_method_newton_iter": [ + "int" + ], + "sim_method_jac_reuse": [ + "ndarray", + [ + "N" + ] + ], + "qp_solver_cond_N": [ + "int" + ], + "qp_solver_warm_start": [ + "int" + ], + "qp_solver_tol_stat": [ + "float" + ], + "qp_solver_tol_eq": [ + "float" + ], + "qp_solver_tol_ineq": [ + "float" + ], + "qp_solver_tol_comp": [ + "float" + ], + "qp_solver_iter_max": [ + "int" + ], + "nlp_solver_tol_stat": [ + "float" + ], + "nlp_solver_tol_eq": [ + "float" + ], + "nlp_solver_tol_ineq": [ + "float" + ], + "nlp_solver_tol_comp": [ + "float" + ], + "nlp_solver_max_iter": [ + "int" + ], + "print_level": [ + "int" + ], + "initialize_t_slacks": [ + "int" + ], + "exact_hess_cost": [ + "int" + ], + "exact_hess_constr": [ + "int" + ], + "exact_hess_dyn": [ + "int" + ], + "ext_cost_num_hess": [ + "int" + ], + "model_external_shared_lib_dir": [ + "str" + ], + "model_external_shared_lib_name": [ + "str" + ] + } +} diff --git a/third_party/acados/acados_template/acados_model.py b/third_party/acados/acados_template/acados_model.py new file mode 100644 index 0000000000..e292cc7477 --- /dev/null +++ b/third_party/acados/acados_template/acados_model.py @@ -0,0 +1,167 @@ +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + + +class AcadosModel(): + """ + Class containing all the information to code generate the external CasADi functions + that are needed when creating an acados ocp solver or acados integrator. + Thus, this class contains: + + a) the :py:attr:`name` of the model, + b) all CasADi variables/expressions needed in the CasADi function generation process. + """ + def __init__(self): + ## common for OCP and Integrator + self.name = None + """ + The model name is used for code generation. Type: string. Default: :code:`None` + """ + self.x = None #: CasADi variable describing the state of the system; Default: :code:`None` + self.xdot = None #: CasADi variable describing the derivative of the state wrt time; Default: :code:`None` + self.u = None #: CasADi variable describing the input of the system; Default: :code:`None` + self.z = [] #: CasADi variable describing the algebraic variables of the DAE; Default: :code:`empty` + self.p = [] #: CasADi variable describing parameters of the DAE; Default: :code:`empty` + # dynamics + self.f_impl_expr = None + """ + CasADi expression for the implicit dynamics :math:`f_\\text{impl}(\dot{x}, x, u, z, p) = 0`. + Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.integrator_type` == 'IRK'. + Default: :code:`None` + """ + self.f_expl_expr = None + """ + CasADi expression for the explicit dynamics :math:`\dot{x} = f_\\text{expl}(x, u, p)`. + Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.integrator_type` == 'ERK'. + Default: :code:`None` + """ + self.disc_dyn_expr = None + """ + CasADi expression for the discrete dynamics :math:`x_{+} = f_\\text{disc}(x, u, p)`. + Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.integrator_type` == 'DISCRETE'. + Default: :code:`None` + """ + + self.dyn_ext_fun_type = 'casadi' #: type of external functions for dynamics module; 'casadi' or 'generic'; Default: 'casadi' + self.dyn_source_discrete = None #: name of source file for discrete dyanamics; Default: :code:`None` + self.dyn_disc_fun_jac_hess = None #: name of function discrete dyanamics + jacobian and hessian; Default: :code:`None` + self.dyn_disc_fun_jac = None #: name of function discrete dyanamics + jacobian; Default: :code:`None` + self.dyn_disc_fun = None #: name of function discrete dyanamics; Default: :code:`None` + + # for GNSF models + self.gnsf = {'nontrivial_f_LO': 1, 'purely_linear': 0} + """ + dictionary containing information on GNSF structure needed when rendering templates. + Contains integers `nontrivial_f_LO`, `purely_linear`. + """ + + ## for OCP + # constraints + self.con_h_expr = None #: CasADi expression for the constraint :math:`h`; Default: :code:`None` + self.con_phi_expr = None #: CasADi expression for the constraint phi; Default: :code:`None` + self.con_r_expr = None #: CasADi expression for the constraint phi(r); Default: :code:`None` + self.con_r_in_phi = None + # terminal + self.con_h_expr_e = None #: CasADi expression for the terminal constraint :math:`h^e`; Default: :code:`None` + self.con_r_expr_e = None #: CasADi expression for the terminal constraint; Default: :code:`None` + self.con_phi_expr_e = None #: CasADi expression for the terminal constraint; Default: :code:`None` + self.con_r_in_phi_e = None + # cost + self.cost_y_expr = None #: CasADi expression for nonlinear least squares; Default: :code:`None` + self.cost_y_expr_e = None #: CasADi expression for nonlinear least squares, terminal; Default: :code:`None` + self.cost_y_expr_0 = None #: CasADi expression for nonlinear least squares, initial; Default: :code:`None` + self.cost_expr_ext_cost = None #: CasADi expression for external cost; Default: :code:`None` + self.cost_expr_ext_cost_e = None #: CasADi expression for external cost, terminal; Default: :code:`None` + self.cost_expr_ext_cost_0 = None #: CasADi expression for external cost, initial; Default: :code:`None` + self.cost_expr_ext_cost_custom_hess = None #: CasADi expression for custom hessian (only for external cost); Default: :code:`None` + self.cost_expr_ext_cost_custom_hess_e = None #: CasADi expression for custom hessian (only for external cost), terminal; Default: :code:`None` + self.cost_expr_ext_cost_custom_hess_0 = None #: CasADi expression for custom hessian (only for external cost), initial; Default: :code:`None` + + +def acados_model_strip_casadi_symbolics(model): + out = model + if 'f_impl_expr' in out.keys(): + del out['f_impl_expr'] + if 'f_expl_expr' in out.keys(): + del out['f_expl_expr'] + if 'disc_dyn_expr' in out.keys(): + del out['disc_dyn_expr'] + if 'x' in out.keys(): + del out['x'] + if 'xdot' in out.keys(): + del out['xdot'] + if 'u' in out.keys(): + del out['u'] + if 'z' in out.keys(): + del out['z'] + if 'p' in out.keys(): + del out['p'] + # constraints + if 'con_phi_expr' in out.keys(): + del out['con_phi_expr'] + if 'con_h_expr' in out.keys(): + del out['con_h_expr'] + if 'con_r_expr' in out.keys(): + del out['con_r_expr'] + if 'con_r_in_phi' in out.keys(): + del out['con_r_in_phi'] + # terminal + if 'con_phi_expr_e' in out.keys(): + del out['con_phi_expr_e'] + if 'con_h_expr_e' in out.keys(): + del out['con_h_expr_e'] + if 'con_r_expr_e' in out.keys(): + del out['con_r_expr_e'] + if 'con_r_in_phi_e' in out.keys(): + del out['con_r_in_phi_e'] + # cost + if 'cost_y_expr' in out.keys(): + del out['cost_y_expr'] + if 'cost_y_expr_e' in out.keys(): + del out['cost_y_expr_e'] + if 'cost_y_expr_0' in out.keys(): + del out['cost_y_expr_0'] + if 'cost_expr_ext_cost' in out.keys(): + del out['cost_expr_ext_cost'] + if 'cost_expr_ext_cost_e' in out.keys(): + del out['cost_expr_ext_cost_e'] + if 'cost_expr_ext_cost_0' in out.keys(): + del out['cost_expr_ext_cost_0'] + if 'cost_expr_ext_cost_custom_hess' in out.keys(): + del out['cost_expr_ext_cost_custom_hess'] + if 'cost_expr_ext_cost_custom_hess_e' in out.keys(): + del out['cost_expr_ext_cost_custom_hess_e'] + if 'cost_expr_ext_cost_custom_hess_0' in out.keys(): + del out['cost_expr_ext_cost_custom_hess_0'] + + return out diff --git a/third_party/acados/acados_template/acados_ocp.py b/third_party/acados/acados_template/acados_ocp.py new file mode 100644 index 0000000000..80970239eb --- /dev/null +++ b/third_party/acados/acados_template/acados_ocp.py @@ -0,0 +1,2955 @@ +# -*- coding: future_fstrings -*- +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + +import numpy as np +import os +from .acados_model import AcadosModel +from .utils import get_acados_path, J_to_idx, J_to_idx_slack + +class AcadosOcpDims: + """ + Class containing the dimensions of the optimal control problem. + """ + def __init__(self): + self.__nx = None + self.__nu = None + self.__nz = 0 + self.__np = 0 + self.__ny = 0 + self.__ny_e = 0 + self.__ny_0 = 0 + self.__nr = 0 + self.__nr_e = 0 + self.__nh = 0 + self.__nh_e = 0 + self.__nphi = 0 + self.__nphi_e = 0 + self.__nbx = 0 + self.__nbx_0 = 0 + self.__nbx_e = 0 + self.__nbu = 0 + self.__nsbx = 0 + self.__nsbx_e = 0 + self.__nsbu = 0 + self.__nsh = 0 + self.__nsh_e = 0 + self.__nsphi = 0 + self.__nsphi_e = 0 + self.__ns = 0 + self.__ns_e = 0 + self.__ng = 0 + self.__ng_e = 0 + self.__nsg = 0 + self.__nsg_e = 0 + self.__nbxe_0 = None + self.__N = None + + + @property + def nx(self): + """:math:`n_x` - number of states. + Type: int; default: None""" + return self.__nx + + @property + def nz(self): + """:math:`n_z` - number of algebraic variables. + Type: int; default: 0""" + return self.__nz + + @property + def nu(self): + """:math:`n_u` - number of inputs. + Type: int; default: None""" + return self.__nu + + @property + def np(self): + """:math:`n_p` - number of parameters. + Type: int; default: 0""" + return self.__np + + @property + def ny(self): + """:math:`n_y` - number of residuals in Lagrange term. + Type: int; default: 0""" + return self.__ny + + @property + def ny_0(self): + """:math:`n_{y}^0` - number of residuals in Mayer term. + Type: int; default: 0""" + return self.__ny_0 + + @property + def ny_e(self): + """:math:`n_{y}^e` - number of residuals in Mayer term. + Type: int; default: 0""" + return self.__ny_e + + @property + def nr(self): + """:math:`n_{\pi}` - dimension of the image of the inner nonlinear function in positive definite constraints. + Type: int; default: 0""" + return self.__nr + + @property + def nr_e(self): + """:math:`n_{\pi}^e` - dimension of the image of the inner nonlinear function in positive definite constraints. + Type: int; default: 0""" + return self.__nr_e + + @property + def nh(self): + """:math:`n_h` - number of nonlinear constraints. + Type: int; default: 0""" + return self.__nh + + @property + def nh_e(self): + """:math:`n_{h}^e` - number of nonlinear constraints at terminal shooting node N. + Type: int; default: 0""" + return self.__nh_e + + @property + def nphi(self): + """:math:`n_{\phi}` - number of convex-over-nonlinear constraints. + Type: int; default: 0""" + return self.__nphi + + @property + def nphi_e(self): + """:math:`n_{\phi}^e` - number of convex-over-nonlinear constraints at terminal shooting node N. + Type: int; default: 0""" + return self.__nphi_e + + @property + def nbx(self): + """:math:`n_{b_x}` - number of state bounds. + Type: int; default: 0""" + return self.__nbx + + @property + def nbxe_0(self): + """:math:`n_{be_{x0}}` - number of state bounds at initial shooting node that are equalities. + Type: int; default: None""" + return self.__nbxe_0 + + @property + def nbx_0(self): + """:math:`n_{b_{x0}}` - number of state bounds for initial state. + Type: int; default: 0""" + return self.__nbx_0 + + @property + def nbx_e(self): + """:math:`n_{b_x}` - number of state bounds at terminal shooting node N. + Type: int; default: 0""" + return self.__nbx_e + + @property + def nbu(self): + """:math:`n_{b_u}` - number of input bounds. + Type: int; default: 0""" + return self.__nbu + + @property + def nsbx(self): + """:math:`n_{{sb}_x}` - number of soft state bounds. + Type: int; default: 0""" + return self.__nsbx + + @property + def nsbx_e(self): + """:math:`n_{{sb}^e_{x}}` - number of soft state bounds at terminal shooting node N. + Type: int; default: 0""" + return self.__nsbx_e + + @property + def nsbu(self): + """:math:`n_{{sb}_u}` - number of soft input bounds. + Type: int; default: 0""" + return self.__nsbu + + @property + def nsg(self): + """:math:`n_{{sg}}` - number of soft general linear constraints. + Type: int; default: 0""" + return self.__nsg + + @property + def nsg_e(self): + """:math:`n_{{sg}^e}` - number of soft general linear constraints at terminal shooting node N. + Type: int; default: 0""" + return self.__nsg_e + + @property + def nsh(self): + """:math:`n_{{sh}}` - number of soft nonlinear constraints. + Type: int; default: 0""" + return self.__nsh + + @property + def nsh_e(self): + """:math:`n_{{sh}}^e` - number of soft nonlinear constraints at terminal shooting node N. + Type: int; default: 0""" + return self.__nsh_e + + @property + def nsphi(self): + """:math:`n_{{s\phi}}` - number of soft convex-over-nonlinear constraints. + Type: int; default: 0""" + return self.__nsphi + + @property + def nsphi_e(self): + """:math:`n_{{s\phi}^e}` - number of soft convex-over-nonlinear constraints at terminal shooting node N. + Type: int; default: 0""" + return self.__nsphi_e + + @property + def ns(self): + """:math:`n_{s}` - total number of slacks. + Type: int; default: 0""" + return self.__ns + + @property + def ns_e(self): + """:math:`n_{s}^e` - total number of slacks at terminal shooting node N. + Type: int; default: 0""" + return self.__ns_e + + @property + def ng(self): + """:math:`n_{g}` - number of general polytopic constraints. + Type: int; default: 0""" + return self.__ng + + @property + def ng_e(self): + """:math:`n_{g}^e` - number of general polytopic constraints at terminal shooting node N. + Type: int; default: 0""" + return self.__ng_e + + @property + def N(self): + """:math:`N` - prediction horizon. + Type: int; default: None""" + return self.__N + + @nx.setter + def nx(self, nx): + if isinstance(nx, int) and nx > 0: + self.__nx = nx + else: + raise Exception('Invalid nx value, expected positive integer. Exiting.') + + @nz.setter + def nz(self, nz): + if isinstance(nz, int) and nz > -1: + self.__nz = nz + else: + raise Exception('Invalid nz value, expected nonnegative integer. Exiting.') + + @nu.setter + def nu(self, nu): + if isinstance(nu, int) and nu > -1: + self.__nu = nu + else: + raise Exception('Invalid nu value, expected nonnegative integer. Exiting.') + + @np.setter + def np(self, np): + if isinstance(np, int) and np > -1: + self.__np = np + else: + raise Exception('Invalid np value, expected nonnegative integer. Exiting.') + + @ny_0.setter + def ny_0(self, ny_0): + if isinstance(ny_0, int) and ny_0 > -1: + self.__ny_0 = ny_0 + else: + raise Exception('Invalid ny_0 value, expected nonnegative integer. Exiting.') + + @ny.setter + def ny(self, ny): + if isinstance(ny, int) and ny > -1: + self.__ny = ny + else: + raise Exception('Invalid ny value, expected nonnegative integer. Exiting.') + + @ny_e.setter + def ny_e(self, ny_e): + if isinstance(ny_e, int) and ny_e > -1: + self.__ny_e = ny_e + else: + raise Exception('Invalid ny_e value, expected nonnegative integer. Exiting.') + + @nr.setter + def nr(self, nr): + if isinstance(nr, int) and nr > -1: + self.__nr = nr + else: + raise Exception('Invalid nr value, expected nonnegative integer. Exiting.') + + @nr_e.setter + def nr_e(self, nr_e): + if isinstance(nr_e, int) and nr_e > -1: + self.__nr_e = nr_e + else: + raise Exception('Invalid nr_e value, expected nonnegative integer. Exiting.') + + @nh.setter + def nh(self, nh): + if isinstance(nh, int) and nh > -1: + self.__nh = nh + else: + raise Exception('Invalid nh value, expected nonnegative integer. Exiting.') + + @nh_e.setter + def nh_e(self, nh_e): + if isinstance(nh_e, int) and nh_e > -1: + self.__nh_e = nh_e + else: + raise Exception('Invalid nh_e value, expected nonnegative integer. Exiting.') + + @nphi.setter + def nphi(self, nphi): + if isinstance(nphi, int) and nphi > -1: + self.__nphi = nphi + else: + raise Exception('Invalid nphi value, expected nonnegative integer. Exiting.') + + @nphi_e.setter + def nphi_e(self, nphi_e): + if isinstance(nphi_e, int) and nphi_e > -1: + self.__nphi_e = nphi_e + else: + raise Exception('Invalid nphi_e value, expected nonnegative integer. Exiting.') + + @nbx.setter + def nbx(self, nbx): + if isinstance(nbx, int) and nbx > -1: + self.__nbx = nbx + else: + raise Exception('Invalid nbx value, expected nonnegative integer. Exiting.') + + @nbxe_0.setter + def nbxe_0(self, nbxe_0): + if isinstance(nbxe_0, int) and nbxe_0 > -1: + self.__nbxe_0 = nbxe_0 + else: + raise Exception('Invalid nbxe_0 value, expected nonnegative integer. Exiting.') + + @nbx_0.setter + def nbx_0(self, nbx_0): + if isinstance(nbx_0, int) and nbx_0 > -1: + self.__nbx_0 = nbx_0 + else: + raise Exception('Invalid nbx_0 value, expected nonnegative integer. Exiting.') + + @nbx_e.setter + def nbx_e(self, nbx_e): + if isinstance(nbx_e, int) and nbx_e > -1: + self.__nbx_e = nbx_e + else: + raise Exception('Invalid nbx_e value, expected nonnegative integer. Exiting.') + + @nbu.setter + def nbu(self, nbu): + if isinstance(nbu, int) and nbu > -1: + self.__nbu = nbu + else: + raise Exception('Invalid nbu value, expected nonnegative integer. Exiting.') + + @nsbx.setter + def nsbx(self, nsbx): + if isinstance(nsbx, int) and nsbx > -1: + self.__nsbx = nsbx + else: + raise Exception('Invalid nsbx value, expected nonnegative integer. Exiting.') + + @nsbx_e.setter + def nsbx_e(self, nsbx_e): + if isinstance(nsbx_e, int) and nsbx_e > -1: + self.__nsbx_e = nsbx_e + else: + raise Exception('Invalid nsbx_e value, expected nonnegative integer. Exiting.') + + @nsbu.setter + def nsbu(self, nsbu): + if isinstance(nsbu, int) and nsbu > -1: + self.__nsbu = nsbu + else: + raise Exception('Invalid nsbu value, expected nonnegative integer. Exiting.') + + @nsg.setter + def nsg(self, nsg): + if isinstance(nsg, int) and nsg > -1: + self.__nsg = nsg + else: + raise Exception('Invalid nsg value, expected nonnegative integer. Exiting.') + + @nsg_e.setter + def nsg_e(self, nsg_e): + if isinstance(nsg_e, int) and nsg_e > -1: + self.__nsg_e = nsg_e + else: + raise Exception('Invalid nsg_e value, expected nonnegative integer. Exiting.') + + @nsh.setter + def nsh(self, nsh): + if isinstance(nsh, int) and nsh > -1: + self.__nsh = nsh + else: + raise Exception('Invalid nsh value, expected nonnegative integer. Exiting.') + + @nsh_e.setter + def nsh_e(self, nsh_e): + if isinstance(nsh_e, int) and nsh_e > -1: + self.__nsh_e = nsh_e + else: + raise Exception('Invalid nsh_e value, expected nonnegative integer. Exiting.') + + @nsphi.setter + def nsphi(self, nsphi): + if isinstance(nsphi, int) and nsphi > -1: + self.__nsphi = nsphi + else: + raise Exception('Invalid nsphi value, expected nonnegative integer. Exiting.') + + @nsphi_e.setter + def nsphi_e(self, nsphi_e): + if isinstance(nsphi_e, int) and nsphi_e > -1: + self.__nsphi_e = nsphi_e + else: + raise Exception('Invalid nsphi_e value, expected nonnegative integer. Exiting.') + + @ns.setter + def ns(self, ns): + if isinstance(ns, int) and ns > -1: + self.__ns = ns + else: + raise Exception('Invalid ns value, expected nonnegative integer. Exiting.') + + @ns_e.setter + def ns_e(self, ns_e): + if isinstance(ns_e, int) and ns_e > -1: + self.__ns_e = ns_e + else: + raise Exception('Invalid ns_e value, expected nonnegative integer. Exiting.') + + @ng.setter + def ng(self, ng): + if isinstance(ng, int) and ng > -1: + self.__ng = ng + else: + raise Exception('Invalid ng value, expected nonnegative integer. Exiting.') + + @ng_e.setter + def ng_e(self, ng_e): + if isinstance(ng_e, int) and ng_e > -1: + self.__ng_e = ng_e + else: + raise Exception('Invalid ng_e value, expected nonnegative integer. Exiting.') + + @N.setter + def N(self, N): + if isinstance(N, int) and N > 0: + self.__N = N + else: + raise Exception('Invalid N value, expected positive integer. Exiting.') + + def set(self, attr, value): + setattr(self, attr, value) + + +class AcadosOcpCost: + """ + Class containing the numerical data of the cost: + + In case of LINEAR_LS: + stage cost is + :math:`l(x,u,z) = || V_x \, x + V_u \, u + V_z \, z - y_\\text{ref}||^2_W`, + terminal cost is + :math:`m(x) = || V^e_x \, x - y_\\text{ref}^e||^2_{W^e}` + + In case of NONLINEAR_LS: + stage cost is + :math:`l(x,u,z) = || y(x,u,z) - y_\\text{ref}||^2_W`, + terminal cost is + :math:`m(x) = || y^e(x) - y_\\text{ref}^e||^2_{W^e}` + """ + def __init__(self): + # initial stage + self.__cost_type_0 = None + self.__W_0 = None + self.__Vx_0 = None + self.__Vu_0 = None + self.__Vz_0 = None + self.__yref_0 = None + self.__cost_ext_fun_type_0 = 'casadi' + # Lagrange term + self.__cost_type = 'LINEAR_LS' # cost type + self.__W = np.zeros((0,0)) + self.__Vx = np.zeros((0,0)) + self.__Vu = np.zeros((0,0)) + self.__Vz = np.zeros((0,0)) + self.__yref = np.array([]) + self.__Zl = np.array([]) + self.__Zu = np.array([]) + self.__zl = np.array([]) + self.__zu = np.array([]) + self.__cost_ext_fun_type = 'casadi' + # Mayer term + self.__cost_type_e = 'LINEAR_LS' + self.__W_e = np.zeros((0,0)) + self.__Vx_e = np.zeros((0,0)) + self.__yref_e = np.array([]) + self.__Zl_e = np.array([]) + self.__Zu_e = np.array([]) + self.__zl_e = np.array([]) + self.__zu_e = np.array([]) + self.__cost_ext_fun_type_e = 'casadi' + + # initial stage + @property + def cost_type_0(self): + """Cost type at initial shooting node (0) + -- string in {EXTERNAL, LINEAR_LS, NONLINEAR_LS} or :code:`None`. + Default: :code:`None`. + + .. note:: Cost at initial stage is the same as for intermediate shooting nodes if not set differently explicitly. + + .. note:: If :py:attr:`cost_type_0` is set to :code:`None` values in :py:attr:`W_0`, :py:attr:`Vx_0`, :py:attr:`Vu_0`, :py:attr:`Vz_0` and :py:attr:`yref_0` are ignored (set to :code:`None`). + """ + return self.__cost_type_0 + + @property + def W_0(self): + """:math:`W_0` - weight matrix at initial shooting node (0). + Default: :code:`None`. + """ + return self.__W_0 + + @property + def Vx_0(self): + """:math:`V_x^0` - x matrix coefficient at initial shooting node (0). + Default: :code:`None`. + """ + return self.__Vx_0 + + @property + def Vu_0(self): + """:math:`V_u^0` - u matrix coefficient at initial shooting node (0). + Default: :code:`None`. + """ + return self.__Vu_0 + + @property + def Vz_0(self): + """:math:`V_z^0` - z matrix coefficient at initial shooting node (0). + Default: :code:`None`. + """ + return self.__Vz_0 + + @property + def yref_0(self): + """:math:`y_\\text{ref}^0` - reference at initial shooting node (0). + Default: :code:`None`. + """ + return self.__yref_0 + + @property + def cost_ext_fun_type_0(self): + """Type of external function for cost at initial shooting node (0) + -- string in {casadi, generic} or :code:`None` + Default: :code:'casadi'. + + .. note:: Cost at initial stage is the same as for intermediate shooting nodes if not set differently explicitly. + """ + return self.__cost_ext_fun_type_0 + + @yref_0.setter + def yref_0(self, yref_0): + if isinstance(yref_0, np.ndarray): + self.__yref_0 = yref_0 + else: + raise Exception('Invalid yref_0 value, expected numpy array. Exiting.') + + @W_0.setter + def W_0(self, W_0): + if isinstance(W_0, np.ndarray) and len(W_0.shape) == 2: + self.__W_0 = W_0 + else: + raise Exception('Invalid cost W_0 value. ' \ + + 'Should be 2 dimensional numpy array. Exiting.') + + @Vx_0.setter + def Vx_0(self, Vx_0): + if isinstance(Vx_0, np.ndarray) and len(Vx_0.shape) == 2: + self.__Vx_0 = Vx_0 + else: + raise Exception('Invalid cost Vx_0 value. ' \ + + 'Should be 2 dimensional numpy array. Exiting.') + + @Vu_0.setter + def Vu_0(self, Vu_0): + if isinstance(Vu_0, np.ndarray) and len(Vu_0.shape) == 2: + self.__Vu_0 = Vu_0 + else: + raise Exception('Invalid cost Vu_0 value. ' \ + + 'Should be 2 dimensional numpy array. Exiting.') + + @Vz_0.setter + def Vz_0(self, Vz_0): + if isinstance(Vz_0, np.ndarray) and len(Vz_0.shape) == 2: + self.__Vz_0 = Vz_0 + else: + raise Exception('Invalid cost Vz_0 value. ' \ + + 'Should be 2 dimensional numpy array. Exiting.') + + @cost_ext_fun_type_0.setter + def cost_ext_fun_type_0(self, cost_ext_fun_type_0): + if cost_ext_fun_type_0 in ['casadi', 'generic']: + self.__cost_ext_fun_type_0 = cost_ext_fun_type_0 + else: + raise Exception('Invalid cost_ext_fun_type_0 value, expected numpy array. Exiting.') + + # Lagrange term + @property + def cost_type(self): + """ + Cost type at intermediate shooting nodes (1 to N-1) + -- string in {EXTERNAL, LINEAR_LS, NONLINEAR_LS}. + Default: 'LINEAR_LS'. + """ + return self.__cost_type + + @property + def W(self): + """:math:`W` - weight matrix at intermediate shooting nodes (1 to N-1). + Default: :code:`np.zeros((0,0))`. + """ + return self.__W + + @property + def Vx(self): + """:math:`V_x` - x matrix coefficient at intermediate shooting nodes (1 to N-1). + Default: :code:`np.zeros((0,0))`. + """ + return self.__Vx + + @property + def Vu(self): + """:math:`V_u` - u matrix coefficient at intermediate shooting nodes (1 to N-1). + Default: :code:`np.zeros((0,0))`. + """ + return self.__Vu + + @property + def Vz(self): + """:math:`V_z` - z matrix coefficient at intermediate shooting nodes (1 to N-1). + Default: :code:`np.zeros((0,0))`. + """ + return self.__Vz + + @property + def yref(self): + """:math:`y_\\text{ref}` - reference at intermediate shooting nodes (1 to N-1). + Default: :code:`np.array([])`. + """ + return self.__yref + + @property + def Zl(self): + """:math:`Z_l` - diagonal of Hessian wrt lower slack at intermediate shooting nodes (1 to N-1). + Default: :code:`np.array([])`. + """ + return self.__Zl + + @property + def Zu(self): + """:math:`Z_u` - diagonal of Hessian wrt upper slack at intermediate shooting nodes (1 to N-1). + Default: :code:`np.array([])`. + """ + return self.__Zu + + @property + def zl(self): + """:math:`z_l` - gradient wrt lower slack at intermediate shooting nodes (1 to N-1). + Default: :code:`np.array([])`. + """ + return self.__zl + + @property + def zu(self): + """:math:`z_u` - gradient wrt upper slack at intermediate shooting nodes (1 to N-1). + Default: :code:`np.array([])`. + """ + return self.__zu + + @property + def cost_ext_fun_type(self): + """Type of external function for cost at intermediate shooting nodes (1 to N-1). + -- string in {casadi, generic} + Default: :code:'casadi'. + """ + return self.__cost_ext_fun_type + + @cost_type.setter + def cost_type(self, cost_type): + cost_types = ('LINEAR_LS', 'NONLINEAR_LS', 'EXTERNAL') + if cost_type in cost_types: + self.__cost_type = cost_type + else: + raise Exception('Invalid cost_type value. Exiting.') + + @cost_type_0.setter + def cost_type_0(self, cost_type_0): + cost_types = ('LINEAR_LS', 'NONLINEAR_LS', 'EXTERNAL') + if cost_type_0 in cost_types: + self.__cost_type_0 = cost_type_0 + else: + raise Exception('Invalid cost_type_0 value. Exiting.') + + @W.setter + def W(self, W): + if isinstance(W, np.ndarray) and len(W.shape) == 2: + self.__W = W + else: + raise Exception('Invalid cost W value. ' \ + + 'Should be 2 dimensional numpy array. Exiting.') + + + @Vx.setter + def Vx(self, Vx): + if isinstance(Vx, np.ndarray) and len(Vx.shape) == 2: + self.__Vx = Vx + else: + raise Exception('Invalid cost Vx value. ' \ + + 'Should be 2 dimensional numpy array. Exiting.') + + @Vu.setter + def Vu(self, Vu): + if isinstance(Vu, np.ndarray) and len(Vu.shape) == 2: + self.__Vu = Vu + else: + raise Exception('Invalid cost Vu value. ' \ + + 'Should be 2 dimensional numpy array. Exiting.') + + @Vz.setter + def Vz(self, Vz): + if isinstance(Vz, np.ndarray) and len(Vz.shape) == 2: + self.__Vz = Vz + else: + raise Exception('Invalid cost Vz value. ' \ + + 'Should be 2 dimensional numpy array. Exiting.') + + @yref.setter + def yref(self, yref): + if isinstance(yref, np.ndarray): + self.__yref = yref + else: + raise Exception('Invalid yref value, expected numpy array. Exiting.') + + @Zl.setter + def Zl(self, Zl): + if isinstance(Zl, np.ndarray): + self.__Zl = Zl + else: + raise Exception('Invalid Zl value, expected numpy array. Exiting.') + + @Zu.setter + def Zu(self, Zu): + if isinstance(Zu, np.ndarray): + self.__Zu = Zu + else: + raise Exception('Invalid Zu value, expected numpy array. Exiting.') + + @zl.setter + def zl(self, zl): + if isinstance(zl, np.ndarray): + self.__zl = zl + else: + raise Exception('Invalid zl value, expected numpy array. Exiting.') + + @zu.setter + def zu(self, zu): + if isinstance(zu, np.ndarray): + self.__zu = zu + else: + raise Exception('Invalid zu value, expected numpy array. Exiting.') + + @cost_ext_fun_type.setter + def cost_ext_fun_type(self, cost_ext_fun_type): + if cost_ext_fun_type in ['casadi', 'generic']: + self.__cost_ext_fun_type = cost_ext_fun_type + else: + raise Exception('Invalid cost_ext_fun_type value, expected numpy array. Exiting.') + + # Mayer term + @property + def cost_type_e(self): + """ + Cost type at terminal shooting node (N) + -- string in {EXTERNAL, LINEAR_LS, NONLINEAR_LS}. + Default: 'LINEAR_LS'. + """ + return self.__cost_type_e + + @property + def W_e(self): + """:math:`W_e` - weight matrix at terminal shooting node (N). + Default: :code:`np.zeros((0,0))`. + """ + return self.__W_e + + @property + def Vx_e(self): + """:math:`V_x^e` - x matrix coefficient for cost at terminal shooting node (N). + Default: :code:`np.zeros((0,0))`. + """ + return self.__Vx_e + + @property + def yref_e(self): + """:math:`y_\\text{ref}^e` - cost reference at terminal shooting node (N). + Default: :code:`np.array([])`. + """ + return self.__yref_e + + @property + def Zl_e(self): + """:math:`Z_l^e` - diagonal of Hessian wrt lower slack at terminal shooting node (N). + Default: :code:`np.array([])`. + """ + return self.__Zl_e + + @property + def Zu_e(self): + """:math:`Z_u^e` - diagonal of Hessian wrt upper slack at terminal shooting node (N). + Default: :code:`np.array([])`. + """ + return self.__Zu_e + + @property + def zl_e(self): + """:math:`z_l^e` - gradient wrt lower slack at terminal shooting node (N). + Default: :code:`np.array([])`. + """ + return self.__zl_e + + @property + def zu_e(self): + """:math:`z_u^e` - gradient wrt upper slack at terminal shooting node (N). + Default: :code:`np.array([])`. + """ + return self.__zu_e + + @property + def cost_ext_fun_type_e(self): + """Type of external function for cost at intermediate shooting nodes (1 to N-1). + -- string in {casadi, generic} + Default: :code:'casadi'. + """ + return self.__cost_ext_fun_type_e + + @cost_type_e.setter + def cost_type_e(self, cost_type_e): + cost_types = ('LINEAR_LS', 'NONLINEAR_LS', 'EXTERNAL') + + if cost_type_e in cost_types: + self.__cost_type_e = cost_type_e + else: + raise Exception('Invalid cost_type_e value. Exiting.') + + @W_e.setter + def W_e(self, W_e): + if isinstance(W_e, np.ndarray) and len(W_e.shape) == 2: + self.__W_e = W_e + else: + raise Exception('Invalid cost W_e value. ' \ + + 'Should be 2 dimensional numpy array. Exiting.') + + @Vx_e.setter + def Vx_e(self, Vx_e): + if isinstance(Vx_e, np.ndarray) and len(Vx_e.shape) == 2: + self.__Vx_e = Vx_e + else: + raise Exception('Invalid cost Vx_e value. ' \ + + 'Should be 2 dimensional numpy array. Exiting.') + + @yref_e.setter + def yref_e(self, yref_e): + if isinstance(yref_e, np.ndarray): + self.__yref_e = yref_e + else: + raise Exception('Invalid yref_e value, expected numpy array. Exiting.') + + @Zl_e.setter + def Zl_e(self, Zl_e): + if isinstance(Zl_e, np.ndarray): + self.__Zl_e = Zl_e + else: + raise Exception('Invalid Zl_e value, expected numpy array. Exiting.') + + @Zu_e.setter + def Zu_e(self, Zu_e): + if isinstance(Zu_e, np.ndarray): + self.__Zu_e = Zu_e + else: + raise Exception('Invalid Zu_e value, expected numpy array. Exiting.') + + @zl_e.setter + def zl_e(self, zl_e): + if isinstance(zl_e, np.ndarray): + self.__zl_e = zl_e + else: + raise Exception('Invalid zl_e value, expected numpy array. Exiting.') + + @zu_e.setter + def zu_e(self, zu_e): + if isinstance(zu_e, np.ndarray): + self.__zu_e = zu_e + else: + raise Exception('Invalid zu_e value, expected numpy array. Exiting.') + + @cost_ext_fun_type_e.setter + def cost_ext_fun_type_e(self, cost_ext_fun_type_e): + if cost_ext_fun_type_e in ['casadi', 'generic']: + self.__cost_ext_fun_type_e = cost_ext_fun_type_e + else: + raise Exception('Invalid cost_ext_fun_type_e value, expected numpy array. Exiting.') + + def set(self, attr, value): + setattr(self, attr, value) + + +def print_J_to_idx_note(): + print("NOTE: J* matrix is converted to zero based vector idx* vector, which is returned here.") + + +class AcadosOcpConstraints: + """ + class containing the description of the constraints + """ + def __init__(self): + self.__constr_type = 'BGH' + self.__constr_type_e = 'BGH' + # initial x + self.__lbx_0 = np.array([]) + self.__ubx_0 = np.array([]) + self.__idxbx_0 = np.array([]) + self.__idxbxe_0 = np.array([]) + # state bounds + self.__lbx = np.array([]) + self.__ubx = np.array([]) + self.__idxbx = np.array([]) + # bounds on x at shooting node N + self.__lbx_e = np.array([]) + self.__ubx_e = np.array([]) + self.__idxbx_e = np.array([]) + # bounds on u + self.__lbu = np.array([]) + self.__ubu = np.array([]) + self.__idxbu = np.array([]) + # polytopic constraints + self.__lg = np.array([]) + self.__ug = np.array([]) + self.__D = np.zeros((0,0)) + self.__C = np.zeros((0,0)) + # polytopic constraints at shooting node N + self.__C_e = np.zeros((0,0)) + self.__lg_e = np.array([]) + self.__ug_e = np.array([]) + # nonlinear constraints + self.__lh = np.array([]) + self.__uh = np.array([]) + # nonlinear constraints at shooting node N + self.__uh_e = np.array([]) + self.__lh_e = np.array([]) + # convex-over-nonlinear constraints + self.__lphi = np.array([]) + self.__uphi = np.array([]) + # nonlinear constraints at shooting node N + self.__uphi_e = np.array([]) + self.__lphi_e = np.array([]) + # SLACK BOUNDS + # soft bounds on x + self.__lsbx = np.array([]) + self.__usbx = np.array([]) + self.__idxsbx = np.array([]) + # soft bounds on u + self.__lsbu = np.array([]) + self.__usbu = np.array([]) + self.__idxsbu = np.array([]) + # soft bounds on x at shooting node N + self.__lsbx_e = np.array([]) + self.__usbx_e = np.array([]) + self.__idxsbx_e= np.array([]) + # soft bounds on general linear constraints + self.__lsg = np.array([]) + self.__usg = np.array([]) + self.__idxsg = np.array([]) + # soft bounds on nonlinear constraints + self.__lsh = np.array([]) + self.__ush = np.array([]) + self.__idxsh = np.array([]) + # soft bounds on nonlinear constraints + self.__lsphi = np.array([]) + self.__usphi = np.array([]) + self.__idxsphi = np.array([]) + # soft bounds on general linear constraints at shooting node N + self.__lsg_e = np.array([]) + self.__usg_e = np.array([]) + self.__idxsg_e = np.array([]) + # soft bounds on nonlinear constraints at shooting node N + self.__lsh_e = np.array([]) + self.__ush_e = np.array([]) + self.__idxsh_e = np.array([]) + # soft bounds on nonlinear constraints at shooting node N + self.__lsphi_e = np.array([]) + self.__usphi_e = np.array([]) + self.__idxsphi_e = np.array([]) + + + # types + @property + def constr_type(self): + """Constraints type for shooting nodes (0 to N-1). string in {BGH, BGP}. + Default: BGH; BGP is for convex over nonlinear.""" + return self.__constr_type + + @property + def constr_type_e(self): + """Constraints type for terminal shooting node N. string in {BGH, BGP}. + Default: BGH; BGP is for convex over nonlinear.""" + return self.__constr_type_e + + # initial bounds on x + @property + def lbx_0(self): + """:math:`\\underline{x_0}` - lower bounds on x at initial stage 0. + Type: :code:`np.ndarray`; default: :code:`np.array([])`.""" + return self.__lbx_0 + + @property + def ubx_0(self): + """:math:`\\bar{x_0}` - upper bounds on x at initial stage 0. + Type: :code:`np.ndarray`; default: :code:`np.array([])`""" + return self.__ubx_0 + + @property + def Jbx_0(self): + """:math:`J_{bx,0}` - matrix coefficient for bounds on x at initial stage 0. + Translated internally to :py:attr:`idxbx_0`""" + print_J_to_idx_note() + return self.__idxbx_0 + + @property + def idxbx_0(self): + """Indices of bounds on x at initial stage 0 + -- can be set automatically via x0. + Can be set by using :py:attr:`Jbx_0`. + Type: :code:`np.ndarray`; default: :code:`np.array([])`""" + return self.__idxbx_0 + + @property + def idxbxe_0(self): + """Indices of bounds on x0 that are equalities -- can be set automatically via :py:attr:`x0`. + Type: :code:`np.ndarray`; default: :code:`np.array([])`""" + return self.__idxbxe_0 + + # bounds on x + @property + def lbx(self): + """:math:`\\underline{x}` - lower bounds on x at intermediate shooting nodes (1 to N-1). + Type: :code:`np.ndarray`; default: :code:`np.array([])`""" + return self.__lbx + + @property + def ubx(self): + """:math:`\\bar{x}` - upper bounds on x at intermediate shooting nodes (1 to N-1). + Type: :code:`np.ndarray`; default: :code:`np.array([])`""" + return self.__ubx + + @property + def idxbx(self): + """indices of bounds on x (defines :math:`J_{bx}`) at intermediate shooting nodes (1 to N-1). + Can be set by using :py:attr:`Jbx`. + Type: :code:`np.ndarray`; default: :code:`np.array([])`""" + return self.__idxbx + + @property + def Jbx(self): + """:math:`J_{bx}` - matrix coefficient for bounds on x + at intermediate shooting nodes (1 to N-1). + Translated internally into :py:attr:`idxbx`.""" + print_J_to_idx_note() + return self.__idxbx + + # bounds on x at shooting node N + @property + def lbx_e(self): + """:math:`\\underline{x}^e` - lower bounds on x at terminal shooting node N. + Type: :code:`np.ndarray`; default: :code:`np.array([])`""" + return self.__lbx_e + + @property + def ubx_e(self): + """:math:`\\bar{x}^e` - upper bounds on x at terminal shooting node N. + Type: :code:`np.ndarray`; default: :code:`np.array([])`""" + return self.__ubx_e + + @property + def idxbx_e(self): + """Indices for bounds on x at terminal shooting node N (defines :math:`J_{bx}^e`). + Can be set by using :py:attr:`Jbx_e`. + Type: :code:`np.ndarray`; default: :code:`np.array([])`""" + return self.__idxbx_e + + @property + def Jbx_e(self): + """:math:`J_{bx}^e` matrix coefficient for bounds on x at terminal shooting node N. + Translated internally into :py:attr:`idxbx_e`.""" + print_J_to_idx_note() + return self.__idxbx_e + + # bounds on u + @property + def lbu(self): + """:math:`\\underline{u}` - lower bounds on u at shooting nodes (0 to N-1). + Type: :code:`np.ndarray`; default: :code:`np.array([])` + """ + return self.__lbu + + @property + def ubu(self): + """:math:`\\bar{u}` - upper bounds on u at shooting nodes (0 to N-1). + Type: :code:`np.ndarray`; default: :code:`np.array([])` + """ + return self.__ubu + + @property + def idxbu(self): + """Indices of bounds on u (defines :math:`J_{bu}`) at shooting nodes (0 to N-1). + Can be set by using :py:attr:`Jbu`. + Type: :code:`np.ndarray`; default: :code:`np.array([])` + """ + return self.__idxbu + + @property + def Jbu(self): + """:math:`J_{bu}` - matrix coefficient for bounds on u at shooting nodes (0 to N-1). + Translated internally to :py:attr:`idxbu`. + """ + print_J_to_idx_note() + return self.__idxbu + + # polytopic constraints + @property + def C(self): + """:math:`C` - C matrix in :math:`\\underline{g} \\leq D \, u + C \, x \\leq \\bar{g}` + at shooting nodes (0 to N-1). + Type: :code:`np.ndarray`; default: :code:`np.array((0,0))`. + """ + return self.__C + + @property + def D(self): + """:math:`D` - D matrix in :math:`\\underline{g} \\leq D \, u + C \, x \\leq \\bar{g}` + at shooting nodes (0 to N-1). + Type: :code:`np.ndarray`; default: :code:`np.array((0,0))` + """ + return self.__D + + @property + def lg(self): + """:math:`\\underline{g}` - lower bound for general polytopic inequalities + at shooting nodes (0 to N-1). + Type: :code:`np.ndarray`; default: :code:`np.array([])` + """ + return self.__lg + + @property + def ug(self): + """:math:`\\bar{g}` - upper bound for general polytopic inequalities + at shooting nodes (0 to N-1). + Type: :code:`np.ndarray`; default: :code:`np.array([])`. + """ + return self.__ug + + # polytopic constraints at shooting node N + @property + def C_e(self): + """:math:`C^e` - C matrix at terminal shooting node N. + Type: :code:`np.ndarray`; default: :code:`np.array((0,0))`. + """ + return self.__C_e + + @property + def lg_e(self): + """:math:`\\underline{g}^e` - lower bound on general polytopic inequalities + at terminal shooting node N. + Type: :code:`np.ndarray`; default: :code:`np.array([])`. + """ + return self.__lg_e + + @property + def ug_e(self): + """:math:`\\bar{g}^e` - upper bound on general polytopic inequalities + at terminal shooting node N. + Type: :code:`np.ndarray`; default: :code:`np.array([])`. + """ + return self.__ug_e + + + # nonlinear constraints + @property + def lh(self): + """:math:`\\underline{h}` - lower bound for nonlinear inequalities + at shooting nodes (0 to N-1). + Type: :code:`np.ndarray`; default: :code:`np.array([])`. + """ + return self.__lh + + @property + def uh(self): + """:math:`\\bar{h}` - upper bound for nonlinear inequalities + at shooting nodes (0 to N-1). + Type: :code:`np.ndarray`; default: :code:`np.array([])`. + """ + return self.__uh + + # nonlinear constraints at shooting node N + @property + def lh_e(self): + """:math:`\\underline{h}^e` - lower bound on nonlinear inequalities + at terminal shooting node N. + Type: :code:`np.ndarray`; default: :code:`np.array([])`. + """ + return self.__lh_e + + @property + def uh_e(self): + """:math:`\\bar{h}^e` - upper bound on nonlinear inequalities + at terminal shooting node N. + Type: :code:`np.ndarray`; default: :code:`np.array([])`. + """ + return self.__uh_e + + # convex-over-nonlinear constraints + @property + def lphi(self): + """:math:`\\underline{\phi}` - lower bound for convex-over-nonlinear inequalities + at shooting nodes (0 to N-1). + Type: :code:`np.ndarray`; default: :code:`np.array([])`. + """ + return self.__lphi + + @property + def uphi(self): + """:math:`\\bar{\phi}` - upper bound for convex-over-nonlinear inequalities + at shooting nodes (0 to N-1). + Type: :code:`np.ndarray`; default: :code:`np.array([])`. + """ + return self.__uphi + + # convex-over-nonlinear constraints at shooting node N + @property + def lphi_e(self): + """:math:`\\underline{\phi}^e` - lower bound on convex-over-nonlinear inequalities + at terminal shooting node N. + Type: :code:`np.ndarray`; default: :code:`np.array([])`. + """ + return self.__lphi_e + + @property + def uphi_e(self): + """:math:`\\bar{\phi}^e` - upper bound on convex-over-nonlinear inequalities + at terminal shooting node N. + Type: :code:`np.ndarray`; default: :code:`np.array([])`. + """ + return self.__uphi_e + + + # SLACK bounds + # soft bounds on x + @property + def lsbx(self): + """Lower bounds on slacks corresponding to soft lower bounds on x + at stages (1 to N-1); + not required - zeros by default""" + return self.__lsbx + + @property + def usbx(self): + """Lower bounds on slacks corresponding to soft upper bounds on x + at stages (1 to N-1); + not required - zeros by default""" + return self.__usbx + + @property + def idxsbx(self): + """Indices of soft bounds on x within the indices of bounds on x + at stages (1 to N-1). + Can be set by using :py:attr:`Jsbx`. + Type: :code:`np.ndarray`; default: :code:`np.array([])`""" + return self.__idxsbx + + @property + def Jsbx(self): + """:math:`J_{sbx}` - matrix coefficient for soft bounds on x + at stages (1 to N-1); + Translated internally into :py:attr:`idxsbx`.""" + print_J_to_idx_note() + return self.__idxsbx + + # soft bounds on u + @property + def lsbu(self): + """Lower bounds on slacks corresponding to soft lower bounds on u + at stages (0 to N-1). + Not required - zeros by default.""" + return self.__lsbu + + @property + def usbu(self): + """Lower bounds on slacks corresponding to soft upper bounds on u + at stages (0 to N-1); + not required - zeros by default""" + return self.__usbu + + @property + def idxsbu(self): + """Indices of soft bounds on u within the indices of bounds on u + at stages (0 to N-1). + Can be set by using :py:attr:`Jsbu`. + Type: :code:`np.ndarray`; default: :code:`np.array([])`""" + return self.__idxsbu + + @property + def Jsbu(self): + """:math:`J_{sbu}` - matrix coefficient for soft bounds on u + at stages (0 to N-1); + internally translated into :py:attr:`idxsbu`""" + print_J_to_idx_note() + return self.__idxsbu + + # soft bounds on x at shooting node N + @property + def lsbx_e(self): + """Lower bounds on slacks corresponding to soft lower bounds on x at shooting node N. + Not required - zeros by default""" + return self.__lsbx_e + + @property + def usbx_e(self): + """Lower bounds on slacks corresponding to soft upper bounds on x at shooting node N. + Not required - zeros by default""" + return self.__usbx_e + + @property + def idxsbx_e(self): + """Indices of soft bounds on x at shooting node N, within the indices of bounds on x at terminal shooting node N. + Can be set by using :py:attr:`Jsbx_e`. + Type: :code:`np.ndarray`; default: :code:`np.array([])`""" + return self.__idxsbx_e + + @property + def Jsbx_e(self): + """:math:`J_{sbx}^e` - matrix coefficient for soft bounds on x at terminal shooting node N. + Translated internally to :py:attr:`idxsbx_e`""" + print_J_to_idx_note() + return self.__idxsbx_e + + # soft general linear constraints + @property + def lsg(self): + """Lower bounds on slacks corresponding to soft lower bounds for general linear constraints + at stages (0 to N-1). + Type: :code:`np.ndarray`; default: :code:`np.array([])` + """ + return self.__lsg + + @property + def usg(self): + """Lower bounds on slacks corresponding to soft upper bounds for general linear constraints. + Not required - zeros by default""" + return self.__usg + + @property + def idxsg(self): + """Indices of soft general linear constraints within the indices of general linear constraints. + Can be set by using :py:attr:`Jsg`. + Type: :code:`np.ndarray`; default: :code:`np.array([])`""" + return self.__idxsg + + @property + def Jsg(self): + """:math:`J_{sg}` - matrix coefficient for soft bounds on general linear constraints. + Translated internally to :py:attr:`idxsg`""" + print_J_to_idx_note() + return self.__idxsg + + # soft nonlinear constraints + @property + def lsh(self): + """Lower bounds on slacks corresponding to soft lower bounds for nonlinear constraints. + Not required - zeros by default""" + return self.__lsh + + @property + def ush(self): + """Lower bounds on slacks corresponding to soft upper bounds for nonlinear constraints. + Not required - zeros by default""" + return self.__ush + + @property + def idxsh(self): + """Indices of soft nonlinear constraints within the indices of nonlinear constraints. + Can be set by using :py:attr:`Jbx`. + Type: :code:`np.ndarray`; default: :code:`np.array([])`""" + return self.__idxsh + + @property + def Jsh(self): + """:math:`J_{sh}` - matrix coefficient for soft bounds on nonlinear constraints. + Translated internally to :py:attr:`idxsh`""" + print_J_to_idx_note() + return self.__idxsh + + # soft bounds on convex-over-nonlinear constraints + @property + def lsphi(self): + """Lower bounds on slacks corresponding to soft lower bounds for convex-over-nonlinear constraints. + Not required - zeros by default""" + return self.__lsphi + + @property + def usphi(self): + """Lower bounds on slacks corresponding to soft upper bounds for convex-over-nonlinear constraints. + Not required - zeros by default""" + return self.__usphi + + @property + def idxsphi(self): + """Indices of soft convex-over-nonlinear constraints within the indices of nonlinear constraints. + Can be set by using :py:attr:`Jsphi`. + Type: :code:`np.ndarray`; default: :code:`np.array([])`""" + return self.__idxsphi + + @property + def Jsphi(self): + """:math:`J_{s, \phi}` - matrix coefficient for soft bounds on convex-over-nonlinear constraints. + Translated internally into :py:attr:`idxsphi`.""" + print_J_to_idx_note() + return self.__idxsphi + + + # soft bounds on general linear constraints at shooting node N + @property + def lsg_e(self): + """Lower bounds on slacks corresponding to soft lower bounds for general linear constraints at shooting node N. + Not required - zeros by default""" + return self.__lsg_e + + @property + def usg_e(self): + """Lower bounds on slacks corresponding to soft upper bounds for general linear constraints at shooting node N. + Not required - zeros by default""" + return self.__usg_e + + @property + def idxsg_e(self): + """Indices of soft general linear constraints at shooting node N within the indices of general linear constraints at shooting node N. + Can be set by using :py:attr:`Jsg_e`.""" + return self.__idxsg_e + + @property + def Jsg_e(self): + """:math:`J_{s,h}^e` - matrix coefficient for soft bounds on general linear constraints at terminal shooting node N. + Translated internally to :py:attr:`idxsg_e`""" + print_J_to_idx_note() + return self.__idxsg_e + + + # soft bounds on nonlinear constraints at shooting node N + @property + def lsh_e(self): + """Lower bounds on slacks corresponding to soft lower bounds for nonlinear constraints at terminal shooting node N. + Not required - zeros by default""" + return self.__lsh_e + + @property + def ush_e(self): + """Lower bounds on slacks corresponding to soft upper bounds for nonlinear constraints at terminal shooting node N. + Not required - zeros by default""" + return self.__ush_e + + @property + def idxsh_e(self): + """Indices of soft nonlinear constraints at shooting node N within the indices of nonlinear constraints at terminal shooting node N. + Can be set by using :py:attr:`Jsh_e`.""" + return self.__idxsh_e + + @property + def Jsh_e(self): + """:math:`J_{s,h}^e` - matrix coefficient for soft bounds on nonlinear constraints at terminal shooting node N; fills :py:attr:`idxsh_e`""" + print_J_to_idx_note() + return self.__idxsh_e + + # soft bounds on convex-over-nonlinear constraints at shooting node N + @property + def lsphi_e(self): + """Lower bounds on slacks corresponding to soft lower bounds for convex-over-nonlinear constraints at terminal shooting node N. + Not required - zeros by default""" + return self.__lsphi_e + + @property + def usphi_e(self): + """Lower bounds on slacks corresponding to soft upper bounds for convex-over-nonlinear constraints at terminal shooting node N. + Not required - zeros by default""" + return self.__usphi_e + + @property + def idxsphi_e(self): + """Indices of soft nonlinear constraints at shooting node N within the indices of nonlinear constraints at terminal shooting node N. + Can be set by using :py:attr:`Jsphi_e`. + Type: :code:`np.ndarray`; default: :code:`np.array([])`""" + return self.__idxsphi_e + + @property + def Jsphi_e(self): + """:math:`J_{sh}^e` - matrix coefficient for soft bounds on convex-over-nonlinear constraints at shooting node N. + Translated internally to :py:attr:`idxsphi_e`""" + print_J_to_idx_note() + return self.__idxsphi_e + + @property + def x0(self): + """:math:`x_0 \\in \mathbb{R}^{n_x}` - initial state -- + Translated internally to :py:attr:`idxbx_0`, :py:attr:`lbx_0`, :py:attr:`ubx_0`, :py:attr:`idxbxe_0` """ + print("x0 is converted to lbx_0, ubx_0, idxbx_0") + print("idxbx_0: ", self.__idxbx_0) + print("lbx_0: ", self.__lbx_0) + print("ubx_0: ", self.__ubx_0) + print("idxbxe_0: ", self.__idxbxe_0) + return None + + # SETTERS + @constr_type.setter + def constr_type(self, constr_type): + constr_types = ('BGH', 'BGP') + if constr_type in constr_types: + self.__constr_type = constr_type + else: + raise Exception('Invalid constr_type value. Possible values are:\n\n' \ + + ',\n'.join(constr_types) + '.\n\nYou have: ' + constr_type + '.\n\nExiting.') + + @constr_type_e.setter + def constr_type_e(self, constr_type_e): + constr_types = ('BGH', 'BGP') + if constr_type_e in constr_types: + self.__constr_type_e = constr_type_e + else: + raise Exception('Invalid constr_type_e value. Possible values are:\n\n' \ + + ',\n'.join(constr_types) + '.\n\nYou have: ' + constr_type_e + '.\n\nExiting.') + + # initial x + @lbx_0.setter + def lbx_0(self, lbx_0): + if isinstance(lbx_0, np.ndarray): + self.__lbx_0 = lbx_0 + else: + raise Exception('Invalid lbx_0 value. Exiting.') + + @ubx_0.setter + def ubx_0(self, ubx_0): + if isinstance(ubx_0, np.ndarray): + self.__ubx_0 = ubx_0 + else: + raise Exception('Invalid ubx_0 value. Exiting.') + + @idxbx_0.setter + def idxbx_0(self, idxbx_0): + if isinstance(idxbx_0, np.ndarray): + self.__idxbx_0 = idxbx_0 + else: + raise Exception('Invalid idxbx_0 value. Exiting.') + + @Jbx_0.setter + def Jbx_0(self, Jbx_0): + if isinstance(Jbx_0, np.ndarray): + self.__idxbx_0 = J_to_idx(Jbx_0) + else: + raise Exception('Invalid Jbx_0 value. Exiting.') + + @idxbxe_0.setter + def idxbxe_0(self, idxbxe_0): + if isinstance(idxbxe_0, np.ndarray): + self.__idxbxe_0 = idxbxe_0 + else: + raise Exception('Invalid idxbxe_0 value. Exiting.') + + + @x0.setter + def x0(self, x0): + if isinstance(x0, np.ndarray): + self.__lbx_0 = x0 + self.__ubx_0 = x0 + self.__idxbx_0 = np.arange(x0.size) + self.__idxbxe_0 = np.arange(x0.size) + else: + raise Exception('Invalid x0 value. Exiting.') + + # bounds on x + @lbx.setter + def lbx(self, lbx): + if isinstance(lbx, np.ndarray): + self.__lbx = lbx + else: + raise Exception('Invalid lbx value. Exiting.') + + @ubx.setter + def ubx(self, ubx): + if isinstance(ubx, np.ndarray): + self.__ubx = ubx + else: + raise Exception('Invalid ubx value. Exiting.') + + @idxbx.setter + def idxbx(self, idxbx): + if isinstance(idxbx, np.ndarray): + self.__idxbx = idxbx + else: + raise Exception('Invalid idxbx value. Exiting.') + + @Jbx.setter + def Jbx(self, Jbx): + if isinstance(Jbx, np.ndarray): + self.__idxbx = J_to_idx(Jbx) + else: + raise Exception('Invalid Jbx value. Exiting.') + + # bounds on u + @lbu.setter + def lbu(self, lbu): + if isinstance(lbu, np.ndarray): + self.__lbu = lbu + else: + raise Exception('Invalid lbu value. Exiting.') + + @ubu.setter + def ubu(self, ubu): + if isinstance(ubu, np.ndarray): + self.__ubu = ubu + else: + raise Exception('Invalid ubu value. Exiting.') + + @idxbu.setter + def idxbu(self, idxbu): + if isinstance(idxbu, np.ndarray): + self.__idxbu = idxbu + else: + raise Exception('Invalid idxbu value. Exiting.') + + @Jbu.setter + def Jbu(self, Jbu): + if isinstance(Jbu, np.ndarray): + self.__idxbu = J_to_idx(Jbu) + else: + raise Exception('Invalid Jbu value. Exiting.') + + # bounds on x at shooting node N + @lbx_e.setter + def lbx_e(self, lbx_e): + if isinstance(lbx_e, np.ndarray): + self.__lbx_e = lbx_e + else: + raise Exception('Invalid lbx_e value. Exiting.') + + @ubx_e.setter + def ubx_e(self, ubx_e): + if isinstance(ubx_e, np.ndarray): + self.__ubx_e = ubx_e + else: + raise Exception('Invalid ubx_e value. Exiting.') + + @idxbx_e.setter + def idxbx_e(self, idxbx_e): + if isinstance(idxbx_e, np.ndarray): + self.__idxbx_e = idxbx_e + else: + raise Exception('Invalid idxbx_e value. Exiting.') + + @Jbx_e.setter + def Jbx_e(self, Jbx_e): + if isinstance(Jbx_e, np.ndarray): + self.__idxbx_e = J_to_idx(Jbx_e) + else: + raise Exception('Invalid Jbx_e value. Exiting.') + + # polytopic constraints + @D.setter + def D(self, D): + if isinstance(D, np.ndarray) and len(D.shape) == 2: + self.__D = D + else: + raise Exception('Invalid constraint D value.' \ + + 'Should be 2 dimensional numpy array. Exiting.') + + @C.setter + def C(self, C): + if isinstance(C, np.ndarray) and len(C.shape) == 2: + self.__C = C + else: + raise Exception('Invalid constraint C value.' \ + + 'Should be 2 dimensional numpy array. Exiting.') + + @lg.setter + def lg(self, lg): + if isinstance(lg, np.ndarray): + self.__lg = lg + else: + raise Exception('Invalid lg value. Exiting.') + + @ug.setter + def ug(self, ug): + if isinstance(ug, np.ndarray): + self.__ug = ug + else: + raise Exception('Invalid ug value. Exiting.') + + # polytopic constraints at shooting node N + @C_e.setter + def C_e(self, C_e): + if isinstance(C_e, np.ndarray) and len(C_e.shape) == 2: + self.__C_e = C_e + else: + raise Exception('Invalid constraint C_e value.' \ + + 'Should be 2 dimensional numpy array. Exiting.') + + @lg_e.setter + def lg_e(self, lg_e): + if isinstance(lg_e, np.ndarray): + self.__lg_e = lg_e + else: + raise Exception('Invalid lg_e value. Exiting.') + + @ug_e.setter + def ug_e(self, ug_e): + if isinstance(ug_e, np.ndarray): + self.__ug_e = ug_e + else: + raise Exception('Invalid ug_e value. Exiting.') + + # nonlinear constraints + @lh.setter + def lh(self, lh): + if isinstance(lh, np.ndarray): + self.__lh = lh + else: + raise Exception('Invalid lh value. Exiting.') + + @uh.setter + def uh(self, uh): + if isinstance(uh, np.ndarray): + self.__uh = uh + else: + raise Exception('Invalid uh value. Exiting.') + + # convex-over-nonlinear constraints + @lphi.setter + def lphi(self, lphi): + if isinstance(lphi, np.ndarray): + self.__lphi = lphi + else: + raise Exception('Invalid lphi value. Exiting.') + + @uphi.setter + def uphi(self, uphi): + if isinstance(uphi, np.ndarray): + self.__uphi = uphi + else: + raise Exception('Invalid uphi value. Exiting.') + + # nonlinear constraints at shooting node N + @lh_e.setter + def lh_e(self, lh_e): + if isinstance(lh_e, np.ndarray): + self.__lh_e = lh_e + else: + raise Exception('Invalid lh_e value. Exiting.') + + @uh_e.setter + def uh_e(self, uh_e): + if isinstance(uh_e, np.ndarray): + self.__uh_e = uh_e + else: + raise Exception('Invalid uh_e value. Exiting.') + + # convex-over-nonlinear constraints at shooting node N + @lphi_e.setter + def lphi_e(self, lphi_e): + if isinstance(lphi_e, np.ndarray): + self.__lphi_e = lphi_e + else: + raise Exception('Invalid lphi_e value. Exiting.') + + @uphi_e.setter + def uphi_e(self, uphi_e): + if isinstance(uphi_e, np.ndarray): + self.__uphi_e = uphi_e + else: + raise Exception('Invalid uphi_e value. Exiting.') + + # SLACK bounds + # soft bounds on x + @lsbx.setter + def lsbx(self, lsbx): + if isinstance(lsbx, np.ndarray): + self.__lsbx = lsbx + else: + raise Exception('Invalid lsbx value. Exiting.') + + @usbx.setter + def usbx(self, usbx): + if isinstance(usbx, np.ndarray): + self.__usbx = usbx + else: + raise Exception('Invalid usbx value. Exiting.') + + @idxsbx.setter + def idxsbx(self, idxsbx): + if isinstance(idxsbx, np.ndarray): + self.__idxsbx = idxsbx + else: + raise Exception('Invalid idxsbx value. Exiting.') + + @Jsbx.setter + def Jsbx(self, Jsbx): + if isinstance(Jsbx, np.ndarray): + self.__idxsbx = J_to_idx_slack(Jsbx) + else: + raise Exception('Invalid Jsbx value, expected numpy array. Exiting.') + + # soft bounds on u + @lsbu.setter + def lsbu(self, lsbu): + if isinstance(lsbu, np.ndarray): + self.__lsbu = lsbu + else: + raise Exception('Invalid lsbu value. Exiting.') + + @usbu.setter + def usbu(self, usbu): + if isinstance(usbu, np.ndarray): + self.__usbu = usbu + else: + raise Exception('Invalid usbu value. Exiting.') + + @idxsbu.setter + def idxsbu(self, idxsbu): + if isinstance(idxsbu, np.ndarray): + self.__idxsbu = idxsbu + else: + raise Exception('Invalid idxsbu value. Exiting.') + + @Jsbu.setter + def Jsbu(self, Jsbu): + if isinstance(Jsbu, np.ndarray): + self.__idxsbu = J_to_idx_slack(Jsbu) + else: + raise Exception('Invalid Jsbu value. Exiting.') + + # soft bounds on x at shooting node N + @lsbx_e.setter + def lsbx_e(self, lsbx_e): + if isinstance(lsbx_e, np.ndarray): + self.__lsbx_e = lsbx_e + else: + raise Exception('Invalid lsbx_e value. Exiting.') + + @usbx_e.setter + def usbx_e(self, usbx_e): + if isinstance(usbx_e, np.ndarray): + self.__usbx_e = usbx_e + else: + raise Exception('Invalid usbx_e value. Exiting.') + + @idxsbx_e.setter + def idxsbx_e(self, idxsbx_e): + if isinstance(idxsbx_e, np.ndarray): + self.__idxsbx_e = idxsbx_e + else: + raise Exception('Invalid idxsbx_e value. Exiting.') + + @Jsbx_e.setter + def Jsbx_e(self, Jsbx_e): + if isinstance(Jsbx_e, np.ndarray): + self.__idxsbx_e = J_to_idx_slack(Jsbx_e) + else: + raise Exception('Invalid Jsbx_e value. Exiting.') + + + # soft bounds on general linear constraints + @lsg.setter + def lsg(self, lsg): + if isinstance(lsg, np.ndarray): + self.__lsg = lsg + else: + raise Exception('Invalid lsg value. Exiting.') + + @usg.setter + def usg(self, usg): + if isinstance(usg, np.ndarray): + self.__usg = usg + else: + raise Exception('Invalid usg value. Exiting.') + + @idxsg.setter + def idxsg(self, idxsg): + if isinstance(idxsg, np.ndarray): + self.__idxsg = idxsg + else: + raise Exception('Invalid idxsg value. Exiting.') + + @Jsg.setter + def Jsg(self, Jsg): + if isinstance(Jsg, np.ndarray): + self.__idxsg = J_to_idx_slack(Jsg) + else: + raise Exception('Invalid Jsg value, expected numpy array. Exiting.') + + + # soft bounds on nonlinear constraints + @lsh.setter + def lsh(self, lsh): + if isinstance(lsh, np.ndarray): + self.__lsh = lsh + else: + raise Exception('Invalid lsh value. Exiting.') + + @ush.setter + def ush(self, ush): + if isinstance(ush, np.ndarray): + self.__ush = ush + else: + raise Exception('Invalid ush value. Exiting.') + + @idxsh.setter + def idxsh(self, idxsh): + if isinstance(idxsh, np.ndarray): + self.__idxsh = idxsh + else: + raise Exception('Invalid idxsh value. Exiting.') + + + @Jsh.setter + def Jsh(self, Jsh): + if isinstance(Jsh, np.ndarray): + self.__idxsh = J_to_idx_slack(Jsh) + else: + raise Exception('Invalid Jsh value, expected numpy array. Exiting.') + + # soft bounds on convex-over-nonlinear constraints + @lsphi.setter + def lsphi(self, lsphi): + if isinstance(lsphi, np.ndarray): + self.__lsphi = lsphi + else: + raise Exception('Invalid lsphi value. Exiting.') + + @usphi.setter + def usphi(self, usphi): + if isinstance(usphi, np.ndarray): + self.__usphi = usphi + else: + raise Exception('Invalid usphi value. Exiting.') + + @idxsphi.setter + def idxsphi(self, idxsphi): + if isinstance(idxsphi, np.ndarray): + self.__idxsphi = idxsphi + else: + raise Exception('Invalid idxsphi value. Exiting.') + + @Jsphi.setter + def Jsphi(self, Jsphi): + if isinstance(Jsphi, np.ndarray): + self.__idxsphi = J_to_idx_slack(Jsphi) + else: + raise Exception('Invalid Jsphi value, expected numpy array. Exiting.') + + # soft bounds on general linear constraints at shooting node N + @lsg_e.setter + def lsg_e(self, lsg_e): + if isinstance(lsg_e, np.ndarray): + self.__lsg_e = lsg_e + else: + raise Exception('Invalid lsg_e value. Exiting.') + + @usg_e.setter + def usg_e(self, usg_e): + if isinstance(usg_e, np.ndarray): + self.__usg_e = usg_e + else: + raise Exception('Invalid usg_e value. Exiting.') + + @idxsg_e.setter + def idxsg_e(self, idxsg_e): + if isinstance(idxsg_e, np.ndarray): + self.__idxsg_e = idxsg_e + else: + raise Exception('Invalid idxsg_e value. Exiting.') + + @Jsg_e.setter + def Jsg_e(self, Jsg_e): + if isinstance(Jsg_e, np.ndarray): + self.__idxsg_e = J_to_idx_slack(Jsg_e) + else: + raise Exception('Invalid Jsg_e value, expected numpy array. Exiting.') + + # soft bounds on nonlinear constraints at shooting node N + @lsh_e.setter + def lsh_e(self, lsh_e): + if isinstance(lsh_e, np.ndarray): + self.__lsh_e = lsh_e + else: + raise Exception('Invalid lsh_e value. Exiting.') + + @ush_e.setter + def ush_e(self, ush_e): + if isinstance(ush_e, np.ndarray): + self.__ush_e = ush_e + else: + raise Exception('Invalid ush_e value. Exiting.') + + @idxsh_e.setter + def idxsh_e(self, idxsh_e): + if isinstance(idxsh_e, np.ndarray): + self.__idxsh_e = idxsh_e + else: + raise Exception('Invalid idxsh_e value. Exiting.') + + @Jsh_e.setter + def Jsh_e(self, Jsh_e): + if isinstance(Jsh_e, np.ndarray): + self.__idxsh_e = J_to_idx_slack(Jsh_e) + else: + raise Exception('Invalid Jsh_e value, expected numpy array. Exiting.') + + + # soft bounds on convex-over-nonlinear constraints at shooting node N + @lsphi_e.setter + def lsphi_e(self, lsphi_e): + if isinstance(lsphi_e, np.ndarray): + self.__lsphi_e = lsphi_e + else: + raise Exception('Invalid lsphi_e value. Exiting.') + + @usphi_e.setter + def usphi_e(self, usphi_e): + if isinstance(usphi_e, np.ndarray): + self.__usphi_e = usphi_e + else: + raise Exception('Invalid usphi_e value. Exiting.') + + @idxsphi_e.setter + def idxsphi_e(self, idxsphi_e): + if isinstance(idxsphi_e, np.ndarray): + self.__idxsphi_e = idxsphi_e + else: + raise Exception('Invalid idxsphi_e value. Exiting.') + + @Jsphi_e.setter + def Jsphi_e(self, Jsphi_e): + if isinstance(Jsphi_e, np.ndarray): + self.__idxsphi_e = J_to_idx_slack(Jsphi_e) + else: + raise Exception('Invalid Jsphi_e value. Exiting.') + + def set(self, attr, value): + setattr(self, attr, value) + + +class AcadosOcpOptions: + """ + class containing the description of the solver options + """ + def __init__(self): + self.__qp_solver = 'PARTIAL_CONDENSING_HPIPM' # qp solver to be used in the NLP solver + self.__hessian_approx = 'GAUSS_NEWTON' # hessian approximation + self.__integrator_type = 'ERK' # integrator type + self.__tf = None # prediction horizon + self.__nlp_solver_type = 'SQP_RTI' # NLP solver + self.__globalization = 'FIXED_STEP' + self.__nlp_solver_step_length = 1.0 # fixed Newton step length + self.__levenberg_marquardt = 0.0 + self.__collocation_type = 'GAUSS_LEGENDRE' + self.__sim_method_num_stages = 4 # number of stages in the integrator + self.__sim_method_num_steps = 1 # number of steps in the integrator + self.__sim_method_newton_iter = 3 # number of Newton iterations in simulation method + self.__sim_method_jac_reuse = 0 + self.__qp_solver_tol_stat = None # QP solver stationarity tolerance + self.__qp_solver_tol_eq = None # QP solver equality tolerance + self.__qp_solver_tol_ineq = None # QP solver inequality + self.__qp_solver_tol_comp = None # QP solver complementarity + self.__qp_solver_iter_max = 50 # QP solver max iter + self.__qp_solver_cond_N = None # QP solver: new horizon after partial condensing + self.__qp_solver_warm_start = 0 + self.__nlp_solver_tol_stat = 1e-6 # NLP solver stationarity tolerance + self.__nlp_solver_tol_eq = 1e-6 # NLP solver equality tolerance + self.__nlp_solver_tol_ineq = 1e-6 # NLP solver inequality + self.__nlp_solver_tol_comp = 1e-6 # NLP solver complementarity + self.__nlp_solver_max_iter = 100 # NLP solver maximum number of iterations + self.__Tsim = None # automatically calculated as tf/N + self.__print_level = 0 # print level + self.__initialize_t_slacks = 0 # possible values: 0, 1 + self.__model_external_shared_lib_dir = None # path to the the .so lib + self.__model_external_shared_lib_name = None # name of the the .so lib + self.__regularize_method = None + self.__time_steps = None + self.__shooting_nodes = None + self.__exact_hess_cost = 1 + self.__exact_hess_dyn = 1 + self.__exact_hess_constr = 1 + self.__ext_cost_num_hess = 0 + self.__alpha_min = 0.05 + self.__alpha_reduction = 0.7 + self.__line_search_use_sufficient_descent = 0 + self.__globalization_use_SOC = 0 + self.__full_step_dual = 0 + self.__eps_sufficient_descent = 1e-4 + self.__hpipm_mode = 'BALANCE' + + + @property + def qp_solver(self): + """QP solver to be used in the NLP solver. + String in ('PARTIAL_CONDENSING_HPIPM', 'FULL_CONDENSING_QPOASES', 'FULL_CONDENSING_HPIPM', 'PARTIAL_CONDENSING_QPDUNES', 'PARTIAL_CONDENSING_OSQP'). + Default: 'PARTIAL_CONDENSING_HPIPM'. + """ + return self.__qp_solver + + @property + def hpipm_mode(self): + """ + Mode of HPIPM to be used, + + String in ('BALANCE', 'SPEED_ABS', 'SPEED', 'ROBUST'). + + Default: 'BALANCE'. + + see https://cdn.syscop.de/publications/Frison2020a.pdf + and the HPIPM code: + https://github.com/giaf/hpipm/blob/master/ocp_qp/x_ocp_qp_ipm.c#L69 + """ + return self.__hpipm_mode + + @property + def hessian_approx(self): + """Hessian approximation. + String in ('GAUSS_NEWTON', 'EXACT'). + Default: 'GAUSS_NEWTON'. + """ + return self.__hessian_approx + + @property + def integrator_type(self): + """ + Integrator type. + String in ('ERK', 'IRK', 'GNSF', 'DISCRETE', 'LIFTED_IRK'). + Default: 'ERK'. + """ + return self.__integrator_type + + @property + def nlp_solver_type(self): + """NLP solver. + String in ('SQP', 'SQP_RTI'). + Default: 'SQP_RTI'. + """ + return self.__nlp_solver_type + + @property + def globalization(self): + """Globalization type. + String in ('FIXED_STEP', 'MERIT_BACKTRACKING'). + Default: 'FIXED_STEP'. + + .. note:: preliminary implementation. + """ + return self.__globalization + + @property + def collocation_type(self): + """Collocation type: relevant for implicit integrators + -- string in {GAUSS_RADAU_IIA, GAUSS_LEGENDRE}. + + Default: GAUSS_LEGENDRE + """ + return self.__collocation_type + + @property + def regularize_method(self): + """Regularization method for the Hessian. + String in ('NO_REGULARIZE', 'MIRROR', 'PROJECT', 'PROJECT_REDUC_HESS', 'CONVEXIFY') or :code:`None`. + + Default: :code:`None`. + """ + return self.__regularize_method + + @property + def nlp_solver_step_length(self): + """ + Fixed Newton step length. + Type: float > 0. + Default: 1.0. + """ + return self.__nlp_solver_step_length + + @property + def levenberg_marquardt(self): + """ + Factor for LM regularization. + Type: float >= 0 + Default: 0.0. + """ + return self.__levenberg_marquardt + + @property + def sim_method_num_stages(self): + """ + Number of stages in the integrator. + Type: int > 0 or ndarray of ints > 0 of shape (N,). + Default: 4 + """ + return self.__sim_method_num_stages + + @property + def sim_method_num_steps(self): + """ + Number of steps in the integrator. + Type: int > 0 or ndarray of ints > 0 of shape (N,). + Default: 1 + """ + return self.__sim_method_num_steps + + @property + def sim_method_newton_iter(self): + """ + Number of Newton iterations in simulation method. + Type: int > 0 + Default: 3 + """ + return self.__sim_method_newton_iter + + @property + def sim_method_jac_reuse(self): + """ + Integer determining if jacobians are reused within integrator or ndarray of ints > 0 of shape (N,). + 0: False (no reuse); 1: True (reuse) + Default: 0 + """ + return self.__sim_method_jac_reuse + + @property + def qp_solver_tol_stat(self): + """ + QP solver stationarity tolerance. + Default: :code:`None` + """ + return self.__qp_solver_tol_stat + + @property + def qp_solver_tol_eq(self): + """ + QP solver equality tolerance. + Default: :code:`None` + """ + return self.__qp_solver_tol_eq + + @property + def qp_solver_tol_ineq(self): + """ + QP solver inequality. + Default: :code:`None` + """ + return self.__qp_solver_tol_ineq + + @property + def qp_solver_tol_comp(self): + """ + QP solver complementarity. + Default: :code:`None` + """ + return self.__qp_solver_tol_comp + + @property + def qp_solver_cond_N(self): + """QP solver: New horizon after partial condensing. + Set to N by default -> no condensing.""" + return self.__qp_solver_cond_N + + @property + def qp_solver_warm_start(self): + """QP solver: Warm starting. + 0: no warm start; 1: warm start; 2: hot start.""" + return self.__qp_solver_warm_start + + @property + def qp_solver_iter_max(self): + """ + QP solver: maximum number of iterations. + Type: int > 0 + Default: 50 + """ + return self.__qp_solver_iter_max + + @property + def tol(self): + """ + NLP solver tolerance. Sets or gets the max of :py:attr:`nlp_solver_tol_eq`, + :py:attr:`nlp_solver_tol_ineq`, :py:attr:`nlp_solver_tol_comp` + and :py:attr:`nlp_solver_tol_stat`. + """ + return max([self.__nlp_solver_tol_eq, self.__nlp_solver_tol_ineq,\ + self.__nlp_solver_tol_comp, self.__nlp_solver_tol_stat]) + + @property + def qp_tol(self): + """ + QP solver tolerance. + Sets all of the following at once or gets the max of + :py:attr:`qp_solver_tol_eq`, :py:attr:`qp_solver_tol_ineq`, + :py:attr:`qp_solver_tol_comp` and + :py:attr:`qp_solver_tol_stat`. + """ + return max([self.__qp_solver_tol_eq, self.__qp_solver_tol_ineq,\ + self.__qp_solver_tol_comp, self.__qp_solver_tol_stat]) + + @property + def nlp_solver_tol_stat(self): + """ + NLP solver stationarity tolerance. + Type: float > 0 + Default: 1e-6 + """ + return self.__nlp_solver_tol_stat + + @property + def nlp_solver_tol_eq(self): + """NLP solver equality tolerance""" + return self.__nlp_solver_tol_eq + + @property + def alpha_min(self): + """Minimal step size for globalization MERIT_BACKTRACKING, default: 0.05.""" + return self.__alpha_min + + @property + def alpha_reduction(self): + """Step size reduction factor for globalization MERIT_BACKTRACKING, default: 0.7.""" + return self.__alpha_reduction + + @property + def line_search_use_sufficient_descent(self): + """ + Determines if sufficient descent (Armijo) condition is used in line search. + Type: int; 0 or 1; + default: 0. + """ + return self.__line_search_use_sufficient_descent + + @property + def eps_sufficient_descent(self): + """ + Factor for sufficient descent (Armijo) conditon, see line_search_use_sufficient_descent. + Type: float, + default: 1e-4. + """ + return self.__eps_sufficient_descent + + @property + def globalization_use_SOC(self): + """ + Determines if second order correction (SOC) is done when using MERIT_BACKTRACKING. + SOC is done if preliminary line search does not return full step. + Type: int; 0 or 1; + default: 0. + """ + return self.__globalization_use_SOC + + @property + def full_step_dual(self): + """ + Determines if dual variables are updated with full steps (alpha=1.0) when primal variables are updated with smaller step. + Type: int; 0 or 1; + default: 0. + """ + return self.__full_step_dual + + @property + def nlp_solver_tol_ineq(self): + """NLP solver inequality tolerance""" + return self.__nlp_solver_tol_ineq + + @property + def nlp_solver_tol_comp(self): + """NLP solver complementarity tolerance""" + return self.__nlp_solver_tol_comp + + @property + def nlp_solver_max_iter(self): + """ + NLP solver maximum number of iterations. + Type: int > 0 + Default: 100 + """ + return self.__nlp_solver_max_iter + + @property + def time_steps(self): + """ + Vector with time steps between the shooting nodes. Set automatically to uniform discretization if :py:attr:`N` and :py:attr:`tf` are provided. + Default: :code:`None` + """ + return self.__time_steps + + @property + def shooting_nodes(self): + """ + Vector with the shooting nodes, time_steps will be computed from it automatically. + Default: :code:`None` + """ + return self.__shooting_nodes + + @property + def tf(self): + """ + Prediction horizon + Type: float > 0 + Default: :code:`None` + """ + return self.__tf + + @property + def Tsim(self): + """ + Time horizon for one integrator step. Automatically calculated as :py:attr:`tf`/:py:attr:`N`. + Default: :code:`None` + """ + return self.__Tsim + + @property + def print_level(self): + """ + Verbosity of printing. + Type: int >= 0 + Default: 0 + """ + return self.__print_level + + @property + def model_external_shared_lib_dir(self): + """Path to the .so lib""" + return self.__model_external_shared_lib_dir + + @property + def model_external_shared_lib_name(self): + """Name of the .so lib""" + return self.__model_external_shared_lib_name + + @property + def exact_hess_constr(self): + """ + Used in case of hessian_approx == 'EXACT'.\n + Can be used to turn off exact hessian contributions from the constraints module. + """ + return self.__exact_hess_constr + + @property + def exact_hess_cost(self): + """ + Used in case of hessian_approx == 'EXACT'.\n + Can be used to turn off exact hessian contributions from the cost module. + """ + return self.__exact_hess_cost + + @property + def exact_hess_dyn(self): + """ + Used in case of hessian_approx == 'EXACT'.\n + Can be used to turn off exact hessian contributions from the dynamics module. + """ + return self.__exact_hess_dyn + + @property + def ext_cost_num_hess(self): + """ + Determines if custom hessian approximation for cost contribution is used (> 0).\n + Or if hessian contribution is evaluated exactly using CasADi external function (=0 - default). + """ + return self.__ext_cost_num_hess + + @qp_solver.setter + def qp_solver(self, qp_solver): + qp_solvers = ('PARTIAL_CONDENSING_HPIPM', \ + 'FULL_CONDENSING_QPOASES', 'FULL_CONDENSING_HPIPM', \ + 'PARTIAL_CONDENSING_QPDUNES', 'PARTIAL_CONDENSING_OSQP') + if qp_solver in qp_solvers: + self.__qp_solver = qp_solver + else: + raise Exception('Invalid qp_solver value. Possible values are:\n\n' \ + + ',\n'.join(qp_solvers) + '.\n\nYou have: ' + qp_solver + '.\n\nExiting.') + + @regularize_method.setter + def regularize_method(self, regularize_method): + regularize_methods = ('NO_REGULARIZE', 'MIRROR', 'PROJECT', \ + 'PROJECT_REDUC_HESS', 'CONVEXIFY') + if regularize_method in regularize_methods: + self.__regularize_method = regularize_method + else: + raise Exception('Invalid regularize_method value. Possible values are:\n\n' \ + + ',\n'.join(regularize_methods) + '.\n\nYou have: ' + regularize_method + '.\n\nExiting.') + + @collocation_type.setter + def collocation_type(self, collocation_type): + collocation_types = ('GAUSS_RADAU_IIA', 'GAUSS_LEGENDRE') + if collocation_type in collocation_types: + self.__collocation_type = collocation_type + else: + raise Exception('Invalid collocation_type value. Possible values are:\n\n' \ + + ',\n'.join(collocation_types) + '.\n\nYou have: ' + collocation_type + '.\n\nExiting.') + + @hpipm_mode.setter + def hpipm_mode(self, hpipm_mode): + hpipm_modes = ('BALANCE', 'SPEED_ABS', 'SPEED', 'ROBUST') + if hpipm_mode in hpipm_modes: + self.__hpipm_mode = hpipm_mode + else: + raise Exception('Invalid hpipm_mode value. Possible values are:\n\n' \ + + ',\n'.join(hpipm_modes) + '.\n\nYou have: ' + hpipm_mode + '.\n\nExiting.') + + @hessian_approx.setter + def hessian_approx(self, hessian_approx): + hessian_approxs = ('GAUSS_NEWTON', 'EXACT') + if hessian_approx in hessian_approxs: + self.__hessian_approx = hessian_approx + else: + raise Exception('Invalid hessian_approx value. Possible values are:\n\n' \ + + ',\n'.join(hessian_approxs) + '.\n\nYou have: ' + hessian_approx + '.\n\nExiting.') + + @integrator_type.setter + def integrator_type(self, integrator_type): + integrator_types = ('ERK', 'IRK', 'GNSF', 'DISCRETE', 'LIFTED_IRK') + if integrator_type in integrator_types: + self.__integrator_type = integrator_type + else: + raise Exception('Invalid integrator_type value. Possible values are:\n\n' \ + + ',\n'.join(integrator_types) + '.\n\nYou have: ' + integrator_type + '.\n\nExiting.') + + @tf.setter + def tf(self, tf): + self.__tf = tf + + @time_steps.setter + def time_steps(self, time_steps): + if isinstance(time_steps, np.ndarray): + if len(time_steps.shape) == 1: + self.__time_steps = time_steps + else: + raise Exception('Invalid time_steps, expected np.ndarray of shape (N,).') + else: + raise Exception('Invalid time_steps, expected np.ndarray.') + + @shooting_nodes.setter + def shooting_nodes(self, shooting_nodes): + if isinstance(shooting_nodes, np.ndarray): + if len(shooting_nodes.shape) == 1: + self.__shooting_nodes = shooting_nodes + else: + raise Exception('Invalid shooting_nodes, expected np.ndarray of shape (N+1,).') + else: + raise Exception('Invalid shooting_nodes, expected np.ndarray.') + + @Tsim.setter + def Tsim(self, Tsim): + self.__Tsim = Tsim + + @globalization.setter + def globalization(self, globalization): + globalization_types = ('MERIT_BACKTRACKING', 'FIXED_STEP') + if globalization in globalization_types: + self.__globalization = globalization + else: + raise Exception('Invalid globalization value. Possible values are:\n\n' \ + + ',\n'.join(globalization_types) + '.\n\nYou have: ' + globalization + '.\n\nExiting.') + + @alpha_min.setter + def alpha_min(self, alpha_min): + self.__alpha_min = alpha_min + + @alpha_reduction.setter + def alpha_reduction(self, alpha_reduction): + self.__alpha_reduction = alpha_reduction + + @line_search_use_sufficient_descent.setter + def line_search_use_sufficient_descent(self, line_search_use_sufficient_descent): + if line_search_use_sufficient_descent in [0, 1]: + self.__line_search_use_sufficient_descent = line_search_use_sufficient_descent + else: + raise Exception(f'Invalid value for line_search_use_sufficient_descent. Possible values are 0, 1, got {line_search_use_sufficient_descent}') + + @globalization_use_SOC.setter + def globalization_use_SOC(self, globalization_use_SOC): + if globalization_use_SOC in [0, 1]: + self.__globalization_use_SOC = globalization_use_SOC + else: + raise Exception(f'Invalid value for globalization_use_SOC. Possible values are 0, 1, got {globalization_use_SOC}') + + @full_step_dual.setter + def full_step_dual(self, full_step_dual): + if full_step_dual in [0, 1]: + self.__full_step_dual = full_step_dual + else: + raise Exception(f'Invalid value for full_step_dual. Possible values are 0, 1, got {full_step_dual}') + + @eps_sufficient_descent.setter + def eps_sufficient_descent(self, eps_sufficient_descent): + if isinstance(eps_sufficient_descent, float) and eps_sufficient_descent > 0: + self.__eps_sufficient_descent = eps_sufficient_descent + else: + raise Exception('Invalid eps_sufficient_descent value. eps_sufficient_descent must be a positive float. Exiting') + + @sim_method_num_stages.setter + def sim_method_num_stages(self, sim_method_num_stages): + + # if isinstance(sim_method_num_stages, int): + # self.__sim_method_num_stages = sim_method_num_stages + # else: + # raise Exception('Invalid sim_method_num_stages value. sim_method_num_stages must be an integer. Exiting.') + + self.__sim_method_num_stages = sim_method_num_stages + + @sim_method_num_steps.setter + def sim_method_num_steps(self, sim_method_num_steps): + + # if isinstance(sim_method_num_steps, int): + # self.__sim_method_num_steps = sim_method_num_steps + # else: + # raise Exception('Invalid sim_method_num_steps value. sim_method_num_steps must be an integer. Exiting.') + self.__sim_method_num_steps = sim_method_num_steps + + + @sim_method_newton_iter.setter + def sim_method_newton_iter(self, sim_method_newton_iter): + + if isinstance(sim_method_newton_iter, int): + self.__sim_method_newton_iter = sim_method_newton_iter + else: + raise Exception('Invalid sim_method_newton_iter value. sim_method_newton_iter must be an integer. Exiting.') + + @sim_method_jac_reuse.setter + def sim_method_jac_reuse(self, sim_method_jac_reuse): + # if sim_method_jac_reuse in (True, False): + self.__sim_method_jac_reuse = sim_method_jac_reuse + # else: + # raise Exception('Invalid sim_method_jac_reuse value. sim_method_jac_reuse must be a Boolean.') + + @nlp_solver_type.setter + def nlp_solver_type(self, nlp_solver_type): + nlp_solver_types = ('SQP', 'SQP_RTI') + if nlp_solver_type in nlp_solver_types: + self.__nlp_solver_type = nlp_solver_type + else: + raise Exception('Invalid nlp_solver_type value. Possible values are:\n\n' \ + + ',\n'.join(nlp_solver_types) + '.\n\nYou have: ' + nlp_solver_type + '.\n\nExiting.') + + @nlp_solver_step_length.setter + def nlp_solver_step_length(self, nlp_solver_step_length): + if isinstance(nlp_solver_step_length, float) and nlp_solver_step_length > 0: + self.__nlp_solver_step_length = nlp_solver_step_length + else: + raise Exception('Invalid nlp_solver_step_length value. nlp_solver_step_length must be a positive float. Exiting') + + @levenberg_marquardt.setter + def levenberg_marquardt(self, levenberg_marquardt): + if isinstance(levenberg_marquardt, float) and levenberg_marquardt >= 0: + self.__levenberg_marquardt = levenberg_marquardt + else: + raise Exception('Invalid levenberg_marquardt value. levenberg_marquardt must be a positive float. Exiting') + + @qp_solver_iter_max.setter + def qp_solver_iter_max(self, qp_solver_iter_max): + if isinstance(qp_solver_iter_max, int) and qp_solver_iter_max > 0: + self.__qp_solver_iter_max = qp_solver_iter_max + else: + raise Exception('Invalid qp_solver_iter_max value. qp_solver_iter_max must be a positive int. Exiting') + + @qp_solver_cond_N.setter + def qp_solver_cond_N(self, qp_solver_cond_N): + if isinstance(qp_solver_cond_N, int) and qp_solver_cond_N >= 0: + self.__qp_solver_cond_N = qp_solver_cond_N + else: + raise Exception('Invalid qp_solver_cond_N value. qp_solver_cond_N must be a positive int. Exiting') + + @qp_solver_warm_start.setter + def qp_solver_warm_start(self, qp_solver_warm_start): + if qp_solver_warm_start in [0, 1, 2]: + self.__qp_solver_warm_start = qp_solver_warm_start + else: + raise Exception('Invalid qp_solver_warm_start value. qp_solver_warm_start must be 0 or 1 or 2. Exiting') + + @qp_tol.setter + def qp_tol(self, qp_tol): + if isinstance(qp_tol, float) and qp_tol > 0: + self.__qp_solver_tol_eq = qp_tol + self.__qp_solver_tol_ineq = qp_tol + self.__qp_solver_tol_stat = qp_tol + self.__qp_solver_tol_comp = qp_tol + else: + raise Exception('Invalid qp_tol value. qp_tol must be a positive float. Exiting') + + @qp_solver_tol_stat.setter + def qp_solver_tol_stat(self, qp_solver_tol_stat): + if isinstance(qp_solver_tol_stat, float) and qp_solver_tol_stat > 0: + self.__qp_solver_tol_stat = qp_solver_tol_stat + else: + raise Exception('Invalid qp_solver_tol_stat value. qp_solver_tol_stat must be a positive float. Exiting') + + @qp_solver_tol_eq.setter + def qp_solver_tol_eq(self, qp_solver_tol_eq): + if isinstance(qp_solver_tol_eq, float) and qp_solver_tol_eq > 0: + self.__qp_solver_tol_eq = qp_solver_tol_eq + else: + raise Exception('Invalid qp_solver_tol_eq value. qp_solver_tol_eq must be a positive float. Exiting') + + @qp_solver_tol_ineq.setter + def qp_solver_tol_ineq(self, qp_solver_tol_ineq): + if isinstance(qp_solver_tol_ineq, float) and qp_solver_tol_ineq > 0: + self.__qp_solver_tol_ineq = qp_solver_tol_ineq + else: + raise Exception('Invalid qp_solver_tol_ineq value. qp_solver_tol_ineq must be a positive float. Exiting') + + @qp_solver_tol_comp.setter + def qp_solver_tol_comp(self, qp_solver_tol_comp): + if isinstance(qp_solver_tol_comp, float) and qp_solver_tol_comp > 0: + self.__qp_solver_tol_comp = qp_solver_tol_comp + else: + raise Exception('Invalid qp_solver_tol_comp value. qp_solver_tol_comp must be a positive float. Exiting') + + @tol.setter + def tol(self, tol): + if isinstance(tol, float) and tol > 0: + self.__nlp_solver_tol_eq = tol + self.__nlp_solver_tol_ineq = tol + self.__nlp_solver_tol_stat = tol + self.__nlp_solver_tol_comp = tol + else: + raise Exception('Invalid tol value. tol must be a positive float. Exiting') + + @nlp_solver_tol_stat.setter + def nlp_solver_tol_stat(self, nlp_solver_tol_stat): + if isinstance(nlp_solver_tol_stat, float) and nlp_solver_tol_stat > 0: + self.__nlp_solver_tol_stat = nlp_solver_tol_stat + else: + raise Exception('Invalid nlp_solver_tol_stat value. nlp_solver_tol_stat must be a positive float. Exiting') + + @nlp_solver_tol_eq.setter + def nlp_solver_tol_eq(self, nlp_solver_tol_eq): + if isinstance(nlp_solver_tol_eq, float) and nlp_solver_tol_eq > 0: + self.__nlp_solver_tol_eq = nlp_solver_tol_eq + else: + raise Exception('Invalid nlp_solver_tol_eq value. nlp_solver_tol_eq must be a positive float. Exiting') + + @nlp_solver_tol_ineq.setter + def nlp_solver_tol_ineq(self, nlp_solver_tol_ineq): + if isinstance(nlp_solver_tol_ineq, float) and nlp_solver_tol_ineq > 0: + self.__nlp_solver_tol_ineq = nlp_solver_tol_ineq + else: + raise Exception('Invalid nlp_solver_tol_ineq value. nlp_solver_tol_ineq must be a positive float. Exiting') + + @nlp_solver_tol_comp.setter + def nlp_solver_tol_comp(self, nlp_solver_tol_comp): + if isinstance(nlp_solver_tol_comp, float) and nlp_solver_tol_comp > 0: + self.__nlp_solver_tol_comp = nlp_solver_tol_comp + else: + raise Exception('Invalid nlp_solver_tol_comp value. nlp_solver_tol_comp must be a positive float. Exiting') + + @nlp_solver_max_iter.setter + def nlp_solver_max_iter(self, nlp_solver_max_iter): + + if isinstance(nlp_solver_max_iter, int) and nlp_solver_max_iter > 0: + self.__nlp_solver_max_iter = nlp_solver_max_iter + else: + raise Exception('Invalid nlp_solver_max_iter value. nlp_solver_max_iter must be a positive int. Exiting') + + @print_level.setter + def print_level(self, print_level): + if isinstance(print_level, int) and print_level >= 0: + self.__print_level = print_level + else: + raise Exception('Invalid print_level value. print_level takes one of the values >=0. Exiting') + + @model_external_shared_lib_dir.setter + def model_external_shared_lib_dir(self, model_external_shared_lib_dir): + if isinstance(model_external_shared_lib_dir, str) : + self.__model_external_shared_lib_dir = model_external_shared_lib_dir + else: + raise Exception('Invalid model_external_shared_lib_dir value. Str expected.' \ + + '.\n\nYou have: ' + type(model_external_shared_lib_dir) + '.\n\nExiting.') + + @model_external_shared_lib_name.setter + def model_external_shared_lib_name(self, model_external_shared_lib_name): + if isinstance(model_external_shared_lib_name, str) : + if model_external_shared_lib_name[-3:] == '.so' : + raise Exception('Invalid model_external_shared_lib_name value. Remove the .so extension.' \ + + '.\n\nYou have: ' + type(model_external_shared_lib_name) + '.\n\nExiting.') + else : + self.__model_external_shared_lib_name = model_external_shared_lib_name + else: + raise Exception('Invalid model_external_shared_lib_name value. Str expected.' \ + + '.\n\nYou have: ' + type(model_external_shared_lib_name) + '.\n\nExiting.') + + @exact_hess_constr.setter + def exact_hess_constr(self, exact_hess_constr): + if exact_hess_constr in [0, 1]: + self.__exact_hess_constr = exact_hess_constr + else: + raise Exception('Invalid exact_hess_constr value. exact_hess_constr takes one of the values 0, 1. Exiting') + + @exact_hess_cost.setter + def exact_hess_cost(self, exact_hess_cost): + if exact_hess_cost in [0, 1]: + self.__exact_hess_cost = exact_hess_cost + else: + raise Exception('Invalid exact_hess_cost value. exact_hess_cost takes one of the values 0, 1. Exiting') + + @exact_hess_dyn.setter + def exact_hess_dyn(self, exact_hess_dyn): + if exact_hess_dyn in [0, 1]: + self.__exact_hess_dyn = exact_hess_dyn + else: + raise Exception('Invalid exact_hess_dyn value. exact_hess_dyn takes one of the values 0, 1. Exiting') + + @ext_cost_num_hess.setter + def ext_cost_num_hess(self, ext_cost_num_hess): + if ext_cost_num_hess in [0, 1]: + self.__ext_cost_num_hess = ext_cost_num_hess + else: + raise Exception('Invalid ext_cost_num_hess value. ext_cost_num_hess takes one of the values 0, 1. Exiting') + + def set(self, attr, value): + setattr(self, attr, value) + + +class AcadosOcp: + """ + Class containing the full description of the optimal control problem. + This object can be used to create an :py:class:`acados_template.acados_ocp_solver.AcadosOcpSolver`. + + The class has the following properties that can be modified to formulate a specific OCP, see below: + + - :py:attr:`dims` of type :py:class:`acados_template.acados_ocp.AcadosOcpDims` + - :py:attr:`model` of type :py:class:`acados_template.acados_model.AcadosModel` + - :py:attr:`cost` of type :py:class:`acados_template.acados_ocp.AcadosOcpCost` + - :py:attr:`constraints` of type :py:class:`acados_template.acados_ocp.AcadosOcpConstraints` + - :py:attr:`solver_options` of type :py:class:`acados_template.acados_ocp.AcadosOcpOptions` + + - :py:attr:`acados_include_path` (set automatically) + - :py:attr:`acados_lib_path` (set automatically) + - :py:attr:`parameter_values` - used to initialize the parameters (can be changed) + """ + def __init__(self, acados_path=''): + """ + Keyword arguments: + acados_path -- path of your acados installation + """ + if acados_path == '': + acados_path = get_acados_path() + + self.dims = AcadosOcpDims() + """Dimension definitions, type :py:class:`acados_template.acados_ocp.AcadosOcpDims`""" + self.model = AcadosModel() + """Model definitions, type :py:class:`acados_template.acados_model.AcadosModel`""" + self.cost = AcadosOcpCost() + """Cost definitions, type :py:class:`acados_template.acados_ocp.AcadosOcpCost`""" + self.constraints = AcadosOcpConstraints() + """Constraints definitions, type :py:class:`acados_template.acados_ocp.AcadosOcpConstraints`""" + self.solver_options = AcadosOcpOptions() + """Solver Options, type :py:class:`acados_template.acados_ocp.AcadosOcpOptions`""" + + self.acados_include_path = os.path.join(acados_path, 'include').replace(os.sep, '/') # the replace part is important on Windows for CMake + """Path to acados include directory (set automatically), type: `string`""" + self.acados_lib_path = os.path.join(acados_path, 'lib').replace(os.sep, '/') # the replace part is important on Windows for CMake + """Path to where acados library is located, type: `string`""" + + import numpy + self.cython_include_dirs = numpy.get_include() + + self.__parameter_values = np.array([]) + self.__problem_class = 'OCP' + + self.code_export_directory = 'c_generated_code' + """Path to where code will be exported. Default: `c_generated_code`.""" + + @property + def parameter_values(self): + """:math:`p` - initial values for parameter - can be updated stagewise""" + return self.__parameter_values + + @parameter_values.setter + def parameter_values(self, parameter_values): + if isinstance(parameter_values, np.ndarray): + self.__parameter_values = parameter_values + else: + raise Exception('Invalid parameter_values value. ' + + f'Expected numpy array, got {type(parameter_values)}.') + + def set(self, attr, value): + # tokenize string + tokens = attr.split('_', 1) + if len(tokens) > 1: + setter_to_call = getattr(getattr(self, tokens[0]), 'set') + else: + setter_to_call = getattr(self, 'set') + + setter_to_call(tokens[1], value) + + return diff --git a/third_party/acados/acados_template/acados_ocp_solver.py b/third_party/acados/acados_template/acados_ocp_solver.py new file mode 100644 index 0000000000..beeda2cb0c --- /dev/null +++ b/third_party/acados/acados_template/acados_ocp_solver.py @@ -0,0 +1,1818 @@ +# -*- coding: future_fstrings -*- +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + +import sys +import os +import json +import numpy as np +from datetime import datetime +import importlib +from ctypes import POINTER, cast, CDLL, c_void_p, c_char_p, c_double, c_int, c_int64, byref + +from copy import deepcopy + +from .generate_c_code_explicit_ode import generate_c_code_explicit_ode +from .generate_c_code_implicit_ode import generate_c_code_implicit_ode +from .generate_c_code_gnsf import generate_c_code_gnsf +from .generate_c_code_discrete_dynamics import generate_c_code_discrete_dynamics +from .generate_c_code_constraint import generate_c_code_constraint +from .generate_c_code_nls_cost import generate_c_code_nls_cost +from .generate_c_code_external_cost import generate_c_code_external_cost +from .acados_ocp import AcadosOcp +from .acados_model import acados_model_strip_casadi_symbolics +from .utils import is_column, is_empty, casadi_length, render_template,\ + format_class_dict, ocp_check_against_layout, np_array_to_list, make_model_consistent,\ + set_up_imported_gnsf_model, get_ocp_nlp_layout, get_python_interface_path +from .builders import CMakeBuilder + + +def make_ocp_dims_consistent(acados_ocp): + dims = acados_ocp.dims + cost = acados_ocp.cost + constraints = acados_ocp.constraints + model = acados_ocp.model + opts = acados_ocp.solver_options + + # nx + if is_column(model.x): + dims.nx = casadi_length(model.x) + else: + raise Exception('model.x should be column vector!') + + # nu + if is_empty(model.u): + dims.nu = 0 + else: + dims.nu = casadi_length(model.u) + + # nz + if is_empty(model.z): + dims.nz = 0 + else: + dims.nz = casadi_length(model.z) + + # np + if is_empty(model.p): + dims.np = 0 + else: + dims.np = casadi_length(model.p) + if acados_ocp.parameter_values.shape[0] != dims.np: + raise Exception('inconsistent dimension np, regarding model.p and parameter_values.' + \ + f'\nGot np = {dims.np}, acados_ocp.parameter_values.shape = {acados_ocp.parameter_values.shape[0]}\n') + + ## cost + # initial stage - if not set, copy fields from path constraints + if cost.cost_type_0 is None: + cost.cost_type_0 = cost.cost_type + cost.W_0 = cost.W + cost.Vx_0 = cost.Vx + cost.Vu_0 = cost.Vu + cost.Vz_0 = cost.Vz + cost.yref_0 = cost.yref + cost.cost_ext_fun_type_0 = cost.cost_ext_fun_type + model.cost_y_expr_0 = model.cost_y_expr + model.cost_expr_ext_cost_0 = model.cost_expr_ext_cost + model.cost_expr_ext_cost_custom_hess_0 = model.cost_expr_ext_cost_custom_hess + + if cost.cost_type_0 == 'LINEAR_LS': + ny_0 = cost.W_0.shape[0] + if cost.Vx_0.shape[0] != ny_0 or cost.Vu_0.shape[0] != ny_0: + raise Exception('inconsistent dimension ny_0, regarding W_0, Vx_0, Vu_0.' + \ + f'\nGot W_0[{cost.W_0.shape}], Vx_0[{cost.Vx_0.shape}], Vu_0[{cost.Vu_0.shape}]\n') + if dims.nz != 0 and cost.Vz_0.shape[0] != ny_0: + raise Exception('inconsistent dimension ny_0, regarding W_0, Vx_0, Vu_0, Vz_0.' + \ + f'\nGot W_0[{cost.W_0.shape}], Vx_0[{cost.Vx_0.shape}], Vu_0[{cost.Vu_0.shape}], Vz_0[{cost.Vz_0.shape}]\n') + if cost.Vx_0.shape[1] != dims.nx and ny_0 != 0: + raise Exception('inconsistent dimension: Vx_0 should have nx columns.') + if cost.Vu_0.shape[1] != dims.nu and ny_0 != 0: + raise Exception('inconsistent dimension: Vu_0 should have nu columns.') + if cost.yref_0.shape[0] != ny_0: + raise Exception('inconsistent dimension: regarding W_0, yref_0.' + \ + f'\nGot W_0[{cost.W_0.shape}], yref_0[{cost.yref_0.shape}]\n') + dims.ny_0 = ny_0 + + elif cost.cost_type_0 == 'NONLINEAR_LS': + ny_0 = cost.W_0.shape[0] + if is_empty(model.cost_y_expr_0) and ny_0 != 0: + raise Exception('inconsistent dimension ny_0: regarding W_0, cost_y_expr.') + elif casadi_length(model.cost_y_expr_0) != ny_0: + raise Exception('inconsistent dimension ny_0: regarding W_0, cost_y_expr.') + if cost.yref_0.shape[0] != ny_0: + raise Exception('inconsistent dimension: regarding W_0, yref_0.' + \ + f'\nGot W_0[{cost.W.shape}], yref_0[{cost.yref_0.shape}]\n') + dims.ny_0 = ny_0 + + elif cost.cost_type_0 == 'EXTERNAL': + if opts.hessian_approx == 'GAUSS_NEWTON' and opts.ext_cost_num_hess == 0 and model.cost_expr_ext_cost_custom_hess_0 is None: + print("\nWARNING: Gauss-Newton Hessian approximation with EXTERNAL cost type not possible!\n" + "got cost_type_0: EXTERNAL, hessian_approx: 'GAUSS_NEWTON.'\n" + "GAUSS_NEWTON hessian is only supported for cost_types [NON]LINEAR_LS.\n" + "If you continue, acados will proceed computing the exact hessian for the cost term.\n" + "Note: There is also the option to use the external cost module with a numerical hessian approximation (see `ext_cost_num_hess`).\n" + "OR the option to provide a symbolic custom hessian approximation (see `cost_expr_ext_cost_custom_hess`).\n") + + # path + if cost.cost_type == 'LINEAR_LS': + ny = cost.W.shape[0] + if cost.Vx.shape[0] != ny or cost.Vu.shape[0] != ny: + raise Exception('inconsistent dimension ny, regarding W, Vx, Vu.' + \ + f'\nGot W[{cost.W.shape}], Vx[{cost.Vx.shape}], Vu[{cost.Vu.shape}]\n') + if dims.nz != 0 and cost.Vz.shape[0] != ny: + raise Exception('inconsistent dimension ny, regarding W, Vx, Vu, Vz.' + \ + f'\nGot W[{cost.W.shape}], Vx[{cost.Vx.shape}], Vu[{cost.Vu.shape}], Vz[{cost.Vz.shape}]\n') + if cost.Vx.shape[1] != dims.nx and ny != 0: + raise Exception('inconsistent dimension: Vx should have nx columns.') + if cost.Vu.shape[1] != dims.nu and ny != 0: + raise Exception('inconsistent dimension: Vu should have nu columns.') + if cost.yref.shape[0] != ny: + raise Exception('inconsistent dimension: regarding W, yref.' + \ + f'\nGot W[{cost.W.shape}], yref[{cost.yref.shape}]\n') + dims.ny = ny + + elif cost.cost_type == 'NONLINEAR_LS': + ny = cost.W.shape[0] + if is_empty(model.cost_y_expr) and ny != 0: + raise Exception('inconsistent dimension ny: regarding W, cost_y_expr.') + elif casadi_length(model.cost_y_expr) != ny: + raise Exception('inconsistent dimension ny: regarding W, cost_y_expr.') + if cost.yref.shape[0] != ny: + raise Exception('inconsistent dimension: regarding W, yref.' + \ + f'\nGot W[{cost.W.shape}], yref[{cost.yref.shape}]\n') + dims.ny = ny + + elif cost.cost_type == 'EXTERNAL': + if opts.hessian_approx == 'GAUSS_NEWTON' and opts.ext_cost_num_hess == 0 and model.cost_expr_ext_cost_custom_hess is None: + print("\nWARNING: Gauss-Newton Hessian approximation with EXTERNAL cost type not possible!\n" + "got cost_type: EXTERNAL, hessian_approx: 'GAUSS_NEWTON.'\n" + "GAUSS_NEWTON hessian is only supported for cost_types [NON]LINEAR_LS.\n" + "If you continue, acados will proceed computing the exact hessian for the cost term.\n" + "Note: There is also the option to use the external cost module with a numerical hessian approximation (see `ext_cost_num_hess`).\n" + "OR the option to provide a symbolic custom hessian approximation (see `cost_expr_ext_cost_custom_hess`).\n") + + # terminal + if cost.cost_type_e == 'LINEAR_LS': + ny_e = cost.W_e.shape[0] + if cost.Vx_e.shape[0] != ny_e: + raise Exception('inconsistent dimension ny_e: regarding W_e, cost_y_expr_e.' + \ + f'\nGot W_e[{cost.W_e.shape}], Vx_e[{cost.Vx_e.shape}]') + if cost.Vx_e.shape[1] != dims.nx and ny_e != 0: + raise Exception('inconsistent dimension: Vx_e should have nx columns.') + if cost.yref_e.shape[0] != ny_e: + raise Exception('inconsistent dimension: regarding W_e, yref_e.') + dims.ny_e = ny_e + + elif cost.cost_type_e == 'NONLINEAR_LS': + ny_e = cost.W_e.shape[0] + if is_empty(model.cost_y_expr_e) and ny_e != 0: + raise Exception('inconsistent dimension ny_e: regarding W_e, cost_y_expr_e.') + elif casadi_length(model.cost_y_expr_e) != ny_e: + raise Exception('inconsistent dimension ny_e: regarding W_e, cost_y_expr_e.') + if cost.yref_e.shape[0] != ny_e: + raise Exception('inconsistent dimension: regarding W_e, yref_e.') + dims.ny_e = ny_e + + elif cost.cost_type_e == 'EXTERNAL': + if opts.hessian_approx == 'GAUSS_NEWTON' and opts.ext_cost_num_hess == 0 and model.cost_expr_ext_cost_custom_hess_e is None: + print("\nWARNING: Gauss-Newton Hessian approximation with EXTERNAL cost type not possible!\n" + "got cost_type_e: EXTERNAL, hessian_approx: 'GAUSS_NEWTON.'\n" + "GAUSS_NEWTON hessian is only supported for cost_types [NON]LINEAR_LS.\n" + "If you continue, acados will proceed computing the exact hessian for the cost term.\n" + "Note: There is also the option to use the external cost module with a numerical hessian approximation (see `ext_cost_num_hess`).\n" + "OR the option to provide a symbolic custom hessian approximation (see `cost_expr_ext_cost_custom_hess`).\n") + + ## constraints + # initial + if (constraints.lbx_0 == [] and constraints.ubx_0 == []): + dims.nbx_0 = 0 + else: + this_shape = constraints.lbx_0.shape + other_shape = constraints.ubx_0.shape + if not this_shape == other_shape: + raise Exception('lbx_0, ubx_0 have different shapes!') + if not is_column(constraints.lbx_0): + raise Exception('lbx_0, ubx_0 must be column vectors!') + dims.nbx_0 = constraints.lbx_0.size + + if all(constraints.lbx_0 == constraints.ubx_0) and dims.nbx_0 == dims.nx \ + and dims.nbxe_0 is None \ + and (constraints.idxbxe_0.shape == constraints.idxbx_0.shape)\ + and all(constraints.idxbxe_0 == constraints.idxbx_0): + # case: x0 was set: nbx0 are all equlities. + dims.nbxe_0 = dims.nbx_0 + elif dims.nbxe_0 is None: + # case: x0 was not set -> dont assume nbx0 to be equality constraints. + dims.nbxe_0 = 0 + + # path + nbx = constraints.idxbx.shape[0] + if constraints.ubx.shape[0] != nbx or constraints.lbx.shape[0] != nbx: + raise Exception('inconsistent dimension nbx, regarding idxbx, ubx, lbx.') + else: + dims.nbx = nbx + + nbu = constraints.idxbu.shape[0] + if constraints.ubu.shape[0] != nbu or constraints.lbu.shape[0] != nbu: + raise Exception('inconsistent dimension nbu, regarding idxbu, ubu, lbu.') + else: + dims.nbu = nbu + + ng = constraints.lg.shape[0] + if constraints.ug.shape[0] != ng or constraints.C.shape[0] != ng \ + or constraints.D.shape[0] != ng: + raise Exception('inconsistent dimension ng, regarding lg, ug, C, D.') + else: + dims.ng = ng + + if not is_empty(model.con_h_expr): + nh = casadi_length(model.con_h_expr) + else: + nh = 0 + + if constraints.uh.shape[0] != nh or constraints.lh.shape[0] != nh: + raise Exception('inconsistent dimension nh, regarding lh, uh, con_h_expr.') + else: + dims.nh = nh + + if is_empty(model.con_phi_expr): + dims.nphi = 0 + dims.nr = 0 + else: + dims.nphi = casadi_length(model.con_phi_expr) + if is_empty(model.con_r_expr): + raise Exception('convex over nonlinear constraints: con_r_expr but con_phi_expr is nonempty') + else: + dims.nr = casadi_length(model.con_r_expr) + + # terminal + nbx_e = constraints.idxbx_e.shape[0] + if constraints.ubx_e.shape[0] != nbx_e or constraints.lbx_e.shape[0] != nbx_e: + raise Exception('inconsistent dimension nbx_e, regarding idxbx_e, ubx_e, lbx_e.') + else: + dims.nbx_e = nbx_e + + ng_e = constraints.lg_e.shape[0] + if constraints.ug_e.shape[0] != ng_e or constraints.C_e.shape[0] != ng_e: + raise Exception('inconsistent dimension ng_e, regarding_e lg_e, ug_e, C_e.') + else: + dims.ng_e = ng_e + + if not is_empty(model.con_h_expr_e): + nh_e = casadi_length(model.con_h_expr_e) + else: + nh_e = 0 + + if constraints.uh_e.shape[0] != nh_e or constraints.lh_e.shape[0] != nh_e: + raise Exception('inconsistent dimension nh_e, regarding lh_e, uh_e, con_h_expr_e.') + else: + dims.nh_e = nh_e + + if is_empty(model.con_phi_expr_e): + dims.nphi_e = 0 + dims.nr_e = 0 + else: + dims.nphi_e = casadi_length(model.con_phi_expr_e) + if is_empty(model.con_r_expr_e): + raise Exception('convex over nonlinear constraints: con_r_expr_e but con_phi_expr_e is nonempty') + else: + dims.nr_e = casadi_length(model.con_r_expr_e) + + # Slack dimensions + nsbx = constraints.idxsbx.shape[0] + if is_empty(constraints.lsbx): + constraints.lsbx = np.zeros((nsbx,)) + elif constraints.lsbx.shape[0] != nsbx: + raise Exception('inconsistent dimension nsbx, regarding idxsbx, lsbx.') + if is_empty(constraints.usbx): + constraints.usbx = np.zeros((nsbx,)) + elif constraints.usbx.shape[0] != nsbx: + raise Exception('inconsistent dimension nsbx, regarding idxsbx, usbx.') + dims.nsbx = nsbx + + nsbu = constraints.idxsbu.shape[0] + if is_empty(constraints.lsbu): + constraints.lsbu = np.zeros((nsbu,)) + elif constraints.lsbu.shape[0] != nsbu: + raise Exception('inconsistent dimension nsbu, regarding idxsbu, lsbu.') + if is_empty(constraints.usbu): + constraints.usbu = np.zeros((nsbu,)) + elif constraints.usbu.shape[0] != nsbu: + raise Exception('inconsistent dimension nsbu, regarding idxsbu, usbu.') + dims.nsbu = nsbu + + nsh = constraints.idxsh.shape[0] + if is_empty(constraints.lsh): + constraints.lsh = np.zeros((nsh,)) + elif constraints.lsh.shape[0] != nsh: + raise Exception('inconsistent dimension nsh, regarding idxsh, lsh.') + if is_empty(constraints.ush): + constraints.ush = np.zeros((nsh,)) + elif constraints.ush.shape[0] != nsh: + raise Exception('inconsistent dimension nsh, regarding idxsh, ush.') + dims.nsh = nsh + + nsphi = constraints.idxsphi.shape[0] + if is_empty(constraints.lsphi): + constraints.lsphi = np.zeros((nsphi,)) + elif constraints.lsphi.shape[0] != nsphi: + raise Exception('inconsistent dimension nsphi, regarding idxsphi, lsphi.') + if is_empty(constraints.usphi): + constraints.usphi = np.zeros((nsphi,)) + elif constraints.usphi.shape[0] != nsphi: + raise Exception('inconsistent dimension nsphi, regarding idxsphi, usphi.') + dims.nsphi = nsphi + + nsg = constraints.idxsg.shape[0] + if is_empty(constraints.lsg): + constraints.lsg = np.zeros((nsg,)) + elif constraints.lsg.shape[0] != nsg: + raise Exception('inconsistent dimension nsg, regarding idxsg, lsg.') + if is_empty(constraints.usg): + constraints.usg = np.zeros((nsg,)) + elif constraints.usg.shape[0] != nsg: + raise Exception('inconsistent dimension nsg, regarding idxsg, usg.') + dims.nsg = nsg + + ns = nsbx + nsbu + nsh + nsg + nsphi + wrong_field = "" + if cost.Zl.shape[0] != ns: + wrong_field = "Zl" + dim = cost.Zl.shape[0] + elif cost.Zu.shape[0] != ns: + wrong_field = "Zu" + dim = cost.Zu.shape[0] + elif cost.zl.shape[0] != ns: + wrong_field = "zl" + dim = cost.zl.shape[0] + elif cost.zu.shape[0] != ns: + wrong_field = "zu" + dim = cost.zu.shape[0] + + if wrong_field != "": + raise Exception(f'Inconsistent size for field {wrong_field}, with dimension {dim}, \n\t'\ + + f'Detected ns = {ns} = nsbx + nsbu + nsg + nsh + nsphi.\n\t'\ + + f'With nsbx = {nsbx}, nsbu = {nsbu}, nsg = {nsg}, nsh = {nsh}, nsphi = {nsphi}') + + dims.ns = ns + + nsbx_e = constraints.idxsbx_e.shape[0] + if is_empty(constraints.lsbx_e): + constraints.lsbx_e = np.zeros((nsbx_e,)) + elif constraints.lsbx_e.shape[0] != nsbx_e: + raise Exception('inconsistent dimension nsbx_e, regarding idxsbx_e, lsbx_e.') + if is_empty(constraints.usbx_e): + constraints.usbx_e = np.zeros((nsbx_e,)) + elif constraints.usbx_e.shape[0] != nsbx_e: + raise Exception('inconsistent dimension nsbx_e, regarding idxsbx_e, usbx_e.') + dims.nsbx_e = nsbx_e + + nsh_e = constraints.idxsh_e.shape[0] + if is_empty(constraints.lsh_e): + constraints.lsh_e = np.zeros((nsh_e,)) + elif constraints.lsh_e.shape[0] != nsh_e: + raise Exception('inconsistent dimension nsh_e, regarding idxsh_e, lsh_e.') + if is_empty(constraints.ush_e): + constraints.ush_e = np.zeros((nsh_e,)) + elif constraints.ush_e.shape[0] != nsh_e: + raise Exception('inconsistent dimension nsh_e, regarding idxsh_e, ush_e.') + dims.nsh_e = nsh_e + + nsg_e = constraints.idxsg_e.shape[0] + if is_empty(constraints.lsg_e): + constraints.lsg_e = np.zeros((nsg_e,)) + elif constraints.lsg_e.shape[0] != nsg_e: + raise Exception('inconsistent dimension nsg_e, regarding idxsg_e, lsg_e.') + if is_empty(constraints.usg_e): + constraints.usg_e = np.zeros((nsg_e,)) + elif constraints.usg_e.shape[0] != nsg_e: + raise Exception('inconsistent dimension nsg_e, regarding idxsg_e, usg_e.') + dims.nsg_e = nsg_e + + nsphi_e = constraints.idxsphi_e.shape[0] + if is_empty(constraints.lsphi_e): + constraints.lsphi_e = np.zeros((nsphi_e,)) + elif constraints.lsphi_e.shape[0] != nsphi_e: + raise Exception('inconsistent dimension nsphi_e, regarding idxsphi_e, lsphi_e.') + if is_empty(constraints.usphi_e): + constraints.usphi_e = np.zeros((nsphi_e,)) + elif constraints.usphi_e.shape[0] != nsphi_e: + raise Exception('inconsistent dimension nsphi_e, regarding idxsphi_e, usphi_e.') + dims.nsphi_e = nsphi_e + + # terminal + ns_e = nsbx_e + nsh_e + nsg_e + nsphi_e + wrong_field = "" + if cost.Zl_e.shape[0] != ns_e: + wrong_field = "Zl_e" + dim = cost.Zl_e.shape[0] + elif cost.Zu_e.shape[0] != ns_e: + wrong_field = "Zu_e" + dim = cost.Zu_e.shape[0] + elif cost.zl_e.shape[0] != ns_e: + wrong_field = "zl_e" + dim = cost.zl_e.shape[0] + elif cost.zu_e.shape[0] != ns_e: + wrong_field = "zu_e" + dim = cost.zu_e.shape[0] + + if wrong_field != "": + raise Exception(f'Inconsistent size for field {wrong_field}, with dimension {dim}, \n\t'\ + + f'Detected ns_e = {ns_e} = nsbx_e + nsg_e + nsh_e + nsphi_e.\n\t'\ + + f'With nsbx_e = {nsbx_e}, nsg_e = {nsg_e}, nsh_e = {nsh_e}, nsphi_e = {nsphi_e}') + + dims.ns_e = ns_e + + # discretization + if is_empty(opts.time_steps) and is_empty(opts.shooting_nodes): + # uniform discretization + opts.time_steps = opts.tf / dims.N * np.ones((dims.N,)) + + elif not is_empty(opts.shooting_nodes): + if np.shape(opts.shooting_nodes)[0] != dims.N+1: + raise Exception('inconsistent dimension N, regarding shooting_nodes.') + + time_steps = opts.shooting_nodes[1:] - opts.shooting_nodes[0:-1] + # identify constant time_steps: due to numerical reasons the content of time_steps might vary a bit + avg_time_steps = np.average(time_steps) + # criterion for constant time step detection: the min/max difference in values normalized by the average + check_const_time_step = (np.max(time_steps)-np.min(time_steps)) / avg_time_steps + # if the criterion is small, we have a constant time_step + if check_const_time_step < 1e-9: + time_steps[:] = avg_time_steps # if we have a constant time_step: apply the average time_step + + opts.time_steps = time_steps + + elif (not is_empty(opts.time_steps)) and (not is_empty(opts.shooting_nodes)): + Exception('Please provide either time_steps or shooting_nodes for nonuniform discretization') + + tf = np.sum(opts.time_steps) + if (tf - opts.tf) / tf > 1e-15: + raise Exception(f'Inconsistent discretization: {opts.tf}'\ + f' = tf != sum(opts.time_steps) = {tf}.') + + # num_steps + if isinstance(opts.sim_method_num_steps, np.ndarray) and opts.sim_method_num_steps.size == 1: + opts.sim_method_num_steps = opts.sim_method_num_steps.item() + + if isinstance(opts.sim_method_num_steps, (int, float)) and opts.sim_method_num_steps % 1 == 0: + opts.sim_method_num_steps = opts.sim_method_num_steps * np.ones((dims.N,), dtype=np.int64) + elif isinstance(opts.sim_method_num_steps, np.ndarray) and opts.sim_method_num_steps.size == dims.N \ + and np.all(np.equal(np.mod(opts.sim_method_num_steps, 1), 0)): + opts.sim_method_num_steps = np.reshape(opts.sim_method_num_steps, (dims.N,)).astype(np.int64) + else: + raise Exception("Wrong value for sim_method_num_steps. Should be either int or array of ints of shape (N,).") + + # num_stages + if isinstance(opts.sim_method_num_stages, np.ndarray) and opts.sim_method_num_stages.size == 1: + opts.sim_method_num_stages = opts.sim_method_num_stages.item() + + if isinstance(opts.sim_method_num_stages, (int, float)) and opts.sim_method_num_stages % 1 == 0: + opts.sim_method_num_stages = opts.sim_method_num_stages * np.ones((dims.N,), dtype=np.int64) + elif isinstance(opts.sim_method_num_stages, np.ndarray) and opts.sim_method_num_stages.size == dims.N \ + and np.all(np.equal(np.mod(opts.sim_method_num_stages, 1), 0)): + opts.sim_method_num_stages = np.reshape(opts.sim_method_num_stages, (dims.N,)).astype(np.int64) + else: + raise Exception("Wrong value for sim_method_num_stages. Should be either int or array of ints of shape (N,).") + + # jac_reuse + if isinstance(opts.sim_method_jac_reuse, np.ndarray) and opts.sim_method_jac_reuse.size == 1: + opts.sim_method_jac_reuse = opts.sim_method_jac_reuse.item() + + if isinstance(opts.sim_method_jac_reuse, (int, float)) and opts.sim_method_jac_reuse % 1 == 0: + opts.sim_method_jac_reuse = opts.sim_method_jac_reuse * np.ones((dims.N,), dtype=np.int64) + elif isinstance(opts.sim_method_jac_reuse, np.ndarray) and opts.sim_method_jac_reuse.size == dims.N \ + and np.all(np.equal(np.mod(opts.sim_method_jac_reuse, 1), 0)): + opts.sim_method_jac_reuse = np.reshape(opts.sim_method_jac_reuse, (dims.N,)).astype(np.int64) + else: + raise Exception("Wrong value for sim_method_jac_reuse. Should be either int or array of ints of shape (N,).") + + +def get_simulink_default_opts(): + python_interface_path = get_python_interface_path() + abs_path = os.path.join(python_interface_path, 'simulink_default_opts.json') + with open(abs_path , 'r') as f: + simulink_default_opts = json.load(f) + return simulink_default_opts + + +def ocp_formulation_json_dump(acados_ocp, simulink_opts, json_file='acados_ocp_nlp.json'): + # Load acados_ocp_nlp structure description + ocp_layout = get_ocp_nlp_layout() + + # Copy input ocp object dictionary + ocp_nlp_dict = dict(deepcopy(acados_ocp).__dict__) + # TODO: maybe make one function with formatting + + for acados_struct, v in ocp_layout.items(): + # skip non dict attributes + if not isinstance(v, dict): + continue + # setattr(ocp_nlp, acados_struct, dict(getattr(acados_ocp, acados_struct).__dict__)) + # Copy ocp object attributes dictionaries + ocp_nlp_dict[acados_struct]=dict(getattr(acados_ocp, acados_struct).__dict__) + + ocp_nlp_dict = format_class_dict(ocp_nlp_dict) + + # strip symbolics + ocp_nlp_dict['model'] = acados_model_strip_casadi_symbolics(ocp_nlp_dict['model']) + + # strip shooting_nodes + ocp_nlp_dict['solver_options'].pop('shooting_nodes', None) + dims_dict = format_class_dict(acados_ocp.dims.__dict__) + + ocp_check_against_layout(ocp_nlp_dict, dims_dict) + + # add simulink options + ocp_nlp_dict['simulink_opts'] = simulink_opts + + with open(json_file, 'w') as f: + json.dump(ocp_nlp_dict, f, default=np_array_to_list, indent=4, sort_keys=True) + + + +def ocp_formulation_json_load(json_file='acados_ocp_nlp.json'): + # Load acados_ocp_nlp structure description + ocp_layout = get_ocp_nlp_layout() + + with open(json_file, 'r') as f: + ocp_nlp_json = json.load(f) + + ocp_nlp_dict = json2dict(ocp_nlp_json, ocp_nlp_json['dims']) + + # Instantiate AcadosOcp object + acados_ocp = AcadosOcp() + + # load class dict + acados_ocp.__dict__ = ocp_nlp_dict + + # load class attributes dict, dims, constraints, etc + for acados_struct, v in ocp_layout.items(): + # skip non dict attributes + if not isinstance(v, dict): + continue + acados_attribute = getattr(acados_ocp, acados_struct) + acados_attribute.__dict__ = ocp_nlp_dict[acados_struct] + setattr(acados_ocp, acados_struct, acados_attribute) + + return acados_ocp + + +def ocp_generate_external_functions(acados_ocp, model): + + model = make_model_consistent(model) + + if acados_ocp.solver_options.hessian_approx == 'EXACT': + opts = dict(generate_hess=1) + else: + opts = dict(generate_hess=0) + code_export_dir = acados_ocp.code_export_directory + opts['code_export_directory'] = code_export_dir + + if acados_ocp.model.dyn_ext_fun_type != 'casadi': + raise Exception("ocp_generate_external_functions: dyn_ext_fun_type only supports 'casadi' for now.\ + Extending the Python interface with generic function support is welcome.") + + if acados_ocp.solver_options.integrator_type == 'ERK': + # explicit model -- generate C code + generate_c_code_explicit_ode(model, opts) + elif acados_ocp.solver_options.integrator_type == 'IRK': + # implicit model -- generate C code + generate_c_code_implicit_ode(model, opts) + elif acados_ocp.solver_options.integrator_type == 'LIFTED_IRK': + generate_c_code_implicit_ode(model, opts) + elif acados_ocp.solver_options.integrator_type == 'GNSF': + generate_c_code_gnsf(model, opts) + elif acados_ocp.solver_options.integrator_type == 'DISCRETE': + generate_c_code_discrete_dynamics(model, opts) + else: + raise Exception("ocp_generate_external_functions: unknown integrator type.") + + if acados_ocp.dims.nphi > 0 or acados_ocp.dims.nh > 0: + generate_c_code_constraint(model, model.name, False, opts) + + if acados_ocp.dims.nphi_e > 0 or acados_ocp.dims.nh_e > 0: + generate_c_code_constraint(model, model.name, True, opts) + + # dummy matrices + if not acados_ocp.cost.cost_type_0 == 'LINEAR_LS': + acados_ocp.cost.Vx_0 = np.zeros((acados_ocp.dims.ny_0, acados_ocp.dims.nx)) + acados_ocp.cost.Vu_0 = np.zeros((acados_ocp.dims.ny_0, acados_ocp.dims.nu)) + if not acados_ocp.cost.cost_type == 'LINEAR_LS': + acados_ocp.cost.Vx = np.zeros((acados_ocp.dims.ny, acados_ocp.dims.nx)) + acados_ocp.cost.Vu = np.zeros((acados_ocp.dims.ny, acados_ocp.dims.nu)) + if not acados_ocp.cost.cost_type_e == 'LINEAR_LS': + acados_ocp.cost.Vx_e = np.zeros((acados_ocp.dims.ny_e, acados_ocp.dims.nx)) + + if acados_ocp.cost.cost_type_0 == 'NONLINEAR_LS': + generate_c_code_nls_cost(model, model.name, 'initial', opts) + elif acados_ocp.cost.cost_type_0 == 'EXTERNAL': + generate_c_code_external_cost(model, 'initial', opts) + + if acados_ocp.cost.cost_type == 'NONLINEAR_LS': + generate_c_code_nls_cost(model, model.name, 'path', opts) + elif acados_ocp.cost.cost_type == 'EXTERNAL': + generate_c_code_external_cost(model, 'path', opts) + + if acados_ocp.cost.cost_type_e == 'NONLINEAR_LS': + generate_c_code_nls_cost(model, model.name, 'terminal', opts) + elif acados_ocp.cost.cost_type_e == 'EXTERNAL': + generate_c_code_external_cost(model, 'terminal', opts) + + +def ocp_get_default_cmake_builder() -> CMakeBuilder: + """ + If :py:class:`~acados_template.acados_ocp_solver.AcadosOcpSolver` is used with `CMake` this function returns a good first setting. + :return: default :py:class:`~acados_template.builders.CMakeBuilder` + """ + cmake_builder = CMakeBuilder() + cmake_builder.options_on = ['BUILD_ACADOS_OCP_SOLVER_LIB'] + return cmake_builder + + +def ocp_render_templates(acados_ocp, json_file, cmake_builder=None): + + name = acados_ocp.model.name + + # setting up loader and environment + json_path = os.path.abspath(json_file) + + if not os.path.exists(json_path): + raise Exception(f'Path "{json_path}" not found!') + + code_export_dir = acados_ocp.code_export_directory + template_dir = code_export_dir + + ## Render templates + in_file = 'main.in.c' + out_file = f'main_{name}.c' + render_template(in_file, out_file, template_dir, json_path) + + in_file = 'acados_solver.in.c' + out_file = f'acados_solver_{name}.c' + render_template(in_file, out_file, template_dir, json_path) + + in_file = 'acados_solver.in.h' + out_file = f'acados_solver_{name}.h' + render_template(in_file, out_file, template_dir, json_path) + + in_file = 'acados_solver.in.pxd' + out_file = f'acados_solver.pxd' + render_template(in_file, out_file, template_dir, json_path) + + if cmake_builder is not None: + in_file = 'CMakeLists.in.txt' + out_file = 'CMakeLists.txt' + render_template(in_file, out_file, template_dir, json_path) + else: + in_file = 'Makefile.in' + out_file = 'Makefile' + render_template(in_file, out_file, template_dir, json_path) + + in_file = 'acados_solver_sfun.in.c' + out_file = f'acados_solver_sfunction_{name}.c' + render_template(in_file, out_file, template_dir, json_path) + + in_file = 'make_sfun.in.m' + out_file = f'make_sfun_{name}.m' + render_template(in_file, out_file, template_dir, json_path) + + # sim + in_file = 'acados_sim_solver.in.c' + out_file = f'acados_sim_solver_{name}.c' + render_template(in_file, out_file, template_dir, json_path) + + in_file = 'acados_sim_solver.in.h' + out_file = f'acados_sim_solver_{name}.h' + render_template(in_file, out_file, template_dir, json_path) + + in_file = 'main_sim.in.c' + out_file = f'main_sim_{name}.c' + render_template(in_file, out_file, template_dir, json_path) + + ## folder model + template_dir = os.path.join(code_export_dir, name + '_model') + in_file = 'model.in.h' + out_file = f'{name}_model.h' + render_template(in_file, out_file, template_dir, json_path) + + # constraints on convex over nonlinear function + if acados_ocp.constraints.constr_type == 'BGP' and acados_ocp.dims.nphi > 0: + # constraints on outer function + template_dir = os.path.join(code_export_dir, name + '_constraints') + in_file = 'phi_constraint.in.h' + out_file = f'{name}_phi_constraint.h' + render_template(in_file, out_file, template_dir, json_path) + + # terminal constraints on convex over nonlinear function + if acados_ocp.constraints.constr_type_e == 'BGP' and acados_ocp.dims.nphi_e > 0: + # terminal constraints on outer function + template_dir = os.path.join(code_export_dir, name + '_constraints') + in_file = 'phi_e_constraint.in.h' + out_file = f'{name}_phi_e_constraint.h' + render_template(in_file, out_file, template_dir, json_path) + + # nonlinear constraints + if acados_ocp.constraints.constr_type == 'BGH' and acados_ocp.dims.nh > 0: + template_dir = os.path.join(code_export_dir, name + '_constraints') + in_file = 'h_constraint.in.h' + out_file = f'{name}_h_constraint.h' + render_template(in_file, out_file, template_dir, json_path) + + # terminal nonlinear constraints + if acados_ocp.constraints.constr_type_e == 'BGH' and acados_ocp.dims.nh_e > 0: + template_dir = os.path.join(code_export_dir, name + '_constraints') + in_file = 'h_e_constraint.in.h' + out_file = f'{name}_h_e_constraint.h' + render_template(in_file, out_file, template_dir, json_path) + + # initial stage Nonlinear LS cost function + if acados_ocp.cost.cost_type_0 == 'NONLINEAR_LS': + template_dir = os.path.join(code_export_dir, name + '_cost') + in_file = 'cost_y_0_fun.in.h' + out_file = f'{name}_cost_y_0_fun.h' + render_template(in_file, out_file, template_dir, json_path) + # external cost - terminal + elif acados_ocp.cost.cost_type_0 == 'EXTERNAL': + template_dir = os.path.join(code_export_dir, name + '_cost') + in_file = 'external_cost_0.in.h' + out_file = f'{name}_external_cost_0.h' + render_template(in_file, out_file, template_dir, json_path) + + # path Nonlinear LS cost function + if acados_ocp.cost.cost_type == 'NONLINEAR_LS': + template_dir = os.path.join(code_export_dir, name + '_cost') + in_file = 'cost_y_fun.in.h' + out_file = f'{name}_cost_y_fun.h' + render_template(in_file, out_file, template_dir, json_path) + + # terminal Nonlinear LS cost function + if acados_ocp.cost.cost_type_e == 'NONLINEAR_LS': + template_dir = os.path.join(code_export_dir, name + '_cost') + in_file = 'cost_y_e_fun.in.h' + out_file = f'{name}_cost_y_e_fun.h' + render_template(in_file, out_file, template_dir, json_path) + + # external cost + if acados_ocp.cost.cost_type == 'EXTERNAL': + template_dir = os.path.join(code_export_dir, name + '_cost') + in_file = 'external_cost.in.h' + out_file = f'{name}_external_cost.h' + render_template(in_file, out_file, template_dir, json_path) + + # external cost - terminal + if acados_ocp.cost.cost_type_e == 'EXTERNAL': + template_dir = os.path.join(code_export_dir, name + '_cost') + in_file = 'external_cost_e.in.h' + out_file = f'{name}_external_cost_e.h' + render_template(in_file, out_file, template_dir, json_path) + + +def remove_x0_elimination(acados_ocp): + acados_ocp.constraints.idxbxe_0 = np.zeros((0,)) + acados_ocp.dims.nbxe_0 = 0 + + +class AcadosOcpSolver: + """ + Class to interact with the acados ocp solver C object. + + :param acados_ocp: type :py:class:`~acados_template.acados_ocp.AcadosOcp` - description of the OCP for acados + :param json_file: name for the json file used to render the templated code - default: acados_ocp_nlp.json + :param simulink_opts: Options to configure Simulink S-function blocks, mainly to activate possible Inputs and Outputs + """ + if sys.platform=="win32": + from ctypes import wintypes + from ctypes import WinDLL + dlclose = WinDLL('kernel32', use_last_error=True).FreeLibrary + dlclose.argtypes = [wintypes.HMODULE] + else: + dlclose = CDLL(None).dlclose + dlclose.argtypes = [c_void_p] + + @classmethod + def generate(cls, acados_ocp, json_file='acados_ocp_nlp.json', simulink_opts=None, cmake_builder: CMakeBuilder = None): + """ + Generates the code for an acados OCP solver, given the description in acados_ocp. + :param acados_ocp: type AcadosOcp - description of the OCP for acados + :param json_file: name for the json file used to render the templated code - default: `acados_ocp_nlp.json` + :param simulink_opts: Options to configure Simulink S-function blocks, mainly to activate possible inputs and + outputs; default: `None` + :param cmake_builder: type :py:class:`~acados_template.builders.CMakeBuilder` generate a `CMakeLists.txt` and use + the `CMake` pipeline instead of a `Makefile` (`CMake` seems to be the better option in conjunction with + `MS Visual Studio`); default: `None` + """ + model = acados_ocp.model + acados_ocp.code_export_directory = os.path.abspath(acados_ocp.code_export_directory) + + if simulink_opts is None: + simulink_opts = get_simulink_default_opts() + + # make dims consistent + make_ocp_dims_consistent(acados_ocp) + + # module dependent post processing + if acados_ocp.solver_options.integrator_type == 'GNSF': + set_up_imported_gnsf_model(acados_ocp) + + if acados_ocp.solver_options.qp_solver == 'PARTIAL_CONDENSING_QPDUNES': + remove_x0_elimination(acados_ocp) + + # set integrator time automatically + acados_ocp.solver_options.Tsim = acados_ocp.solver_options.time_steps[0] + + # generate external functions + ocp_generate_external_functions(acados_ocp, model) + + # dump to json + ocp_formulation_json_dump(acados_ocp, simulink_opts, json_file) + + # render templates + ocp_render_templates(acados_ocp, json_file, cmake_builder=cmake_builder) + acados_ocp.json_file = json_file + + + @classmethod + def build(cls, code_export_dir, with_cython=False, cmake_builder: CMakeBuilder = None): + """ + Builds the code for an acados OCP solver, that has been generated in code_export_dir + :param code_export_dir: directory in which acados OCP solver has been generated, see generate() + :param with_cython: option indicating if the cython interface is build, default: False. + :param cmake_builder: type :py:class:`~acados_template.builders.CMakeBuilder` generate a `CMakeLists.txt` and use + the `CMake` pipeline instead of a `Makefile` (`CMake` seems to be the better option in conjunction with + `MS Visual Studio`); default: `None` + """ + code_export_dir = os.path.abspath(code_export_dir) + cwd=os.getcwd() + os.chdir(code_export_dir) + if with_cython: + os.system('make clean_ocp_cython') + os.system('make ocp_cython') + else: + if cmake_builder is not None: + cmake_builder.exec(code_export_dir) + else: + os.system('make clean_ocp_shared_lib') + os.system('make ocp_shared_lib') + os.chdir(cwd) + + + @classmethod + def create_cython_solver(cls, json_file): + """ + Returns an `AcadosOcpSolverCython` object. + + This is an alternative Cython based Python wrapper to the acados OCP solver in C. + This offers faster interaction with the solver, because getter and setter calls, which include shape checking are done in compiled C code. + + The default wrapper `AcadosOcpSolver` is based on ctypes. + """ + with open(json_file, 'r') as f: + acados_ocp_json = json.load(f) + code_export_directory = acados_ocp_json['code_export_directory'] + + importlib.invalidate_caches() + rel_code_export_directory = os.path.relpath(code_export_directory) + acados_ocp_solver_pyx = importlib.import_module(f'{rel_code_export_directory}.acados_ocp_solver_pyx') + + AcadosOcpSolverCython = getattr(acados_ocp_solver_pyx, 'AcadosOcpSolverCython') + return AcadosOcpSolverCython(acados_ocp_json['model']['name'], + acados_ocp_json['solver_options']['nlp_solver_type'], + acados_ocp_json['dims']['N']) + + + def __init__(self, acados_ocp, json_file='acados_ocp_nlp.json', simulink_opts=None, build=True, generate=True, cmake_builder: CMakeBuilder = None): + + self.solver_created = False + if generate: + self.generate(acados_ocp, json_file=json_file, simulink_opts=simulink_opts, cmake_builder=cmake_builder) + + # load json, store options in object + with open(json_file, 'r') as f: + acados_ocp_json = json.load(f) + self.N = acados_ocp_json['dims']['N'] + self.model_name = acados_ocp_json['model']['name'] + self.solver_options = acados_ocp_json['solver_options'] + + acados_lib_path = acados_ocp_json['acados_lib_path'] + code_export_directory = acados_ocp_json['code_export_directory'] + + if build: + self.build(code_export_directory, with_cython=False, cmake_builder=cmake_builder) + + # prepare library loading + lib_prefix = 'lib' + lib_ext = '.so' + if os.name == 'nt': + lib_prefix = '' + lib_ext = '' + # ToDo: check for mac + + # Load acados library to avoid unloading the library. + # This is necessary if acados was compiled with OpenMP, since the OpenMP threads can't be destroyed. + # Unloading a library which uses OpenMP results in a segfault (on any platform?). + # see [https://stackoverflow.com/questions/34439956/vc-crash-when-freeing-a-dll-built-with-openmp] + # or [https://python.hotexamples.com/examples/_ctypes/-/dlclose/python-dlclose-function-examples.html] + libacados_name = f'{lib_prefix}acados{lib_ext}' + libacados_filepath = os.path.join(acados_lib_path, libacados_name) + self.__acados_lib = CDLL(libacados_filepath) + # find out if acados was compiled with OpenMP + try: + self.__acados_lib_uses_omp = getattr(self.__acados_lib, 'omp_get_thread_num') is not None + except AttributeError as e: + self.__acados_lib_uses_omp = False + if self.__acados_lib_uses_omp: + print('acados was compiled with OpenMP.') + else: + print('acados was compiled without OpenMP.') + libacados_ocp_solver_name = f'{lib_prefix}acados_ocp_solver_{self.model_name}{lib_ext}' + self.shared_lib_name = os.path.join(code_export_directory, libacados_ocp_solver_name) + + # get shared_lib + self.shared_lib = CDLL(self.shared_lib_name) + + # create capsule + getattr(self.shared_lib, f"{self.model_name}_acados_create_capsule").restype = c_void_p + self.capsule = getattr(self.shared_lib, f"{self.model_name}_acados_create_capsule")() + + # create solver + getattr(self.shared_lib, f"{self.model_name}_acados_create").argtypes = [c_void_p] + getattr(self.shared_lib, f"{self.model_name}_acados_create").restype = c_int + assert getattr(self.shared_lib, f"{self.model_name}_acados_create")(self.capsule)==0 + self.solver_created = True + + # get pointers solver + self.__get_pointers_solver() + + self.status = 0 + + + def __get_pointers_solver(self): + """ + Private function to get the pointers for solver + """ + # get pointers solver + getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_opts").argtypes = [c_void_p] + getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_opts").restype = c_void_p + self.nlp_opts = getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_opts")(self.capsule) + + getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_dims").argtypes = [c_void_p] + getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_dims").restype = c_void_p + self.nlp_dims = getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_dims")(self.capsule) + + getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_config").argtypes = [c_void_p] + getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_config").restype = c_void_p + self.nlp_config = getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_config")(self.capsule) + + getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_out").argtypes = [c_void_p] + getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_out").restype = c_void_p + self.nlp_out = getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_out")(self.capsule) + + getattr(self.shared_lib, f"{self.model_name}_acados_get_sens_out").argtypes = [c_void_p] + getattr(self.shared_lib, f"{self.model_name}_acados_get_sens_out").restype = c_void_p + self.sens_out = getattr(self.shared_lib, f"{self.model_name}_acados_get_sens_out")(self.capsule) + + getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_in").argtypes = [c_void_p] + getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_in").restype = c_void_p + self.nlp_in = getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_in")(self.capsule) + + getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_solver").argtypes = [c_void_p] + getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_solver").restype = c_void_p + self.nlp_solver = getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_solver")(self.capsule) + + + def solve(self): + """ + Solve the ocp with current input. + """ + getattr(self.shared_lib, f"{self.model_name}_acados_solve").argtypes = [c_void_p] + getattr(self.shared_lib, f"{self.model_name}_acados_solve").restype = c_int + self.status = getattr(self.shared_lib, f"{self.model_name}_acados_solve")(self.capsule) + + return self.status + + + def reset(self): + """ + Sets current iterate to all zeros. + """ + getattr(self.shared_lib, f"{self.model_name}_acados_reset").argtypes = [c_void_p] + getattr(self.shared_lib, f"{self.model_name}_acados_reset").restype = c_int + getattr(self.shared_lib, f"{self.model_name}_acados_reset")(self.capsule) + + return + + + def set_new_time_steps(self, new_time_steps): + """ + Set new time steps. + Recreates the solver if N changes. + + :param new_time_steps: 1 dimensional np array of new time steps for the solver + + .. note:: This allows for different use-cases: either set a new size of time_steps or a new distribution of + the shooting nodes without changing the number, e.g., to reach a different final time. Both cases + do not require a new code export and compilation. + """ + + # unlikely but still possible + if not self.solver_created: + raise Exception('Solver was not yet created!') + + # check if time steps really changed in value + if np.array_equal(self.solver_options['time_steps'], new_time_steps): + return + + N = new_time_steps.size + new_time_steps_data = cast(new_time_steps.ctypes.data, POINTER(c_double)) + + # check if recreation of acados is necessary (no need to recreate acados if sizes are identical) + if len(self.solver_options['time_steps']) == N: + getattr(self.shared_lib, f"{self.model_name}_acados_update_time_steps").argtypes = [c_void_p, c_int, c_void_p] + getattr(self.shared_lib, f"{self.model_name}_acados_update_time_steps").restype = c_int + assert getattr(self.shared_lib, f"{self.model_name}_acados_update_time_steps")(self.capsule, N, new_time_steps_data) == 0 + else: # recreate the solver with the new time steps + self.solver_created = False + + # delete old memory (analog to __del__) + getattr(self.shared_lib, f"{self.model_name}_acados_free").argtypes = [c_void_p] + getattr(self.shared_lib, f"{self.model_name}_acados_free").restype = c_int + getattr(self.shared_lib, f"{self.model_name}_acados_free")(self.capsule) + + # create solver with new time steps + getattr(self.shared_lib, f"{self.model_name}_acados_create_with_discretization").argtypes = [c_void_p, c_int, c_void_p] + getattr(self.shared_lib, f"{self.model_name}_acados_create_with_discretization").restype = c_int + assert getattr(self.shared_lib, f"{self.model_name}_acados_create_with_discretization")(self.capsule, N, new_time_steps_data) == 0 + + self.solver_created = True + + # get pointers solver + self.__get_pointers_solver() + + # store time_steps, N + self.solver_options['time_steps'] = new_time_steps + self.N = N + self.solver_options['Tsim'] = self.solver_options['time_steps'][0] + + + def update_qp_solver_cond_N(self, qp_solver_cond_N: int): + """ + Recreate solver with new value `qp_solver_cond_N` with a partial condensing QP solver. + This function is relevant for code reuse, i.e., if either `set_new_time_steps(...)` is used or + the influence of a different `qp_solver_cond_N` is studied without code export and compilation. + :param qp_solver_cond_N: new number of condensing stages for the solver + + .. note:: This function can only be used in combination with a partial condensing QP solver. + + .. note:: After `set_new_time_steps(...)` is used and depending on the new number of time steps it might be + necessary to change `qp_solver_cond_N` as well (using this function), i.e., typically + `qp_solver_cond_N < N`. + """ + # unlikely but still possible + if not self.solver_created: + raise Exception('Solver was not yet created!') + if self.N < qp_solver_cond_N: + raise Exception('Setting qp_solver_cond_N to be larger than N does not work!') + if self.solver_options['qp_solver_cond_N'] != qp_solver_cond_N: + self.solver_created = False + + # recreate the solver + fun_name = f'{self.model_name}_acados_update_qp_solver_cond_N' + getattr(self.shared_lib, fun_name).argtypes = [c_void_p, c_int] + getattr(self.shared_lib, fun_name).restype = c_int + assert getattr(self.shared_lib, fun_name)(self.capsule, qp_solver_cond_N) == 0 + + # store the new value + self.solver_options['qp_solver_cond_N'] = qp_solver_cond_N + self.solver_created = True + + # get pointers solver + self.__get_pointers_solver() + + + def eval_param_sens(self, index, stage=0, field="ex"): + """ + Calculate the sensitivity of the curent solution with respect to the initial state component of index + + :param index: integer corresponding to initial state index in range(nx) + """ + + field_ = field + field = field_.encode('utf-8') + + # checks + if not isinstance(index, int): + raise Exception('AcadosOcpSolver.eval_param_sens(): index must be Integer.') + + self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = [c_void_p, c_void_p, c_void_p, c_int, c_char_p] + self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int + nx = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, 0, "x".encode('utf-8')) + + if index < 0 or index > nx: + raise Exception(f'AcadosOcpSolver.eval_param_sens(): index must be in [0, nx-1], got: {index}.') + + # actual eval_param + self.shared_lib.ocp_nlp_eval_param_sens.argtypes = [c_void_p, c_char_p, c_int, c_int, c_void_p] + self.shared_lib.ocp_nlp_eval_param_sens.restype = None + self.shared_lib.ocp_nlp_eval_param_sens(self.nlp_solver, field, stage, index, self.sens_out) + + return + + + def get(self, stage_, field_): + """ + Get the last solution of the solver: + + :param stage: integer corresponding to shooting node + :param field: string in ['x', 'u', 'z', 'pi', 'lam', 't', 'sl', 'su',] + + .. note:: regarding lam, t: \n + the inequalities are internally organized in the following order: \n + [ lbu lbx lg lh lphi ubu ubx ug uh uphi; \n + lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi] + + .. note:: pi: multipliers for dynamics equality constraints \n + lam: multipliers for inequalities \n + t: slack variables corresponding to evaluation of all inequalities (at the solution) \n + sl: slack variables of soft lower inequality constraints \n + su: slack variables of soft upper inequality constraints \n + """ + + out_fields = ['x', 'u', 'z', 'pi', 'lam', 't', 'sl', 'su'] + # mem_fields = ['sl', 'su'] + sens_fields = ['sens_u', "sens_x"] + all_fields = out_fields + sens_fields + + field = field_ + + if (field_ not in all_fields): + raise Exception('AcadosOcpSolver.get(): {} is an invalid argument.\ + \n Possible values are {}. Exiting.'.format(field_, all_fields)) + + if not isinstance(stage_, int): + raise Exception('AcadosOcpSolver.get(): stage index must be Integer.') + + if stage_ < 0 or stage_ > self.N: + raise Exception('AcadosOcpSolver.get(): stage index must be in [0, N], got: {}.'.format(stage_)) + + if stage_ == self.N and field_ == 'pi': + raise Exception('AcadosOcpSolver.get(): field {} does not exist at final stage {}.'\ + .format(field_, stage_)) + + if field_ in sens_fields: + field = field_.replace('sens_', '') + + field = field.encode('utf-8') + + self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = \ + [c_void_p, c_void_p, c_void_p, c_int, c_char_p] + self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int + + dims = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, \ + self.nlp_dims, self.nlp_out, stage_, field) + + out = np.ascontiguousarray(np.zeros((dims,)), dtype=np.float64) + out_data = cast(out.ctypes.data, POINTER(c_double)) + + if (field_ in out_fields): + self.shared_lib.ocp_nlp_out_get.argtypes = \ + [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] + self.shared_lib.ocp_nlp_out_get(self.nlp_config, \ + self.nlp_dims, self.nlp_out, stage_, field, out_data) + # elif field_ in mem_fields: + # self.shared_lib.ocp_nlp_get_at_stage.argtypes = \ + # [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] + # self.shared_lib.ocp_nlp_get_at_stage(self.nlp_config, \ + # self.nlp_dims, self.nlp_solver, stage_, field, out_data) + elif field_ in sens_fields: + self.shared_lib.ocp_nlp_out_get.argtypes = \ + [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] + self.shared_lib.ocp_nlp_out_get(self.nlp_config, \ + self.nlp_dims, self.sens_out, stage_, field, out_data) + + return out + + + def print_statistics(self): + """ + prints statistics of previous solver run as a table: + - iter: iteration number + - res_stat: stationarity residual + - res_eq: residual wrt equality constraints (dynamics) + - res_ineq: residual wrt inequality constraints (constraints) + - res_comp: residual wrt complementarity conditions + - qp_stat: status of QP solver + - qp_iter: number of QP iterations + - alpha: SQP step size + - qp_res_stat: stationarity residual of the last QP solution + - qp_res_eq: residual wrt equality constraints (dynamics) of the last QP solution + - qp_res_ineq: residual wrt inequality constraints (constraints) of the last QP solution + - qp_res_comp: residual wrt complementarity conditions of the last QP solution + """ + stat = self.get_stats("statistics") + + if self.solver_options['nlp_solver_type'] == 'SQP': + print('\niter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter\talpha') + if stat.shape[0]>8: + print('\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp') + for jj in range(stat.shape[1]): + print(f'{int(stat[0][jj]):d}\t{stat[1][jj]:e}\t{stat[2][jj]:e}\t{stat[3][jj]:e}\t' + + f'{stat[4][jj]:e}\t{int(stat[5][jj]):d}\t{int(stat[6][jj]):d}\t{stat[7][jj]:e}\t') + if stat.shape[0]>8: + print('\t{:e}\t{:e}\t{:e}\t{:e}'.format( \ + stat[8][jj], stat[9][jj], stat[10][jj], stat[11][jj])) + print('\n') + elif self.solver_options['nlp_solver_type'] == 'SQP_RTI': + print('\niter\tqp_stat\tqp_iter') + if stat.shape[0]>3: + print('\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp') + for jj in range(stat.shape[1]): + print('{:d}\t{:d}\t{:d}'.format( int(stat[0][jj]), int(stat[1][jj]), int(stat[2][jj]))) + if stat.shape[0]>3: + print('\t{:e}\t{:e}\t{:e}\t{:e}'.format( \ + stat[3][jj], stat[4][jj], stat[5][jj], stat[6][jj])) + print('\n') + + return + + + def store_iterate(self, filename='', overwrite=False): + """ + Stores the current iterate of the ocp solver in a json file. + + :param filename: if not set, use model_name + timestamp + '.json' + :param overwrite: if false and filename exists add timestamp to filename + """ + if filename == '': + filename += self.model_name + '_' + 'iterate' + '.json' + + if not overwrite: + # append timestamp + if os.path.isfile(filename): + filename = filename[:-5] + filename += datetime.utcnow().strftime('%Y-%m-%d-%H:%M:%S.%f') + '.json' + + # get iterate: + solution = dict() + + for i in range(self.N+1): + solution['x_'+str(i)] = self.get(i,'x') + solution['u_'+str(i)] = self.get(i,'u') + solution['z_'+str(i)] = self.get(i,'z') + solution['lam_'+str(i)] = self.get(i,'lam') + solution['t_'+str(i)] = self.get(i, 't') + solution['sl_'+str(i)] = self.get(i, 'sl') + solution['su_'+str(i)] = self.get(i, 'su') + for i in range(self.N): + solution['pi_'+str(i)] = self.get(i,'pi') + + # save + with open(filename, 'w') as f: + json.dump(solution, f, default=np_array_to_list, indent=4, sort_keys=True) + print("stored current iterate in ", os.path.join(os.getcwd(), filename)) + + + def load_iterate(self, filename): + """ + Loads the iterate stored in json file with filename into the ocp solver. + """ + if not os.path.isfile(filename): + raise Exception('load_iterate: failed, file does not exist: ' + os.path.join(os.getcwd(), filename)) + + with open(filename, 'r') as f: + solution = json.load(f) + + print(f"loading iterate {filename}") + for key in solution.keys(): + (field, stage) = key.split('_') + self.set(int(stage), field, np.array(solution[key])) + + + def get_stats(self, field_): + """ + Get the information of the last solver call. + + :param field: string in ['statistics', 'time_tot', 'time_lin', 'time_sim', 'time_sim_ad', 'time_sim_la', 'time_qp', 'time_qp_solver_call', 'time_reg', 'sqp_iter', 'residuals', 'qp_iter', 'alpha'] + + Available fileds: + - time_tot: total CPU time previous call + - time_lin: CPU time for linearization + - time_sim: CPU time for integrator + - time_sim_ad: CPU time for integrator contribution of external function calls + - time_sim_la: CPU time for integrator contribution of linear algebra + - time_qp: CPU time qp solution + - time_qp_solver_call: CPU time inside qp solver (without converting the QP) + - time_qp_xcond: time_glob: CPU time globalization + - time_solution_sensitivities: CPU time for previous call to eval_param_sens + - time_reg: CPU time regularization + - sqp_iter: number of SQP iterations + - qp_iter: vector of QP iterations for last SQP call + - statistics: table with info about last iteration + - stat_m: number of rows in statistics matrix + - stat_n: number of columns in statistics matrix + - residuals: residuals of last iterate + - alpha: step sizes of SQP iterations + """ + + double_fields = ['time_tot', + 'time_lin', + 'time_sim', + 'time_sim_ad', + 'time_sim_la', + 'time_qp', + 'time_qp_solver_call', + 'time_qp_xcond', + 'time_glob', + 'time_solution_sensitivities', + 'time_reg' + ] + fields = double_fields + [ + 'sqp_iter', + 'qp_iter', + 'statistics', + 'stat_m', + 'stat_n', + 'residuals', + 'alpha', + ] + field = field_.encode('utf-8') + + + if field_ in ['sqp_iter', 'stat_m', 'stat_n']: + out = np.ascontiguousarray(np.zeros((1,)), dtype=np.int64) + out_data = cast(out.ctypes.data, POINTER(c_int64)) + self.shared_lib.ocp_nlp_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p] + self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data) + return out + + # TODO: just return double instead of np. + elif field_ in double_fields: + out = np.zeros((1,)) + out_data = cast(out.ctypes.data, POINTER(c_double)) + self.shared_lib.ocp_nlp_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p] + self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data) + return out + + elif field_ == 'statistics': + sqp_iter = self.get_stats("sqp_iter") + stat_m = self.get_stats("stat_m") + stat_n = self.get_stats("stat_n") + min_size = min([stat_m, sqp_iter+1]) + out = np.ascontiguousarray( + np.zeros((stat_n[0]+1, min_size[0])), dtype=np.float64) + out_data = cast(out.ctypes.data, POINTER(c_double)) + self.shared_lib.ocp_nlp_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p] + self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data) + return out + + elif field_ == 'qp_iter': + full_stats = self.get_stats('statistics') + if self.solver_options['nlp_solver_type'] == 'SQP': + return full_stats[6, :] + elif self.solver_options['nlp_solver_type'] == 'SQP_RTI': + return full_stats[2, :] + + elif field_ == 'alpha': + full_stats = self.get_stats('statistics') + if self.solver_options['nlp_solver_type'] == 'SQP': + return full_stats[7, :] + else: # self.solver_options['nlp_solver_type'] == 'SQP_RTI': + raise Exception("alpha values are not available for SQP_RTI") + + elif field_ == 'residuals': + return self.get_residuals() + + else: + raise Exception(f'AcadosOcpSolver.get_stats(): {field} is not a valid argument.' + + f'\n Possible values are {fields}.') + + + def get_cost(self): + """ + Returns the cost value of the current solution. + """ + # compute cost internally + self.shared_lib.ocp_nlp_eval_cost.argtypes = [c_void_p, c_void_p, c_void_p] + self.shared_lib.ocp_nlp_eval_cost(self.nlp_solver, self.nlp_in, self.nlp_out) + + # create output array + out = np.ascontiguousarray(np.zeros((1,)), dtype=np.float64) + out_data = cast(out.ctypes.data, POINTER(c_double)) + + # call getter + self.shared_lib.ocp_nlp_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p] + + field = "cost_value".encode('utf-8') + self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data) + + return out[0] + + + def get_residuals(self, recompute=False): + """ + Returns an array of the form [res_stat, res_eq, res_ineq, res_comp]. + """ + # compute residuals if RTI + if self.solver_options['nlp_solver_type'] == 'SQP_RTI' or recompute: + self.shared_lib.ocp_nlp_eval_residuals.argtypes = [c_void_p, c_void_p, c_void_p] + self.shared_lib.ocp_nlp_eval_residuals(self.nlp_solver, self.nlp_in, self.nlp_out) + + # create output array + out = np.ascontiguousarray(np.zeros((4, 1)), dtype=np.float64) + out_data = cast(out.ctypes.data, POINTER(c_double)) + + # call getters + self.shared_lib.ocp_nlp_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p] + + field = "res_stat".encode('utf-8') + self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data) + + out_data = cast(out[1].ctypes.data, POINTER(c_double)) + field = "res_eq".encode('utf-8') + self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data) + + out_data = cast(out[2].ctypes.data, POINTER(c_double)) + field = "res_ineq".encode('utf-8') + self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data) + + out_data = cast(out[3].ctypes.data, POINTER(c_double)) + field = "res_comp".encode('utf-8') + self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data) + return out.flatten() + + + # Note: this function should not be used anymore, better use cost_set, constraints_set + def set(self, stage_, field_, value_): + """ + Set numerical data inside the solver. + + :param stage: integer corresponding to shooting node + :param field: string in ['x', 'u', 'pi', 'lam', 't', 'p', 'xdot_guess', 'z_guess'] + + .. note:: regarding lam, t: \n + the inequalities are internally organized in the following order: \n + [ lbu lbx lg lh lphi ubu ubx ug uh uphi; \n + lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi] + + .. note:: pi: multipliers for dynamics equality constraints \n + lam: multipliers for inequalities \n + t: slack variables corresponding to evaluation of all inequalities (at the solution) \n + sl: slack variables of soft lower inequality constraints \n + su: slack variables of soft upper inequality constraints \n + """ + cost_fields = ['y_ref', 'yref'] + constraints_fields = ['lbx', 'ubx', 'lbu', 'ubu'] + out_fields = ['x', 'u', 'pi', 'lam', 't', 'z', 'sl', 'su'] + mem_fields = ['xdot_guess', 'z_guess'] + + # cast value_ to avoid conversion issues + if isinstance(value_, (float, int)): + value_ = np.array([value_]) + value_ = value_.astype(float) + + field = field_ + field = field.encode('utf-8') + + stage = c_int(stage_) + + # treat parameters separately + if field_ == 'p': + getattr(self.shared_lib, f"{self.model_name}_acados_update_params").argtypes = [c_void_p, c_int, POINTER(c_double)] + getattr(self.shared_lib, f"{self.model_name}_acados_update_params").restype = c_int + + value_data = cast(value_.ctypes.data, POINTER(c_double)) + + assert getattr(self.shared_lib, f"{self.model_name}_acados_update_params")(self.capsule, stage, value_data, value_.shape[0])==0 + else: + if field_ not in constraints_fields + cost_fields + out_fields: + raise Exception("AcadosOcpSolver.set(): {} is not a valid argument.\ + \nPossible values are {}. Exiting.".format(field, \ + constraints_fields + cost_fields + out_fields + ['p'])) + + self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = \ + [c_void_p, c_void_p, c_void_p, c_int, c_char_p] + self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int + + dims = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, \ + self.nlp_dims, self.nlp_out, stage_, field) + + if value_.shape[0] != dims: + msg = 'AcadosOcpSolver.set(): mismatching dimension for field "{}" '.format(field_) + msg += 'with dimension {} (you have {})'.format(dims, value_.shape[0]) + raise Exception(msg) + + value_data = cast(value_.ctypes.data, POINTER(c_double)) + value_data_p = cast((value_data), c_void_p) + + if field_ in constraints_fields: + self.shared_lib.ocp_nlp_constraints_model_set.argtypes = \ + [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] + self.shared_lib.ocp_nlp_constraints_model_set(self.nlp_config, \ + self.nlp_dims, self.nlp_in, stage, field, value_data_p) + elif field_ in cost_fields: + self.shared_lib.ocp_nlp_cost_model_set.argtypes = \ + [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] + self.shared_lib.ocp_nlp_cost_model_set(self.nlp_config, \ + self.nlp_dims, self.nlp_in, stage, field, value_data_p) + elif field_ in out_fields: + self.shared_lib.ocp_nlp_out_set.argtypes = \ + [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] + self.shared_lib.ocp_nlp_out_set(self.nlp_config, \ + self.nlp_dims, self.nlp_out, stage, field, value_data_p) + elif field_ in mem_fields: + self.shared_lib.ocp_nlp_set.argtypes = \ + [c_void_p, c_void_p, c_int, c_char_p, c_void_p] + self.shared_lib.ocp_nlp_set(self.nlp_config, \ + self.nlp_solver, stage, field, value_data_p) + return + + + def cost_set(self, stage_, field_, value_, api='warn'): + """ + Set numerical data in the cost module of the solver. + + :param stage: integer corresponding to shooting node + :param field: string, e.g. 'yref', 'W', 'ext_cost_num_hess' + :param value: of appropriate size + """ + # cast value_ to avoid conversion issues + if isinstance(value_, (float, int)): + value_ = np.array([value_]) + value_ = value_.astype(float) + + field = field_ + field = field.encode('utf-8') + + stage = c_int(stage_) + self.shared_lib.ocp_nlp_cost_dims_get_from_attr.argtypes = \ + [c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)] + self.shared_lib.ocp_nlp_cost_dims_get_from_attr.restype = c_int + + dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc) + dims_data = cast(dims.ctypes.data, POINTER(c_int)) + + self.shared_lib.ocp_nlp_cost_dims_get_from_attr(self.nlp_config, \ + self.nlp_dims, self.nlp_out, stage_, field, dims_data) + + value_shape = value_.shape + if len(value_shape) == 1: + value_shape = (value_shape[0], 0) + + elif len(value_shape) == 2: + if api=='old': + pass + elif api=='warn': + if not np.all(np.ravel(value_, order='F')==np.ravel(value_, order='K')): + raise Exception("Ambiguity in API detected.\n" + "Are you making an acados model from scrach? Add api='new' to cost_set and carry on.\n" + "Are you seeing this error suddenly in previously running code? Read on.\n" + " You are relying on a now-fixed bug in cost_set for field '{}'.\n".format(field_) + + " acados_template now correctly passes on any matrices to acados in column major format.\n" + + " Two options to fix this error: \n" + + " * Add api='old' to cost_set to restore old incorrect behaviour\n" + + " * Add api='new' to cost_set and remove any unnatural manipulation of the value argument " + + "such as non-mathematical transposes, reshaping, casting to fortran order, etc... " + + "If there is no such manipulation, then you have probably been getting an incorrect solution before.") + # Get elements in column major order + value_ = np.ravel(value_, order='F') + elif api=='new': + # Get elements in column major order + value_ = np.ravel(value_, order='F') + else: + raise Exception("Unknown api: '{}'".format(api)) + + if value_shape != tuple(dims): + raise Exception('AcadosOcpSolver.cost_set(): mismatching dimension' + + f' for field "{field_}" at stage {stage} with dimension {tuple(dims)} (you have {value_shape})') + + value_data = cast(value_.ctypes.data, POINTER(c_double)) + value_data_p = cast((value_data), c_void_p) + + self.shared_lib.ocp_nlp_cost_model_set.argtypes = \ + [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] + self.shared_lib.ocp_nlp_cost_model_set(self.nlp_config, \ + self.nlp_dims, self.nlp_in, stage, field, value_data_p) + + return + + + def constraints_set(self, stage_, field_, value_, api='warn'): + """ + Set numerical data in the constraint module of the solver. + + :param stage: integer corresponding to shooting node + :param field: string in ['lbx', 'ubx', 'lbu', 'ubu', 'lg', 'ug', 'lh', 'uh', 'uphi', 'C', 'D'] + :param value: of appropriate size + """ + # cast value_ to avoid conversion issues + if isinstance(value_, (float, int)): + value_ = np.array([value_]) + value_ = value_.astype(float) + + field = field_ + field = field.encode('utf-8') + + stage = c_int(stage_) + self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.argtypes = \ + [c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)] + self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.restype = c_int + + dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc) + dims_data = cast(dims.ctypes.data, POINTER(c_int)) + + self.shared_lib.ocp_nlp_constraint_dims_get_from_attr(self.nlp_config, \ + self.nlp_dims, self.nlp_out, stage_, field, dims_data) + + value_shape = value_.shape + if len(value_shape) == 1: + value_shape = (value_shape[0], 0) + elif len(value_shape) == 2: + if api=='old': + pass + elif api=='warn': + if not np.all(np.ravel(value_, order='F')==np.ravel(value_, order='K')): + raise Exception("Ambiguity in API detected.\n" + "Are you making an acados model from scrach? Add api='new' to constraints_set and carry on.\n" + "Are you seeing this error suddenly in previously running code? Read on.\n" + " You are relying on a now-fixed bug in constraints_set for field '{}'.\n".format(field_) + + " acados_template now correctly passes on any matrices to acados in column major format.\n" + + " Two options to fix this error: \n" + + " * Add api='old' to constraints_set to restore old incorrect behaviour\n" + + " * Add api='new' to constraints_set and remove any unnatural manipulation of the value argument " + + "such as non-mathematical transposes, reshaping, casting to fortran order, etc... " + + "If there is no such manipulation, then you have probably been getting an incorrect solution before.") + # Get elements in column major order + value_ = np.ravel(value_, order='F') + elif api=='new': + # Get elements in column major order + value_ = np.ravel(value_, order='F') + else: + raise Exception("Unknown api: '{}'".format(api)) + + if value_shape != tuple(dims): + raise Exception(f'AcadosOcpSolver.constraints_set(): mismatching dimension' + + f' for field "{field_}" at stage {stage} with dimension {tuple(dims)} (you have {value_shape})') + + value_data = cast(value_.ctypes.data, POINTER(c_double)) + value_data_p = cast((value_data), c_void_p) + + self.shared_lib.ocp_nlp_constraints_model_set.argtypes = \ + [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] + self.shared_lib.ocp_nlp_constraints_model_set(self.nlp_config, \ + self.nlp_dims, self.nlp_in, stage, field, value_data_p) + + return + + + def dynamics_get(self, stage_, field_): + """ + Get numerical data from the dynamics module of the solver: + + :param stage: integer corresponding to shooting node + :param field: string, e.g. 'A' + """ + + field = field_ + field = field.encode('utf-8') + stage = c_int(stage_) + + # get dims + self.shared_lib.ocp_nlp_dynamics_dims_get_from_attr.argtypes = \ + [c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)] + self.shared_lib.ocp_nlp_dynamics_dims_get_from_attr.restype = c_int + + dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc) + dims_data = cast(dims.ctypes.data, POINTER(c_int)) + + self.shared_lib.ocp_nlp_dynamics_dims_get_from_attr(self.nlp_config, \ + self.nlp_dims, self.nlp_out, stage_, field, dims_data) + + # create output data + out = np.ascontiguousarray(np.zeros((np.prod(dims),)), dtype=np.float64) + out = out.reshape(dims[0], dims[1], order='F') + + out_data = cast(out.ctypes.data, POINTER(c_double)) + out_data_p = cast((out_data), c_void_p) + + # call getter + self.shared_lib.ocp_nlp_get_at_stage.argtypes = \ + [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] + self.shared_lib.ocp_nlp_get_at_stage(self.nlp_config, \ + self.nlp_dims, self.nlp_solver, stage, field, out_data_p) + + return out + + + def options_set(self, field_, value_): + """ + Set options of the solver. + + :param field: string, e.g. 'print_level', 'rti_phase', 'initialize_t_slacks', 'step_length', 'alpha_min', 'alpha_reduction', 'qp_warm_start', 'line_search_use_sufficient_descent', 'full_step_dual', 'globalization_use_SOC', 'qp_tol_stat', 'qp_tol_eq', 'qp_tol_ineq', 'qp_tol_comp', 'qp_tau_min', 'qp_mu0' + + :param value: of type int, float, string + + - qp_tol_stat: QP solver tolerance stationarity + - qp_tol_eq: QP solver tolerance equalities + - qp_tol_ineq: QP solver tolerance inequalities + - qp_tol_comp: QP solver tolerance complementarity + - qp_tau_min: for HPIPM QP solvers: minimum value of barrier parameter in HPIPM + - qp_mu0: for HPIPM QP solvers: initial value for complementarity slackness + - warm_start_first_qp: indicates if first QP in SQP is warm_started + """ + int_fields = ['print_level', 'rti_phase', 'initialize_t_slacks', 'qp_warm_start', 'line_search_use_sufficient_descent', 'full_step_dual', 'globalization_use_SOC', 'warm_start_first_qp'] + double_fields = ['step_length', 'tol_eq', 'tol_stat', 'tol_ineq', 'tol_comp', 'alpha_min', 'alpha_reduction', 'eps_sufficient_descent', + 'qp_tol_stat', 'qp_tol_eq', 'qp_tol_ineq', 'qp_tol_comp', 'qp_tau_min', 'qp_mu0'] + string_fields = ['globalization'] + + # check field availability and type + if field_ in int_fields: + if not isinstance(value_, int): + raise Exception('solver option {} must be of type int. You have {}.'.format(field_, type(value_))) + else: + value_ctypes = c_int(value_) + + elif field_ in double_fields: + if not isinstance(value_, float): + raise Exception('solver option {} must be of type float. You have {}.'.format(field_, type(value_))) + else: + value_ctypes = c_double(value_) + + elif field_ in string_fields: + if not isinstance(value_, str): + raise Exception('solver option {} must be of type str. You have {}.'.format(field_, type(value_))) + else: + value_ctypes = value_.encode('utf-8') + else: + raise Exception('AcadosOcpSolver.options_set() does not support field {}.'\ + '\n Possible values are {}.'.format(field_, ', '.join(int_fields + double_fields + string_fields))) + + + if field_ == 'rti_phase': + if value_ < 0 or value_ > 2: + raise Exception('AcadosOcpSolver.options_set(): argument \'rti_phase\' can ' + 'take only values 0, 1, 2 for SQP-RTI-type solvers') + if self.solver_options['nlp_solver_type'] != 'SQP_RTI' and value_ > 0: + raise Exception('AcadosOcpSolver.options_set(): argument \'rti_phase\' can ' + 'take only value 0 for SQP-type solvers') + + # encode + field = field_ + field = field.encode('utf-8') + + # call C interface + if field_ in string_fields: + self.shared_lib.ocp_nlp_solver_opts_set.argtypes = \ + [c_void_p, c_void_p, c_char_p, c_char_p] + self.shared_lib.ocp_nlp_solver_opts_set(self.nlp_config, \ + self.nlp_opts, field, value_ctypes) + else: + self.shared_lib.ocp_nlp_solver_opts_set.argtypes = \ + [c_void_p, c_void_p, c_char_p, c_void_p] + self.shared_lib.ocp_nlp_solver_opts_set(self.nlp_config, \ + self.nlp_opts, field, byref(value_ctypes)) + return + + + def __del__(self): + if self.solver_created: + getattr(self.shared_lib, f"{self.model_name}_acados_free").argtypes = [c_void_p] + getattr(self.shared_lib, f"{self.model_name}_acados_free").restype = c_int + getattr(self.shared_lib, f"{self.model_name}_acados_free")(self.capsule) + + getattr(self.shared_lib, f"{self.model_name}_acados_free_capsule").argtypes = [c_void_p] + getattr(self.shared_lib, f"{self.model_name}_acados_free_capsule").restype = c_int + getattr(self.shared_lib, f"{self.model_name}_acados_free_capsule")(self.capsule) + + try: + self.dlclose(self.shared_lib._handle) + except: + pass diff --git a/third_party/acados/acados_template/acados_ocp_solver_pyx.pyx b/third_party/acados/acados_template/acados_ocp_solver_pyx.pyx new file mode 100644 index 0000000000..fe7fa8425a --- /dev/null +++ b/third_party/acados/acados_template/acados_ocp_solver_pyx.pyx @@ -0,0 +1,707 @@ +# -*- coding: future_fstrings -*- +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# +# cython: language_level=3 +# cython: profile=False +# distutils: language=c + +cimport cython +from libc cimport string + +cimport acados_solver_common +# TODO: make this import more clear? it is not a general solver, but problem specific. +cimport acados_solver + +cimport numpy as cnp + +import os +from datetime import datetime +import numpy as np + + +cdef class AcadosOcpSolverCython: + """ + Class to interact with the acados ocp solver C object. + """ + + cdef acados_solver.nlp_solver_capsule *capsule + cdef void *nlp_opts + cdef acados_solver_common.ocp_nlp_dims *nlp_dims + cdef acados_solver_common.ocp_nlp_config *nlp_config + cdef acados_solver_common.ocp_nlp_out *nlp_out + cdef acados_solver_common.ocp_nlp_out *sens_out + cdef acados_solver_common.ocp_nlp_in *nlp_in + cdef acados_solver_common.ocp_nlp_solver *nlp_solver + + cdef int status + cdef bint solver_created + + cdef str model_name + cdef int N + + cdef str nlp_solver_type + + def __cinit__(self, model_name, nlp_solver_type, N): + + self.solver_created = False + + self.N = N + self.model_name = model_name + self.nlp_solver_type = nlp_solver_type + + # create capsule + self.capsule = acados_solver.acados_create_capsule() + + # create solver + assert acados_solver.acados_create(self.capsule) == 0 + self.solver_created = True + + # get pointers solver + self.__get_pointers_solver() + self.status = 0 + + + def __get_pointers_solver(self): + """ + Private function to get the pointers for solver + """ + # get pointers solver + self.nlp_opts = acados_solver.acados_get_nlp_opts(self.capsule) + self.nlp_dims = acados_solver.acados_get_nlp_dims(self.capsule) + self.nlp_config = acados_solver.acados_get_nlp_config(self.capsule) + self.nlp_out = acados_solver.acados_get_nlp_out(self.capsule) + self.sens_out = acados_solver.acados_get_sens_out(self.capsule) + self.nlp_in = acados_solver.acados_get_nlp_in(self.capsule) + self.nlp_solver = acados_solver.acados_get_nlp_solver(self.capsule) + + + def solve(self): + """ + Solve the ocp with current input. + """ + return acados_solver.acados_solve(self.capsule) + + + def reset(self): + """ + Sets current iterate to all zeros. + """ + return acados_solver.acados_reset(self.capsule) + + + def set_new_time_steps(self, new_time_steps): + """ + Set new time steps. + Recreates the solver if N changes. + + :param new_time_steps: 1 dimensional np array of new time steps for the solver + + .. note:: This allows for different use-cases: either set a new size of time-steps or a new distribution of + the shooting nodes without changing the number, e.g., to reach a different final time. Both cases + do not require a new code export and compilation. + """ + + raise NotImplementedError("AcadosOcpSolverCython: does not support set_new_time_steps() since it is only a prototyping feature") + # # unlikely but still possible + # if not self.solver_created: + # raise Exception('Solver was not yet created!') + + # ## check if time steps really changed in value + # # get time steps + # cdef cnp.ndarray[cnp.float64_t, ndim=1] old_time_steps = np.ascontiguousarray(np.zeros((self.N,)), dtype=np.float64) + # assert acados_solver.acados_get_time_steps(self.capsule, self.N, old_time_steps.data) + + # if np.array_equal(old_time_steps, new_time_steps): + # return + + # N = new_time_steps.size + # cdef cnp.ndarray[cnp.float64_t, ndim=1] value = np.ascontiguousarray(new_time_steps, dtype=np.float64) + + # # check if recreation of acados is necessary (no need to recreate acados if sizes are identical) + # if len(old_time_steps) == N: + # assert acados_solver.acados_update_time_steps(self.capsule, N, value.data) == 0 + + # else: # recreate the solver with the new time steps + # self.solver_created = False + + # # delete old memory (analog to __del__) + # acados_solver.acados_free(self.capsule) + + # # create solver with new time steps + # assert acados_solver.acados_create_with_discretization(self.capsule, N, value.data) == 0 + + # self.solver_created = True + + # # get pointers solver + # self.__get_pointers_solver() + + # # store time_steps, N + # self.time_steps = new_time_steps + # self.N = N + + + def update_qp_solver_cond_N(self, qp_solver_cond_N: int): + """ + Recreate solver with new value `qp_solver_cond_N` with a partial condensing QP solver. + This function is relevant for code reuse, i.e., if either `set_new_time_steps(...)` is used or + the influence of a different `qp_solver_cond_N` is studied without code export and compilation. + :param qp_solver_cond_N: new number of condensing stages for the solver + + .. note:: This function can only be used in combination with a partial condensing QP solver. + + .. note:: After `set_new_time_steps(...)` is used and depending on the new number of time steps it might be + necessary to change `qp_solver_cond_N` as well (using this function), i.e., typically + `qp_solver_cond_N < N`. + """ + raise NotImplementedError("AcadosOcpSolverCython: does not support update_qp_solver_cond_N() since it is only a prototyping feature") + + # # unlikely but still possible + # if not self.solver_created: + # raise Exception('Solver was not yet created!') + # if self.N < qp_solver_cond_N: + # raise Exception('Setting qp_solver_cond_N to be larger than N does not work!') + # if self.qp_solver_cond_N != qp_solver_cond_N: + # self.solver_created = False + + # # recreate the solver + # acados_solver.acados_update_qp_solver_cond_N(self.capsule, qp_solver_cond_N) + + # # store the new value + # self.qp_solver_cond_N = qp_solver_cond_N + # self.solver_created = True + + # # get pointers solver + # self.__get_pointers_solver() + + + def eval_param_sens(self, index, stage=0, field="ex"): + """ + Calculate the sensitivity of the curent solution with respect to the initial state component of index + + :param index: integer corresponding to initial state index in range(nx) + """ + + field_ = field + field = field_.encode('utf-8') + + # checks + if not isinstance(index, int): + raise Exception('AcadosOcpSolverCython.eval_param_sens(): index must be Integer.') + + cdef int nx = acados_solver_common.ocp_nlp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, 0, "x".encode('utf-8')) + + if index < 0 or index > nx: + raise Exception(f'AcadosOcpSolverCython.eval_param_sens(): index must be in [0, nx-1], got: {index}.') + + # actual eval_param + acados_solver_common.ocp_nlp_eval_param_sens(self.nlp_solver, field, stage, index, self.sens_out) + + return + + + def get(self, int stage, str field_): + """ + Get the last solution of the solver: + + :param stage: integer corresponding to shooting node + :param field: string in ['x', 'u', 'z', 'pi', 'lam', 't', 'sl', 'su',] + + .. note:: regarding lam, t: \n + the inequalities are internally organized in the following order: \n + [ lbu lbx lg lh lphi ubu ubx ug uh uphi; \n + lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi] + + .. note:: pi: multipliers for dynamics equality constraints \n + lam: multipliers for inequalities \n + t: slack variables corresponding to evaluation of all inequalities (at the solution) \n + sl: slack variables of soft lower inequality constraints \n + su: slack variables of soft upper inequality constraints \n + """ + + out_fields = ['x', 'u', 'z', 'pi', 'lam', 't', 'sl', 'su'] + field = field_.encode('utf-8') + + if field_ not in out_fields: + raise Exception('AcadosOcpSolverCython.get(): {} is an invalid argument.\ + \n Possible values are {}. Exiting.'.format(field_, out_fields)) + + if stage < 0 or stage > self.N: + raise Exception('AcadosOcpSolverCython.get(): stage index must be in [0, N], got: {}.'.format(self.N)) + + if stage == self.N and field_ == 'pi': + raise Exception('AcadosOcpSolverCython.get(): field {} does not exist at final stage {}.'\ + .format(field_, stage)) + + cdef int dims = acados_solver_common.ocp_nlp_dims_get_from_attr(self.nlp_config, + self.nlp_dims, self.nlp_out, stage, field) + + cdef cnp.ndarray[cnp.float64_t, ndim=1] out = np.zeros((dims,)) + acados_solver_common.ocp_nlp_out_get(self.nlp_config, \ + self.nlp_dims, self.nlp_out, stage, field, out.data) + + return out + + + def print_statistics(self): + """ + prints statistics of previous solver run as a table: + - iter: iteration number + - res_stat: stationarity residual + - res_eq: residual wrt equality constraints (dynamics) + - res_ineq: residual wrt inequality constraints (constraints) + - res_comp: residual wrt complementarity conditions + - qp_stat: status of QP solver + - qp_iter: number of QP iterations + - qp_res_stat: stationarity residual of the last QP solution + - qp_res_eq: residual wrt equality constraints (dynamics) of the last QP solution + - qp_res_ineq: residual wrt inequality constraints (constraints) of the last QP solution + - qp_res_comp: residual wrt complementarity conditions of the last QP solution + """ + acados_solver.acados_print_stats(self.capsule) + + + def store_iterate(self, filename='', overwrite=False): + """ + Stores the current iterate of the ocp solver in a json file. + + :param filename: if not set, use model_name + timestamp + '.json' + :param overwrite: if false and filename exists add timestamp to filename + """ + import json + if filename == '': + filename += self.model_name + '_' + 'iterate' + '.json' + + if not overwrite: + # append timestamp + if os.path.isfile(filename): + filename = filename[:-5] + filename += datetime.utcnow().strftime('%Y-%m-%d-%H:%M:%S.%f') + '.json' + + # get iterate: + solution = dict() + + for i in range(self.N+1): + solution['x_'+str(i)] = self.get(i,'x') + solution['u_'+str(i)] = self.get(i,'u') + solution['z_'+str(i)] = self.get(i,'z') + solution['lam_'+str(i)] = self.get(i,'lam') + solution['t_'+str(i)] = self.get(i, 't') + solution['sl_'+str(i)] = self.get(i, 'sl') + solution['su_'+str(i)] = self.get(i, 'su') + for i in range(self.N): + solution['pi_'+str(i)] = self.get(i,'pi') + + # save + with open(filename, 'w') as f: + json.dump(solution, f, default=lambda x: x.tolist(), indent=4, sort_keys=True) + print("stored current iterate in ", os.path.join(os.getcwd(), filename)) + + + def load_iterate(self, filename): + """ + Loads the iterate stored in json file with filename into the ocp solver. + """ + import json + if not os.path.isfile(filename): + raise Exception('load_iterate: failed, file does not exist: ' + os.path.join(os.getcwd(), filename)) + + with open(filename, 'r') as f: + solution = json.load(f) + + for key in solution.keys(): + (field, stage) = key.split('_') + self.set(int(stage), field, np.array(solution[key])) + + + def get_stats(self, field_): + """ + Get the information of the last solver call. + + :param field: string in ['statistics', 'time_tot', 'time_lin', 'time_sim', 'time_sim_ad', 'time_sim_la', 'time_qp', 'time_qp_solver_call', 'time_reg', 'sqp_iter'] + Available fileds: + - time_tot: total CPU time previous call + - time_lin: CPU time for linearization + - time_sim: CPU time for integrator + - time_sim_ad: CPU time for integrator contribution of external function calls + - time_sim_la: CPU time for integrator contribution of linear algebra + - time_qp: CPU time qp solution + - time_qp_solver_call: CPU time inside qp solver (without converting the QP) + - time_qp_xcond: time_glob: CPU time globalization + - time_solution_sensitivities: CPU time for previous call to eval_param_sens + - time_reg: CPU time regularization + - sqp_iter: number of SQP iterations + - qp_iter: vector of QP iterations for last SQP call + - statistics: table with info about last iteration + - stat_m: number of rows in statistics matrix + - stat_n: number of columns in statistics matrix + - residuals: residuals of last iterate + - alpha: step sizes of SQP iterations + """ + + double_fields = ['time_tot', + 'time_lin', + 'time_sim', + 'time_sim_ad', + 'time_sim_la', + 'time_qp', + 'time_qp_solver_call', + 'time_qp_xcond', + 'time_glob', + 'time_solution_sensitivities', + 'time_reg' + ] + fields = double_fields + [ + 'sqp_iter', + 'qp_iter', + 'statistics', + 'stat_m', + 'stat_n', + 'residuals', + 'alpha', + ] + field = field_.encode('utf-8') + + if field_ in ['sqp_iter', 'stat_m', 'stat_n']: + return self.__get_stat_int(field) + + elif field_ in double_fields: + return self.__get_stat_double(field) + + elif field_ == 'statistics': + sqp_iter = self.get_stats("sqp_iter") + stat_m = self.get_stats("stat_m") + stat_n = self.get_stats("stat_n") + min_size = min([stat_m, sqp_iter+1]) + return self.__get_stat_matrix(field, stat_n+1, min_size) + + elif field_ == 'qp_iter': + full_stats = self.get_stats('statistics') + if self.nlp_solver_type == 'SQP': + return full_stats[6, :] + elif self.nlp_solver_type == 'SQP_RTI': + return full_stats[2, :] + + elif field_ == 'alpha': + full_stats = self.get_stats('statistics') + if self.nlp_solver_type == 'SQP': + return full_stats[7, :] + else: # self.nlp_solver_type == 'SQP_RTI': + raise Exception("alpha values are not available for SQP_RTI") + + elif field_ == 'residuals': + return self.get_residuals() + + else: + raise NotImplementedError("TODO!") + + + def __get_stat_int(self, field): + cdef int out + acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, &out) + return out + + def __get_stat_double(self, field): + cdef cnp.ndarray[cnp.float64_t, ndim=1] out = np.zeros((1,)) + acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out.data) + return out + + def __get_stat_matrix(self, field, n, m): + cdef cnp.ndarray[cnp.float64_t, ndim=2] out_mat = np.ascontiguousarray(np.zeros((n, m)), dtype=np.float64) + acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_mat.data) + return out_mat + + + def get_cost(self): + """ + Returns the cost value of the current solution. + """ + # compute cost internally + acados_solver_common.ocp_nlp_eval_cost(self.nlp_solver, self.nlp_in, self.nlp_out) + + # create output + cdef double out + + # call getter + acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, "cost_value", &out) + + return out + + + def get_residuals(self, recompute=False): + """ + Returns an array of the form [res_stat, res_eq, res_ineq, res_comp]. + """ + # compute residuals if RTI + if self.nlp_solver_type == 'SQP_RTI' or recompute: + acados_solver_common.ocp_nlp_eval_residuals(self.nlp_solver, self.nlp_in, self.nlp_out) + + # create output array + cdef cnp.ndarray[cnp.float64_t, ndim=1] out = np.ascontiguousarray(np.zeros((4,), dtype=np.float64)) + cdef double double_value + + field = "res_stat".encode('utf-8') + acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, &double_value) + out[0] = double_value + + field = "res_eq".encode('utf-8') + acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, &double_value) + out[1] = double_value + + field = "res_ineq".encode('utf-8') + acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, &double_value) + out[2] = double_value + + field = "res_comp".encode('utf-8') + acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, &double_value) + out[3] = double_value + + return out + + + # Note: this function should not be used anymore, better use cost_set, constraints_set + def set(self, int stage, str field_, value_): + + """ + Set numerical data inside the solver. + + :param stage: integer corresponding to shooting node + :param field: string in ['x', 'u', 'pi', 'lam', 't', 'p'] + + .. note:: regarding lam, t: \n + the inequalities are internally organized in the following order: \n + [ lbu lbx lg lh lphi ubu ubx ug uh uphi; \n + lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi] + + .. note:: pi: multipliers for dynamics equality constraints \n + lam: multipliers for inequalities \n + t: slack variables corresponding to evaluation of all inequalities (at the solution) \n + sl: slack variables of soft lower inequality constraints \n + su: slack variables of soft upper inequality constraints \n + """ + cost_fields = ['y_ref', 'yref'] + constraints_fields = ['lbx', 'ubx', 'lbu', 'ubu'] + out_fields = ['x', 'u', 'pi', 'lam', 't', 'z', 'sl', 'su'] + mem_fields = ['xdot_guess', 'z_guess'] + + field = field_.encode('utf-8') + + cdef cnp.ndarray[cnp.float64_t, ndim=1] value = np.ascontiguousarray(value_, dtype=np.float64) + + # treat parameters separately + if field_ == 'p': + assert acados_solver.acados_update_params(self.capsule, stage, value.data, value.shape[0]) == 0 + else: + if field_ not in constraints_fields + cost_fields + out_fields: + raise Exception("AcadosOcpSolverCython.set(): {} is not a valid argument.\ + \nPossible values are {}. Exiting.".format(field, \ + constraints_fields + cost_fields + out_fields + ['p'])) + + dims = acados_solver_common.ocp_nlp_dims_get_from_attr(self.nlp_config, + self.nlp_dims, self.nlp_out, stage, field) + + if value_.shape[0] != dims: + msg = 'AcadosOcpSolverCython.set(): mismatching dimension for field "{}" '.format(field_) + msg += 'with dimension {} (you have {})'.format(dims, value_.shape[0]) + raise Exception(msg) + + if field_ in constraints_fields: + acados_solver_common.ocp_nlp_constraints_model_set(self.nlp_config, + self.nlp_dims, self.nlp_in, stage, field, value.data) + elif field_ in cost_fields: + acados_solver_common.ocp_nlp_cost_model_set(self.nlp_config, + self.nlp_dims, self.nlp_in, stage, field, value.data) + elif field_ in out_fields: + acados_solver_common.ocp_nlp_out_set(self.nlp_config, + self.nlp_dims, self.nlp_out, stage, field, value.data) + elif field_ in mem_fields: + acados_solver_common.ocp_nlp_set(self.nlp_config, \ + self.nlp_solver, stage, field, value.data) + + + def cost_set(self, int stage, str field_, value_): + """ + Set numerical data in the cost module of the solver. + + :param stage: integer corresponding to shooting node + :param field: string, e.g. 'yref', 'W', 'ext_cost_num_hess' + :param value: of appropriate size + """ + field = field_.encode('utf-8') + + cdef int dims[2] + acados_solver_common.ocp_nlp_cost_dims_get_from_attr(self.nlp_config, \ + self.nlp_dims, self.nlp_out, stage, field, &dims[0]) + + cdef double[::1,:] value + + value_shape = value_.shape + if len(value_shape) == 1: + value_shape = (value_shape[0], 0) + value = np.asfortranarray(value_[None,:]) + + elif len(value_shape) == 2: + # Get elements in column major order + value = np.asfortranarray(value_) + + if value_shape[0] != dims[0] or value_shape[1] != dims[1]: + raise Exception('AcadosOcpSolverCython.cost_set(): mismatching dimension' + + f' for field "{field_}" at stage {stage} with dimension {tuple(dims)} (you have {value_shape})') + + acados_solver_common.ocp_nlp_cost_model_set(self.nlp_config, \ + self.nlp_dims, self.nlp_in, stage, field, &value[0][0]) + + + def constraints_set(self, int stage, str field_, value_): + """ + Set numerical data in the constraint module of the solver. + + :param stage: integer corresponding to shooting node + :param field: string in ['lbx', 'ubx', 'lbu', 'ubu', 'lg', 'ug', 'lh', 'uh', 'uphi', 'C', 'D'] + :param value: of appropriate size + """ + field = field_.encode('utf-8') + + cdef int dims[2] + acados_solver_common.ocp_nlp_constraint_dims_get_from_attr(self.nlp_config, \ + self.nlp_dims, self.nlp_out, stage, field, &dims[0]) + + cdef double[::1,:] value + + value_shape = value_.shape + if len(value_shape) == 1: + value_shape = (value_shape[0], 0) + value = np.asfortranarray(value_[None,:]) + + elif len(value_shape) == 2: + # Get elements in column major order + value = np.asfortranarray(value_) + + if value_shape[0] != dims[0] or value_shape[1] != dims[1]: + raise Exception(f'AcadosOcpSolverCython.constraints_set(): mismatching dimension' + + f' for field "{field_}" at stage {stage} with dimension {tuple(dims)} (you have {value_shape})') + + acados_solver_common.ocp_nlp_constraints_model_set(self.nlp_config, \ + self.nlp_dims, self.nlp_in, stage, field, &value[0][0]) + + return + + + def dynamics_get(self, int stage, str field_): + """ + Get numerical data from the dynamics module of the solver: + + :param stage: integer corresponding to shooting node + :param field: string, e.g. 'A' + """ + field = field_.encode('utf-8') + + # get dims + cdef int[2] dims + acados_solver_common.ocp_nlp_dynamics_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, stage, field, &dims[0]) + + # create output data + cdef cnp.ndarray[cnp.float64_t, ndim=2] out = np.zeros((dims[0], dims[1]), order='F') + + # call getter + acados_solver_common.ocp_nlp_get_at_stage(self.nlp_config, self.nlp_dims, self.nlp_solver, stage, field, out.data) + + return out + + + def options_set(self, str field_, value_): + """ + Set options of the solver. + + :param field: string, e.g. 'print_level', 'rti_phase', 'initialize_t_slacks', 'step_length', 'alpha_min', 'alpha_reduction', 'qp_warm_start', 'line_search_use_sufficient_descent', 'full_step_dual', 'globalization_use_SOC', 'qp_tol_stat', 'qp_tol_eq', 'qp_tol_ineq', 'qp_tol_comp', 'qp_tau_min', 'qp_mu0' + + :param value: of type int, float, string + + - qp_tol_stat: QP solver tolerance stationarity + - qp_tol_eq: QP solver tolerance equalities + - qp_tol_ineq: QP solver tolerance inequalities + - qp_tol_comp: QP solver tolerance complementarity + - qp_tau_min: for HPIPM QP solvers: minimum value of barrier parameter in HPIPM + - qp_mu0: for HPIPM QP solvers: initial value for complementarity slackness + - warm_start_first_qp: indicates if first QP in SQP is warm_started + """ + int_fields = ['print_level', 'rti_phase', 'initialize_t_slacks', 'qp_warm_start', 'line_search_use_sufficient_descent', 'full_step_dual', 'globalization_use_SOC', 'warm_start_first_qp'] + double_fields = ['step_length', 'tol_eq', 'tol_stat', 'tol_ineq', 'tol_comp', 'alpha_min', 'alpha_reduction', 'eps_sufficient_descent', + 'qp_tol_stat', 'qp_tol_eq', 'qp_tol_ineq', 'qp_tol_comp', 'qp_tau_min', 'qp_mu0'] + string_fields = ['globalization'] + + # encode + field = field_.encode('utf-8') + + cdef int int_value + cdef double double_value + cdef unsigned char[::1] string_value + + # check field availability and type + if field_ in int_fields: + if not isinstance(value_, int): + raise Exception('solver option {} must be of type int. You have {}.'.format(field_, type(value_))) + + if field_ == 'rti_phase': + if value_ < 0 or value_ > 2: + raise Exception('AcadosOcpSolverCython.solve(): argument \'rti_phase\' can ' + 'take only values 0, 1, 2 for SQP-RTI-type solvers') + if self.nlp_solver_type != 'SQP_RTI' and value_ > 0: + raise Exception('AcadosOcpSolverCython.solve(): argument \'rti_phase\' can ' + 'take only value 0 for SQP-type solvers') + + int_value = value_ + acados_solver_common.ocp_nlp_solver_opts_set(self.nlp_config, self.nlp_opts, field, &int_value) + + elif field_ in double_fields: + if not isinstance(value_, float): + raise Exception('solver option {} must be of type float. You have {}.'.format(field_, type(value_))) + + double_value = value_ + acados_solver_common.ocp_nlp_solver_opts_set(self.nlp_config, self.nlp_opts, field, &double_value) + + elif field_ in string_fields: + if not isinstance(value_, bytes): + raise Exception('solver option {} must be of type str. You have {}.'.format(field_, type(value_))) + + string_value = value_.encode('utf-8') + acados_solver_common.ocp_nlp_solver_opts_set(self.nlp_config, self.nlp_opts, field, &string_value[0]) + + else: + raise Exception('AcadosOcpSolverCython.options_set() does not support field {}.'\ + '\n Possible values are {}.'.format(field_, ', '.join(int_fields + double_fields + string_fields))) + + + def __del__(self): + if self.solver_created: + acados_solver.acados_free(self.capsule) + acados_solver.acados_free_capsule(self.capsule) diff --git a/third_party/acados/acados_template/acados_sim.py b/third_party/acados/acados_template/acados_sim.py new file mode 100644 index 0000000000..93d5f298db --- /dev/null +++ b/third_party/acados/acados_template/acados_sim.py @@ -0,0 +1,331 @@ +# -*- coding: future_fstrings -*- +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + +import numpy as np +import casadi as ca +import os +from .acados_model import AcadosModel +from .utils import get_acados_path + +class AcadosSimDims: + """ + Class containing the dimensions of the model to be simulated. + """ + def __init__(self): + self.__nx = None + self.__nu = None + self.__nz = 0 + self.__np = 0 + + @property + def nx(self): + """:math:`n_x` - number of states. Type: int > 0""" + return self.__nx + + @property + def nz(self): + """:math:`n_z` - number of algebraic variables. Type: int >= 0""" + return self.__nz + + @property + def nu(self): + """:math:`n_u` - number of inputs. Type: int >= 0""" + return self.__nu + + @property + def np(self): + """:math:`n_p` - number of parameters. Type: int >= 0""" + return self.__np + + @nx.setter + def nx(self, nx): + if isinstance(nx, int) and nx > 0: + self.__nx = nx + else: + raise Exception('Invalid nx value, expected positive integer. Exiting.') + + @nz.setter + def nz(self, nz): + if isinstance(nz, int) and nz > -1: + self.__nz = nz + else: + raise Exception('Invalid nz value, expected nonnegative integer. Exiting.') + + @nu.setter + def nu(self, nu): + if isinstance(nu, int) and nu > -1: + self.__nu = nu + else: + raise Exception('Invalid nu value, expected nonnegative integer. Exiting.') + + @np.setter + def np(self, np): + if isinstance(np, int) and np > -1: + self.__np = np + else: + raise Exception('Invalid np value, expected nonnegative integer. Exiting.') + + def set(self, attr, value): + setattr(self, attr, value) + + +class AcadosSimOpts: + """ + class containing the solver options + """ + def __init__(self): + self.__integrator_type = 'ERK' + self.__collocation_type = 'GAUSS_LEGENDRE' + self.__Tsim = None + # ints + self.__sim_method_num_stages = 1 + self.__sim_method_num_steps = 1 + self.__sim_method_newton_iter = 3 + # bools + self.__sens_forw = True + self.__sens_adj = False + self.__sens_algebraic = False + self.__sens_hess = False + self.__output_z = False + self.__sim_method_jac_reuse = 0 + + @property + def integrator_type(self): + """Integrator type. Default: 'ERK'.""" + return self.__integrator_type + + @property + def num_stages(self): + """Number of stages in the integrator. Default: 1""" + return self.__sim_method_num_stages + + @property + def num_steps(self): + """Number of steps in the integrator. Default: 1""" + return self.__sim_method_num_steps + + @property + def newton_iter(self): + """Number of Newton iterations in simulation method. Default: 3""" + return self.__sim_method_newton_iter + + @property + def sens_forw(self): + """Boolean determining if forward sensitivities are computed. Default: True""" + return self.__sens_forw + + @property + def sens_adj(self): + """Boolean determining if adjoint sensitivities are computed. Default: False""" + return self.__sens_adj + + @property + def sens_algebraic(self): + """Boolean determining if sensitivities wrt algebraic variables are computed. Default: False""" + return self.__sens_algebraic + + @property + def sens_hess(self): + """Boolean determining if hessians are computed. Default: False""" + return self.__sens_hess + + @property + def output_z(self): + """Boolean determining if values for algebraic variables (corresponding to start of simulation interval) are computed. Default: False""" + return self.__output_z + + @property + def sim_method_jac_reuse(self): + """Integer determining if jacobians are reused (0 or 1). Default: 0""" + return self.__sim_method_jac_reuse + + @property + def T(self): + """Time horizon""" + return self.__Tsim + + @property + def collocation_type(self): + """Collocation type: relevant for implicit integrators + -- string in {GAUSS_RADAU_IIA, GAUSS_LEGENDRE} + + Default: GAUSS_LEGENDRE + """ + return self.__collocation_type + + @integrator_type.setter + def integrator_type(self, integrator_type): + integrator_types = ('ERK', 'IRK', 'GNSF') + if integrator_type in integrator_types: + self.__integrator_type = integrator_type + else: + raise Exception('Invalid integrator_type value. Possible values are:\n\n' \ + + ',\n'.join(integrator_types) + '.\n\nYou have: ' + integrator_type + '.\n\nExiting.') + + @collocation_type.setter + def collocation_type(self, collocation_type): + collocation_types = ('GAUSS_RADAU_IIA', 'GAUSS_LEGENDRE') + if collocation_type in collocation_types: + self.__collocation_type = collocation_type + else: + raise Exception('Invalid collocation_type value. Possible values are:\n\n' \ + + ',\n'.join(collocation_types) + '.\n\nYou have: ' + collocation_type + '.\n\nExiting.') + + @T.setter + def T(self, T): + self.__Tsim = T + + @num_stages.setter + def num_stages(self, num_stages): + if isinstance(num_stages, int): + self.__sim_method_num_stages = num_stages + else: + raise Exception('Invalid num_stages value. num_stages must be an integer.') + + @num_steps.setter + def num_steps(self, num_steps): + if isinstance(num_steps, int): + self.__sim_method_num_steps = num_steps + else: + raise Exception('Invalid num_steps value. num_steps must be an integer.') + + @newton_iter.setter + def newton_iter(self, newton_iter): + if isinstance(newton_iter, int): + self.__sim_method_newton_iter = newton_iter + else: + raise Exception('Invalid newton_iter value. newton_iter must be an integer.') + + @sens_forw.setter + def sens_forw(self, sens_forw): + if sens_forw in (True, False): + self.__sens_forw = sens_forw + else: + raise Exception('Invalid sens_forw value. sens_forw must be a Boolean.') + + @sens_adj.setter + def sens_adj(self, sens_adj): + if sens_adj in (True, False): + self.__sens_adj = sens_adj + else: + raise Exception('Invalid sens_adj value. sens_adj must be a Boolean.') + + @sens_hess.setter + def sens_hess(self, sens_hess): + if sens_hess in (True, False): + self.__sens_hess = sens_hess + else: + raise Exception('Invalid sens_hess value. sens_hess must be a Boolean.') + + @sens_algebraic.setter + def sens_algebraic(self, sens_algebraic): + if sens_algebraic in (True, False): + self.__sens_algebraic = sens_algebraic + else: + raise Exception('Invalid sens_algebraic value. sens_algebraic must be a Boolean.') + + @output_z.setter + def output_z(self, output_z): + if output_z in (True, False): + self.__output_z = output_z + else: + raise Exception('Invalid output_z value. output_z must be a Boolean.') + + @sim_method_jac_reuse.setter + def sim_method_jac_reuse(self, sim_method_jac_reuse): + if sim_method_jac_reuse in (0, 1): + self.__sim_method_jac_reuse = sim_method_jac_reuse + else: + raise Exception('Invalid sim_method_jac_reuse value. sim_method_jac_reuse must be 0 or 1.') + +class AcadosSim: + """ + The class has the following properties that can be modified to formulate a specific simulation problem, see below: + + :param acados_path: string with the path to acados. It is used to generate the include and lib paths. + + - :py:attr:`dims` of type :py:class:`acados_template.acados_ocp.AcadosSimDims` - are automatically detected from model + - :py:attr:`model` of type :py:class:`acados_template.acados_model.AcadosModel` + - :py:attr:`solver_options` of type :py:class:`acados_template.acados_sim.AcadosSimOpts` + + - :py:attr:`acados_include_path` (set automatically) + - :py:attr:`acados_lib_path` (set automatically) + - :py:attr:`parameter_values` - used to initialize the parameters (can be changed) + + """ + def __init__(self, acados_path=''): + if acados_path == '': + acados_path = get_acados_path() + self.dims = AcadosSimDims() + """Dimension definitions, automatically detected from :py:attr:`model`. Type :py:class:`acados_template.acados_sim.AcadosSimDims`""" + self.model = AcadosModel() + """Model definitions, type :py:class:`acados_template.acados_model.AcadosModel`""" + self.solver_options = AcadosSimOpts() + """Solver Options, type :py:class:`acados_template.acados_sim.AcadosSimOpts`""" + + self.acados_include_path = os.path.join(acados_path, 'include').replace(os.sep, '/') # the replace part is important on Windows for CMake + """Path to acados include directory (set automatically), type: `string`""" + self.acados_lib_path = os.path.join(acados_path, 'lib').replace(os.sep, '/') # the replace part is important on Windows for CMake + """Path to where acados library is located (set automatically), type: `string`""" + + self.code_export_directory = 'c_generated_code' + """Path to where code will be exported. Default: `c_generated_code`.""" + + self.cython_include_dirs = '' + self.__parameter_values = np.array([]) + + @property + def parameter_values(self): + """:math:`p` - initial values for parameter - can be updated""" + return self.__parameter_values + + @parameter_values.setter + def parameter_values(self, parameter_values): + if isinstance(parameter_values, np.ndarray): + self.__parameter_values = parameter_values + else: + raise Exception('Invalid parameter_values value. ' + + f'Expected numpy array, got {type(parameter_values)}.') + + def set(self, attr, value): + # tokenize string + tokens = attr.split('_', 1) + if len(tokens) > 1: + setter_to_call = getattr(getattr(self, tokens[0]), 'set') + else: + setter_to_call = getattr(self, 'set') + + setter_to_call(tokens[1], value) + + return diff --git a/third_party/acados/acados_template/acados_sim_layout.json b/third_party/acados/acados_template/acados_sim_layout.json new file mode 100644 index 0000000000..25b149613b --- /dev/null +++ b/third_party/acados/acados_template/acados_sim_layout.json @@ -0,0 +1,47 @@ +{ + "acados_include_path": [ + "str" + ], + "model": { + "name" : [ + "str" + ] + }, + "acados_lib_path": [ + "str" + ], + "dims": { + "np": [ + "int" + ], + "nu": [ + "int" + ], + "nx": [ + "int" + ], + "nz": [ + "int" + ] + }, + "solver_options": { + "integrator_type": [ + "str" + ], + "collocation_type": [ + "str" + ], + "Tsim": [ + "float" + ], + "sim_method_num_stages": [ + "int" + ], + "sim_method_num_steps": [ + "int" + ], + "sim_method_newton_iter": [ + "int" + ] + } +} diff --git a/third_party/acados/acados_template/acados_sim_solver.py b/third_party/acados/acados_template/acados_sim_solver.py new file mode 100644 index 0000000000..3588dd38cd --- /dev/null +++ b/third_party/acados/acados_template/acados_sim_solver.py @@ -0,0 +1,454 @@ +# -*- coding: future_fstrings -*- +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + +import sys, os, json + +import numpy as np + +from ctypes import * +from copy import deepcopy + +from .generate_c_code_explicit_ode import generate_c_code_explicit_ode +from .generate_c_code_implicit_ode import generate_c_code_implicit_ode +from .generate_c_code_gnsf import generate_c_code_gnsf +from .acados_sim import AcadosSim +from .acados_ocp import AcadosOcp +from .acados_model import acados_model_strip_casadi_symbolics +from .utils import is_column, render_template, format_class_dict, np_array_to_list,\ + make_model_consistent, set_up_imported_gnsf_model, get_python_interface_path +from .builders import CMakeBuilder + + +def make_sim_dims_consistent(acados_sim): + dims = acados_sim.dims + model = acados_sim.model + # nx + if is_column(model.x): + dims.nx = model.x.shape[0] + else: + raise Exception("model.x should be column vector!") + + # nu + if is_column(model.u): + dims.nu = model.u.shape[0] + elif model.u == None or model.u == []: + dims.nu = 0 + else: + raise Exception("model.u should be column vector or None!") + + # nz + if is_column(model.z): + dims.nz = model.z.shape[0] + elif model.z == None or model.z == []: + dims.nz = 0 + else: + raise Exception("model.z should be column vector or None!") + + # np + if is_column(model.p): + dims.np = model.p.shape[0] + elif model.p == None or model.p == []: + dims.np = 0 + else: + raise Exception("model.p should be column vector or None!") + + +def get_sim_layout(): + python_interface_path = get_python_interface_path() + abs_path = os.path.join(python_interface_path, 'acados_sim_layout.json') + with open(abs_path, 'r') as f: + sim_layout = json.load(f) + return sim_layout + + +def sim_formulation_json_dump(acados_sim, json_file='acados_sim.json'): + # Load acados_sim structure description + sim_layout = get_sim_layout() + + # Copy input sim object dictionary + sim_dict = dict(deepcopy(acados_sim).__dict__) + + for key, v in sim_layout.items(): + # skip non dict attributes + if not isinstance(v, dict): continue + # Copy sim object attributes dictionaries + sim_dict[key]=dict(getattr(acados_sim, key).__dict__) + + sim_dict['model'] = acados_model_strip_casadi_symbolics(sim_dict['model']) + sim_json = format_class_dict(sim_dict) + + with open(json_file, 'w') as f: + json.dump(sim_json, f, default=np_array_to_list, indent=4, sort_keys=True) + + +def sim_get_default_cmake_builder() -> CMakeBuilder: + """ + If :py:class:`~acados_template.acados_sim_solver.AcadosSimSolver` is used with `CMake` this function returns a good first setting. + :return: default :py:class:`~acados_template.builders.CMakeBuilder` + """ + cmake_builder = CMakeBuilder() + cmake_builder.options_on = ['BUILD_ACADOS_SIM_SOLVER_LIB'] + return cmake_builder + + +def sim_render_templates(json_file, model_name, code_export_dir, cmake_options: CMakeBuilder = None): + # setting up loader and environment + json_path = os.path.join(os.getcwd(), json_file) + + if not os.path.exists(json_path): + raise Exception(f"{json_path} not found!") + + template_dir = code_export_dir + + ## Render templates + in_file = 'acados_sim_solver.in.c' + out_file = f'acados_sim_solver_{model_name}.c' + render_template(in_file, out_file, template_dir, json_path) + + in_file = 'acados_sim_solver.in.h' + out_file = f'acados_sim_solver_{model_name}.h' + render_template(in_file, out_file, template_dir, json_path) + + # Builder + if cmake_options is not None: + in_file = 'CMakeLists.in.txt' + out_file = 'CMakeLists.txt' + render_template(in_file, out_file, template_dir, json_path) + else: + in_file = 'Makefile.in' + out_file = 'Makefile' + render_template(in_file, out_file, template_dir, json_path) + + in_file = 'main_sim.in.c' + out_file = f'main_sim_{model_name}.c' + render_template(in_file, out_file, template_dir, json_path) + + ## folder model + template_dir = os.path.join(code_export_dir, model_name + '_model') + + in_file = 'model.in.h' + out_file = f'{model_name}_model.h' + render_template(in_file, out_file, template_dir, json_path) + + +def sim_generate_casadi_functions(acados_sim): + model = acados_sim.model + model = make_model_consistent(model) + + integrator_type = acados_sim.solver_options.integrator_type + + opts = dict(generate_hess = acados_sim.solver_options.sens_hess, + code_export_directory = acados_sim.code_export_directory) + # generate external functions + if integrator_type == 'ERK': + generate_c_code_explicit_ode(model, opts) + elif integrator_type == 'IRK': + generate_c_code_implicit_ode(model, opts) + elif integrator_type == 'GNSF': + generate_c_code_gnsf(model, opts) + + +class AcadosSimSolver: + """ + Class to interact with the acados integrator C object. + + :param acados_sim: type :py:class:`~acados_template.acados_ocp.AcadosOcp` (takes values to generate an instance :py:class:`~acados_template.acados_sim.AcadosSim`) or :py:class:`~acados_template.acados_sim.AcadosSim` + :param json_file: Default: 'acados_sim.json' + :param build: Default: True + :param cmake_builder: type :py:class:`~acados_template.utils.CMakeBuilder` generate a `CMakeLists.txt` and use + the `CMake` pipeline instead of a `Makefile` (`CMake` seems to be the better option in conjunction with + `MS Visual Studio`); default: `None` + """ + def __init__(self, acados_sim_, json_file='acados_sim.json', build=True, cmake_builder: CMakeBuilder = None): + + self.solver_created = False + + if isinstance(acados_sim_, AcadosOcp): + # set up acados_sim_ + acados_sim = AcadosSim() + acados_sim.model = acados_sim_.model + acados_sim.dims.nx = acados_sim_.dims.nx + acados_sim.dims.nu = acados_sim_.dims.nu + acados_sim.dims.nz = acados_sim_.dims.nz + acados_sim.dims.np = acados_sim_.dims.np + acados_sim.solver_options.integrator_type = acados_sim_.solver_options.integrator_type + acados_sim.code_export_directory = acados_sim_.code_export_directory + + elif isinstance(acados_sim_, AcadosSim): + acados_sim = acados_sim_ + + acados_sim.__problem_class = 'SIM' + + model_name = acados_sim.model.name + make_sim_dims_consistent(acados_sim) + + # reuse existing json and casadi functions, when creating integrator from ocp + if isinstance(acados_sim_, AcadosSim): + if acados_sim.solver_options.integrator_type == 'GNSF': + set_up_imported_gnsf_model(acados_sim) + + sim_generate_casadi_functions(acados_sim) + sim_formulation_json_dump(acados_sim, json_file) + + code_export_dir = acados_sim.code_export_directory + if build: + # render templates + sim_render_templates(json_file, model_name, code_export_dir, cmake_builder) + + # Compile solver + cwd = os.getcwd() + code_export_dir = os.path.abspath(code_export_dir) + os.chdir(code_export_dir) + if cmake_builder is not None: + cmake_builder.exec(code_export_dir) + else: + os.system('make sim_shared_lib') + os.chdir(cwd) + + self.sim_struct = acados_sim + model_name = self.sim_struct.model.name + self.model_name = model_name + + # Load acados library to avoid unloading the library. + # This is necessary if acados was compiled with OpenMP, since the OpenMP threads can't be destroyed. + # Unloading a library which uses OpenMP results in a segfault (on any platform?). + # see [https://stackoverflow.com/questions/34439956/vc-crash-when-freeing-a-dll-built-with-openmp] + # or [https://python.hotexamples.com/examples/_ctypes/-/dlclose/python-dlclose-function-examples.html] + libacados_name = 'libacados.so' + libacados_filepath = os.path.join(acados_sim.acados_lib_path, libacados_name) + self.__acados_lib = CDLL(libacados_filepath) + # find out if acados was compiled with OpenMP + try: + self.__acados_lib_uses_omp = getattr(self.__acados_lib, 'omp_get_thread_num') is not None + except AttributeError as e: + self.__acados_lib_uses_omp = False + if self.__acados_lib_uses_omp: + print('acados was compiled with OpenMP.') + else: + print('acados was compiled without OpenMP.') + + # Ctypes + lib_prefix = 'lib' + lib_ext = '.so' + if os.name == 'nt': + lib_prefix = '' + lib_ext = '' + self.shared_lib_name = os.path.join(code_export_dir, f'{lib_prefix}acados_sim_solver_{model_name}{lib_ext}') + print(f'self.shared_lib_name = "{self.shared_lib_name}"') + + self.shared_lib = CDLL(self.shared_lib_name) + + + # create capsule + getattr(self.shared_lib, f"{model_name}_acados_sim_solver_create_capsule").restype = c_void_p + self.capsule = getattr(self.shared_lib, f"{model_name}_acados_sim_solver_create_capsule")() + + # create solver + getattr(self.shared_lib, f"{model_name}_acados_sim_create").argtypes = [c_void_p] + getattr(self.shared_lib, f"{model_name}_acados_sim_create").restype = c_int + assert getattr(self.shared_lib, f"{model_name}_acados_sim_create")(self.capsule)==0 + self.solver_created = True + + getattr(self.shared_lib, f"{model_name}_acados_get_sim_opts").argtypes = [c_void_p] + getattr(self.shared_lib, f"{model_name}_acados_get_sim_opts").restype = c_void_p + self.sim_opts = getattr(self.shared_lib, f"{model_name}_acados_get_sim_opts")(self.capsule) + + getattr(self.shared_lib, f"{model_name}_acados_get_sim_dims").argtypes = [c_void_p] + getattr(self.shared_lib, f"{model_name}_acados_get_sim_dims").restype = c_void_p + self.sim_dims = getattr(self.shared_lib, f"{model_name}_acados_get_sim_dims")(self.capsule) + + getattr(self.shared_lib, f"{model_name}_acados_get_sim_config").argtypes = [c_void_p] + getattr(self.shared_lib, f"{model_name}_acados_get_sim_config").restype = c_void_p + self.sim_config = getattr(self.shared_lib, f"{model_name}_acados_get_sim_config")(self.capsule) + + getattr(self.shared_lib, f"{model_name}_acados_get_sim_out").argtypes = [c_void_p] + getattr(self.shared_lib, f"{model_name}_acados_get_sim_out").restype = c_void_p + self.sim_out = getattr(self.shared_lib, f"{model_name}_acados_get_sim_out")(self.capsule) + + getattr(self.shared_lib, f"{model_name}_acados_get_sim_in").argtypes = [c_void_p] + getattr(self.shared_lib, f"{model_name}_acados_get_sim_in").restype = c_void_p + self.sim_in = getattr(self.shared_lib, f"{model_name}_acados_get_sim_in")(self.capsule) + + getattr(self.shared_lib, f"{model_name}_acados_get_sim_solver").argtypes = [c_void_p] + getattr(self.shared_lib, f"{model_name}_acados_get_sim_solver").restype = c_void_p + self.sim_solver = getattr(self.shared_lib, f"{model_name}_acados_get_sim_solver")(self.capsule) + + nu = self.sim_struct.dims.nu + nx = self.sim_struct.dims.nx + nz = self.sim_struct.dims.nz + self.gettable = { + 'x': nx, + 'xn': nx, + 'u': nu, + 'z': nz, + 'S_forw': nx*(nx+nu), + 'Sx': nx*nx, + 'Su': nx*nu, + 'S_adj': nx+nu, + 'S_hess': (nx+nu)*(nx+nu), + 'S_algebraic': (nz)*(nx+nu), + } + + self.settable = ['S_adj', 'T', 'x', 'u', 'xdot', 'z', 'p'] # S_forw + + + def solve(self): + """ + Solve the simulation problem with current input. + """ + getattr(self.shared_lib, f"{self.model_name}_acados_sim_solve").argtypes = [c_void_p] + getattr(self.shared_lib, f"{self.model_name}_acados_sim_solve").restype = c_int + + status = getattr(self.shared_lib, f"{self.model_name}_acados_sim_solve")(self.capsule) + return status + + + def get(self, field_): + """ + Get the last solution of the solver. + + :param str field: string in ['x', 'u', 'z', 'S_forw', 'Sx', 'Su', 'S_adj', 'S_hess', 'S_algebraic'] + """ + field = field_ + field = field.encode('utf-8') + + if field_ in self.gettable.keys(): + + # allocate array + dims = self.gettable[field_] + out = np.ascontiguousarray(np.zeros((dims,)), dtype=np.float64) + out_data = cast(out.ctypes.data, POINTER(c_double)) + + self.shared_lib.sim_out_get.argtypes = [c_void_p, c_void_p, c_void_p, c_char_p, c_void_p] + self.shared_lib.sim_out_get(self.sim_config, self.sim_dims, self.sim_out, field, out_data) + + if field_ == 'S_forw': + nu = self.sim_struct.dims.nu + nx = self.sim_struct.dims.nx + out = out.reshape(nx, nx+nu, order='F') + elif field_ == 'Sx': + nx = self.sim_struct.dims.nx + out = out.reshape(nx, nx, order='F') + elif field_ == 'Su': + nx = self.sim_struct.dims.nx + nu = self.sim_struct.dims.nu + out = out.reshape(nx, nu, order='F') + elif field_ == 'S_hess': + nx = self.sim_struct.dims.nx + nu = self.sim_struct.dims.nu + out = out.reshape(nx+nu, nx+nu, order='F') + elif field_ == 'S_algebraic': + nx = self.sim_struct.dims.nx + nu = self.sim_struct.dims.nu + nz = self.sim_struct.dims.nz + out = out.reshape(nz, nx+nu, order='F') + else: + raise Exception(f'AcadosSimSolver.get(): Unknown field {field_},' \ + f' available fields are {", ".join(self.gettable.keys())}') + + return out + + + def set(self, field_, value_): + """ + Set numerical data inside the solver. + + :param field: string in ['p', 'S_adj', 'T', 'x', 'u', 'xdot', 'z'] + :param value: the value with appropriate size. + """ + # cast value_ to avoid conversion issues + if isinstance(value_, (float, int)): + value_ = np.array([value_]) + + value_ = value_.astype(float) + value_data = cast(value_.ctypes.data, POINTER(c_double)) + value_data_p = cast((value_data), c_void_p) + + field = field_ + field = field.encode('utf-8') + + # treat parameters separately + if field_ == 'p': + model_name = self.sim_struct.model.name + getattr(self.shared_lib, f"{model_name}_acados_sim_update_params").argtypes = [c_void_p, POINTER(c_double), c_int] + value_data = cast(value_.ctypes.data, POINTER(c_double)) + getattr(self.shared_lib, f"{model_name}_acados_sim_update_params")(self.capsule, value_data, value_.shape[0]) + return + else: + # dimension check + dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc) + dims_data = cast(dims.ctypes.data, POINTER(c_int)) + + self.shared_lib.sim_dims_get_from_attr.argtypes = [c_void_p, c_void_p, c_char_p, POINTER(c_int)] + self.shared_lib.sim_dims_get_from_attr(self.sim_config, self.sim_dims, field, dims_data) + + value_ = np.ravel(value_, order='F') + + value_shape = value_.shape + if len(value_shape) == 1: + value_shape = (value_shape[0], 0) + + if value_shape != tuple(dims): + raise Exception('AcadosSimSolver.set(): mismatching dimension' \ + ' for field "{}" with dimension {} (you have {})'.format(field_, tuple(dims), value_shape)) + + # set + if field_ in ['xdot', 'z']: + self.shared_lib.sim_solver_set.argtypes = [c_void_p, c_char_p, c_void_p] + self.shared_lib.sim_solver_set(self.sim_solver, field, value_data_p) + elif field_ in self.settable: + self.shared_lib.sim_in_set.argtypes = [c_void_p, c_void_p, c_void_p, c_char_p, c_void_p] + self.shared_lib.sim_in_set(self.sim_config, self.sim_dims, self.sim_in, field, value_data_p) + else: + raise Exception(f'AcadosSimSolver.set(): Unknown field {field_},' \ + f' available fields are {", ".join(self.settable)}') + + return + + + def __del__(self): + + if self.solver_created: + getattr(self.shared_lib, f"{self.model_name}_acados_sim_free").argtypes = [c_void_p] + getattr(self.shared_lib, f"{self.model_name}_acados_sim_free").restype = c_int + getattr(self.shared_lib, f"{self.model_name}_acados_sim_free")(self.capsule) + + getattr(self.shared_lib, f"{self.model_name}_acados_sim_solver_free_capsule").argtypes = [c_void_p] + getattr(self.shared_lib, f"{self.model_name}_acados_sim_solver_free_capsule").restype = c_int + getattr(self.shared_lib, f"{self.model_name}_acados_sim_solver_free_capsule")(self.capsule) + + try: + self.dlclose(self.shared_lib._handle) + except: + pass diff --git a/third_party/acados/acados_template/acados_solver_common.pxd b/third_party/acados/acados_template/acados_solver_common.pxd new file mode 100644 index 0000000000..fedd7190d9 --- /dev/null +++ b/third_party/acados/acados_template/acados_solver_common.pxd @@ -0,0 +1,103 @@ +# -*- coding: future_fstrings -*- +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + + +cdef extern from "acados/ocp_nlp/ocp_nlp_common.h": + ctypedef struct ocp_nlp_config: + pass + + ctypedef struct ocp_nlp_dims: + pass + + ctypedef struct ocp_nlp_in: + pass + + ctypedef struct ocp_nlp_out: + pass + + +cdef extern from "acados_c/ocp_nlp_interface.h": + ctypedef enum ocp_nlp_solver_t: + pass + + ctypedef enum ocp_nlp_cost_t: + pass + + ctypedef enum ocp_nlp_dynamics_t: + pass + + ctypedef enum ocp_nlp_constraints_t: + pass + + ctypedef enum ocp_nlp_reg_t: + pass + + ctypedef struct ocp_nlp_plan: + pass + + ctypedef struct ocp_nlp_solver: + pass + + int ocp_nlp_cost_model_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in_, + int start_stage, const char *field, void *value) + int ocp_nlp_constraints_model_set(ocp_nlp_config *config, ocp_nlp_dims *dims, + ocp_nlp_in *in_, int stage, const char *field, void *value) + + # out + void ocp_nlp_out_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, + int stage, const char *field, void *value) + void ocp_nlp_out_get(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, + int stage, const char *field, void *value) + void ocp_nlp_get_at_stage(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_solver *solver, + int stage, const char *field, void *value) + int ocp_nlp_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, + int stage, const char *field) + void ocp_nlp_constraint_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, + int stage, const char *field, int *dims_out) + void ocp_nlp_cost_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, + int stage, const char *field, int *dims_out) + void ocp_nlp_dynamics_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, + int stage, const char *field, int *dims_out) + + # opts + void ocp_nlp_solver_opts_set(ocp_nlp_config *config, void *opts_, const char *field, void* value) + + # solver + 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) + void ocp_nlp_eval_cost(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in_, ocp_nlp_out *nlp_out) + + # get/set + void ocp_nlp_get(ocp_nlp_config *config, ocp_nlp_solver *solver, const char *field, void *return_value_) + void ocp_nlp_set(ocp_nlp_config *config, ocp_nlp_solver *solver, int stage, const char *field, void *value) diff --git a/third_party/acados/acados_template/builders.py b/third_party/acados/acados_template/builders.py new file mode 100644 index 0000000000..f595033ceb --- /dev/null +++ b/third_party/acados/acados_template/builders.py @@ -0,0 +1,116 @@ +# -*- coding: future_fstrings -*- +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + +import os +import sys +from subprocess import call + + +class CMakeBuilder: + """ + Class to work with the `CMake` build system. + """ + def __init__(self): + self._source_dir = None # private source directory, this is set to code_export_dir + self.build_dir = 'build' + self._build_dir = None # private build directory, usually rendered to abspath(build_dir) + self.generator = None + """Defines the generator, options can be found via `cmake --help` under 'Generator'. Type: string. Linux default 'Unix Makefiles', Windows 'Visual Studio 15 2017 Win64'; default value: `None`.""" + # set something for Windows + if os.name == 'nt': + self.generator = 'Visual Studio 15 2017 Win64' + self.build_targets = None + """A comma-separated list of the build targets, if `None` then all targets will be build; type: List of strings; default: `None`.""" + self.options_on = None + """List of strings as CMake options which are translated to '-D Opt[0]=ON -D Opt[1]=ON ...'; default: `None`.""" + + # Generate the command string for handling the cmake command. + def get_cmd1_cmake(self): + defines_str = '' + if self.options_on is not None: + defines_arr = [f' -D{opt}=ON' for opt in self.options_on] + defines_str = ' '.join(defines_arr) + generator_str = '' + if self.generator is not None: + generator_str = f' -G"{self.generator}"' + return f'cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX="{self._source_dir}"{defines_str}{generator_str} -Wdev -S"{self._source_dir}" -B"{self._build_dir}"' + + # Generate the command string for handling the build. + def get_cmd2_build(self): + import multiprocessing + cmd = f'cmake --build "{self._build_dir}" --config Release -j{multiprocessing.cpu_count()}' + if self.build_targets is not None: + cmd += f' -t {self.build_targets}' + return cmd + + # Generate the command string for handling the install command. + def get_cmd3_install(self): + return f'cmake --install "{self._build_dir}"' + + def exec(self, code_export_directory): + """ + Execute the compilation using `CMake` with the given settings. + :param code_export_directory: must be the absolute path to the directory where the code was exported to + """ + if(os.path.isabs(code_export_directory) is False): + print(f'(W) the code export directory "{code_export_directory}" is not an absolute path!') + self._source_dir = code_export_directory + self._build_dir = os.path.abspath(self.build_dir) + try: + os.mkdir(self._build_dir) + except FileExistsError as e: + pass + + try: + os.chdir(self._build_dir) + cmd_str = self.get_cmd1_cmake() + print(f'call("{cmd_str})"') + retcode = call(cmd_str, shell=True) + if retcode != 0: + raise RuntimeError(f'CMake command "{cmd_str}" was terminated by signal {retcode}') + cmd_str = self.get_cmd2_build() + print(f'call("{cmd_str}")') + retcode = call(cmd_str, shell=True) + if retcode != 0: + raise RuntimeError(f'Build command "{cmd_str}" was terminated by signal {retcode}') + cmd_str = self.get_cmd3_install() + print(f'call("{cmd_str}")') + retcode = call(cmd_str, shell=True) + if retcode != 0: + raise RuntimeError(f'Install command "{cmd_str}" was terminated by signal {retcode}') + except OSError as e: + print("Execution failed:", e, file=sys.stderr) + except Exception as e: + print("Execution failed:", e, file=sys.stderr) + exit(1) diff --git a/third_party/acados/acados_template/c_templates_tera/CMakeLists.in.txt b/third_party/acados/acados_template/c_templates_tera/CMakeLists.in.txt new file mode 100644 index 0000000000..3d6483b5d2 --- /dev/null +++ b/third_party/acados/acados_template/c_templates_tera/CMakeLists.in.txt @@ -0,0 +1,374 @@ +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + +{%- if solver_options.qp_solver %} + {%- set qp_solver = solver_options.qp_solver %} +{%- else %} + {%- set qp_solver = "FULL_CONDENSING_HPIPM" %} +{%- endif %} + +{%- if solver_options.hessian_approx %} + {%- set hessian_approx = solver_options.hessian_approx %} +{%- elif solver_options.sens_hess %} + {%- set hessian_approx = "EXACT" %} +{%- else %} + {%- set hessian_approx = "GAUSS_NEWTON" %} +{%- endif %} + +{%- if constraints.constr_type %} + {%- set constr_type = constraints.constr_type %} +{%- else %} + {%- set constr_type = "NONE" %} +{%- endif %} + +{%- if constraints.constr_type_e %} + {%- set constr_type_e = constraints.constr_type_e %} +{%- else %} + {%- set constr_type_e = "NONE" %} +{%- endif %} + +{%- if cost.cost_type %} + {%- set cost_type = cost.cost_type %} +{%- else %} + {%- set cost_type = "NONE" %} +{%- endif %} + +{%- if cost.cost_type_e %} + {%- set cost_type_e = cost.cost_type_e %} +{%- else %} + {%- set cost_type_e = "NONE" %} +{%- endif %} + +{%- if cost.cost_type_0 %} + {%- set cost_type_0 = cost.cost_type_0 %} +{%- else %} + {%- set cost_type_0 = "NONE" %} +{%- endif %} + +{%- if dims.nh %} + {%- set dims_nh = dims.nh %} +{%- else %} + {%- set dims_nh = 0 %} +{%- endif %} + +{%- if dims.nphi %} + {%- set dims_nphi = dims.nphi %} +{%- else %} + {%- set dims_nphi = 0 %} +{%- endif %} + +{%- if dims.nh_e %} + {%- set dims_nh_e = dims.nh_e %} +{%- else %} + {%- set dims_nh_e = 0 %} +{%- endif %} + +{%- if dims.nphi_e %} + {%- set dims_nphi_e = dims.nphi_e %} +{%- else %} + {%- set dims_nphi_e = 0 %} +{%- endif %} + +{%- if solver_options.model_external_shared_lib_dir %} + {%- set model_external_shared_lib_dir = solver_options.model_external_shared_lib_dir %} +{%- endif %} + +{%- if solver_options.model_external_shared_lib_name %} + {%- set model_external_shared_lib_name = solver_options.model_external_shared_lib_name %} +{%- endif %} + +{#- control operator #} +{%- if os and os == "pc" %} + {%- set control = "&" %} +{%- else %} + {%- set control = ";" %} +{%- endif %} + +{%- if acados_link_libs and os and os == "pc" %}{# acados linking libraries and flags #} + {%- set link_libs = acados_link_libs.qpoases ~ " " ~ acados_link_libs.hpmpc ~ " " ~ acados_link_libs.osqp -%} + {%- set openmp_flag = acados_link_libs.openmp %} +{%- else %} + {%- set openmp_flag = " " %} + {%- if qp_solver == "FULL_CONDENSING_QPOASES" %} + {%- set link_libs = "-lqpOASES_e" %} + {%- else %} + {%- set link_libs = "" %} + {%- endif %} +{%- endif %} + +cmake_minimum_required(VERSION 3.10) + +project({{ model.name }}) + +# build options. +option(BUILD_ACADOS_SOLVER_LIB "Should the solver library acados_solver_{{ model.name }} be build?" OFF) +option(BUILD_ACADOS_OCP_SOLVER_LIB "Should the OCP solver library acados_ocp_solver_{{ model.name }} be build?" OFF) +option(BUILD_EXAMPLE "Should the example main_{{ model.name }} be build?" OFF) +{%- if solver_options.integrator_type != "DISCRETE" %} +option(BUILD_SIM_EXAMPLE "Should the simulation example main_sim_{{ model.name }} be build?" OFF) +option(BUILD_ACADOS_SIM_SOLVER_LIB "Should the simulation solver library acados_sim_solver_{{ model.name }} be build?" OFF) +{%- endif %} + +# object target names +set(MODEL_OBJ model_{{ model.name }}) +set(OCP_OBJ ocp_{{ model.name }}) +set(SIM_OBJ sim_{{ model.name }}) + +# model +set(MODEL_SRC +{%- if solver_options.integrator_type == "ERK" %} + {{ model.name }}_model/{{ model.name }}_expl_ode_fun.c + {{ model.name }}_model/{{ model.name }}_expl_vde_forw.c + {%- if hessian_approx == "EXACT" %} + {{ model.name }}_model/{{ model.name }}_expl_ode_hess.c + {%- endif %} +{%- elif solver_options.integrator_type == "IRK" %} + {{ model.name }}_model/{{ model.name }}_impl_dae_fun.c + {{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_z.c + {{ model.name }}_model/{{ model.name }}_impl_dae_jac_x_xdot_u_z.c + {%- if hessian_approx == "EXACT" %} + {{ model.name }}_model/{{ model.name }}_impl_dae_hess.c + {%- endif %} +{%- elif solver_options.integrator_type == "LIFTED_IRK" %} + {{ model.name }}_model/{{ model.name }}_impl_dae_fun.c + {{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_u.c + {%- if hessian_approx == "EXACT" %} + {{ model.name }}_model/{{ model.name }}_impl_dae_hess.c + {%- endif %} +{%- elif solver_options.integrator_type == "GNSF" %} + {% if model.gnsf.purely_linear != 1 %} + {{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.c + {{ model.name }}_model/{{ model.name }}_gnsf_phi_fun_jac_y.c + {{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.c + {% if model.gnsf.nontrivial_f_LO == 1 %} + {{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c + {%- endif %} + {%- endif %} + {{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.c +{%- elif solver_options.integrator_type == "DISCRETE" %} + {%- if model.dyn_ext_fun_type == "casadi" %} + {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun.c + {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac.c + {%- if hessian_approx == "EXACT" %} + {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac_hess.c + {%- endif %} + {%- else %} + {{ model.name }}_model/{{ model.dyn_source_discrete }} + {%- endif %} +{%- endif -%} +) +add_library(${MODEL_OBJ} OBJECT ${MODEL_SRC} ) + +# optimal control problem - mostly CasADi exports +if(${BUILD_ACADOS_SOLVER_LIB} OR ${BUILD_ACADOS_OCP_SOLVER_LIB} OR ${BUILD_EXAMPLE}) + set(OCP_SRC +{%- if constr_type == "BGP" and dims_nphi > 0 %} + {{ model.name }}_constraints/{{ model.name }}_phi_constraint.c +{%- endif %} +{%- if constr_type_e == "BGP" and dims_nphi_e > 0 %} + {{ model.name }}_constraints/{{ model.name }}_phi_e_constraint.c +{%- endif %} + +{%- if constr_type == "BGH" and dims_nh > 0 %} + {{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt.c + {{ model.name }}_constraints/{{ model.name }}_constr_h_fun.c + {%- if hessian_approx == "EXACT" %} + {{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt_hess.c + {%- endif %} +{%- endif %} + +{%- if constr_type_e == "BGH" and dims_nh_e > 0 %} + {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt.c + {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun.c + {%- if hessian_approx == "EXACT" %} + {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess.c + {%- endif %} +{%- endif %} + +{%- if cost_type_0 == "NONLINEAR_LS" %} + {{ model.name }}_cost/{{ model.name }}_cost_y_0_fun.c + {{ model.name }}_cost/{{ model.name }}_cost_y_0_fun_jac_ut_xt.c + {{ model.name }}_cost/{{ model.name }}_cost_y_0_hess.c +{%- elif cost_type_0 == "EXTERNAL" %} + {%- if cost.cost_ext_fun_type_0 == "casadi" %} + {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun.c + {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac.c + {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac_hess.c + {%- else %} + {{ model.name }}_cost/{{ cost.cost_source_ext_cost_0 }} + {%- endif %} +{%- endif %} +{%- if cost_type == "NONLINEAR_LS" %} + {{ model.name }}_cost/{{ model.name }}_cost_y_fun.c + {{ model.name }}_cost/{{ model.name }}_cost_y_fun_jac_ut_xt.c + {{ model.name }}_cost/{{ model.name }}_cost_y_hess.c +{%- elif cost_type == "EXTERNAL" %} + {%- if cost.cost_ext_fun_type == "casadi" %} + {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun.c + {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac.c + {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac_hess.c + {%- elif cost.cost_source_ext_cost != cost.cost_source_ext_cost_0 %} + {{ model.name }}_cost/{{ cost.cost_source_ext_cost }} + {%- endif %} +{%- endif %} +{%- if cost_type_e == "NONLINEAR_LS" %} + {{ model.name }}_cost/{{ model.name }}_cost_y_e_fun.c + {{ model.name }}_cost/{{ model.name }}_cost_y_e_fun_jac_ut_xt.c + {{ model.name }}_cost/{{ model.name }}_cost_y_e_hess.c +{%- elif cost_type_e == "EXTERNAL" %} + {%- if cost.cost_ext_fun_type_e == "casadi" %} + {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun.c + {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac.c + {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac_hess.c + {%- elif cost.cost_source_ext_cost_e != cost.cost_source_ext_cost_0 %} + {{ model.name }}_cost/{{ cost.cost_source_ext_cost_e }} + {%- endif %} +{%- endif %} + acados_solver_{{ model.name }}.c) + add_library(${OCP_OBJ} OBJECT ${OCP_SRC}) +endif() + +{%- if solver_options.integrator_type != "DISCRETE" %} +# for sim solver +if(${BUILD_ACADOS_SOLVER_LIB} OR ${BUILD_EXAMPLE} + {%- if solver_options.integrator_type != "DISCRETE" %} + OR ${BUILD_SIM_EXAMPLE} OR ${BUILD_ACADOS_SIM_SOLVER_LIB} + {%- endif -%} + ) + set(SIM_SRC acados_sim_solver_{{ model.name }}.c) + add_library(${SIM_OBJ} OBJECT ${SIM_SRC}) +endif() +{%- endif %} + +# for target example +set(EX_SRC main_{{ model.name }}.c) +set(EX_EXE main_{{ model.name }}) + +{%- if model_external_shared_lib_dir and model_external_shared_lib_name %} +set(EXTERNAL_DIR {{ model_external_shared_lib_dir }}) +set(EXTERNAL_LIB {{ model_external_shared_lib_name }}) +{%- else %} +set(EXTERNAL_DIR) +set(EXTERNAL_LIB) +{%- endif %} + +# set some search paths for preprocessor and linker +set(ACADOS_INCLUDE_PATH {{ acados_include_path }} CACHE PATH "Define the path which contains the include directory for acados.") +set(ACADOS_LIB_PATH {{ acados_lib_path }} CACHE PATH "Define the path which contains the lib directory for acados.") + +# c-compiler flags for debugging +set(CMAKE_C_FLAGS_DEBUG "-O0 -ggdb") + +set(CMAKE_C_FLAGS " +{%- if qp_solver == "FULL_CONDENSING_QPOASES" -%} + -DACADOS_WITH_QPOASES +{%- endif -%} +{%- if qp_solver == "PARTIAL_CONDENSING_OSQP" -%} + -DACADOS_WITH_OSQP +{%- endif -%} +{%- if qp_solver == "PARTIAL_CONDENSING_QPDUNES" -%} + -DACADOS_WITH_QPDUNES +{%- endif -%} + -fPIC -std=c99 {{ openmp_flag }}") +#-fno-diagnostics-show-line-numbers -g + +include_directories( + ${ACADOS_INCLUDE_PATH} + ${ACADOS_INCLUDE_PATH}/acados + ${ACADOS_INCLUDE_PATH}/blasfeo/include + ${ACADOS_INCLUDE_PATH}/hpipm/include +{%- if qp_solver == "FULL_CONDENSING_QPOASES" %} + ${ACADOS_INCLUDE_PATH}/qpOASES_e/ +{%- endif %} +) + +# linker flags +link_directories(${ACADOS_LIB_PATH}) + +# link to libraries +if(UNIX) + link_libraries(acados hpipm blasfeo m {{ link_libs }}) +else() + link_libraries(acados hpipm blasfeo {{ link_libs }}) +endif() + +# the targets + +# bundled_shared_lib +if(${BUILD_ACADOS_SOLVER_LIB}) + set(LIB_ACADOS_SOLVER acados_solver_{{ model.name }}) + add_library(${LIB_ACADOS_SOLVER} SHARED $ $ + {%- if solver_options.integrator_type != "DISCRETE" %} + $ + {%- endif -%} + ) + install(TARGETS ${LIB_ACADOS_SOLVER} DESTINATION ${CMAKE_INSTALL_PREFIX}) +endif(${BUILD_ACADOS_SOLVER_LIB}) + +# ocp_shared_lib +if(${BUILD_ACADOS_OCP_SOLVER_LIB}) + set(LIB_ACADOS_OCP_SOLVER acados_ocp_solver_{{ model.name }}) + add_library(${LIB_ACADOS_OCP_SOLVER} SHARED $ $) + # Specify libraries or flags to use when linking a given target and/or its dependents. + target_link_libraries(${LIB_ACADOS_OCP_SOLVER} PRIVATE ${EXTERNAL_LIB}) + target_link_directories(${LIB_ACADOS_OCP_SOLVER} PRIVATE ${EXTERNAL_DIR}) + install(TARGETS ${LIB_ACADOS_OCP_SOLVER} DESTINATION ${CMAKE_INSTALL_PREFIX}) +endif(${BUILD_ACADOS_OCP_SOLVER_LIB}) + +# example +if(${BUILD_EXAMPLE}) + add_executable(${EX_EXE} ${EX_SRC} $ $ + {%- if solver_options.integrator_type != "DISCRETE" %} + $ + {%- endif -%} + ) + install(TARGETS ${EX_EXE} DESTINATION ${CMAKE_INSTALL_PREFIX}) +endif(${BUILD_EXAMPLE}) + +{% if solver_options.integrator_type != "DISCRETE" -%} +# example_sim +if(${BUILD_SIM_EXAMPLE}) + set(EX_SIM_SRC main_sim_{{ model.name }}.c) + set(EX_SIM_EXE main_sim_{{ model.name }}) + add_executable(${EX_SIM_EXE} ${EX_SIM_SRC} $ $) + install(TARGETS ${EX_SIM_EXE} DESTINATION ${CMAKE_INSTALL_PREFIX}) +endif(${BUILD_SIM_EXAMPLE}) + +# sim_shared_lib +if(${BUILD_ACADOS_SIM_SOLVER_LIB}) + set(LIB_ACADOS_SIM_SOLVER acados_sim_solver_{{ model.name }}) + add_library(${LIB_ACADOS_SIM_SOLVER} SHARED $ $) + install(TARGETS ${LIB_ACADOS_SIM_SOLVER} DESTINATION ${CMAKE_INSTALL_PREFIX}) +endif(${BUILD_ACADOS_SIM_SOLVER_LIB}) +{%- endif %} + diff --git a/third_party/acados/acados_template/c_templates_tera/CPPLINT.cfg b/third_party/acados/acados_template/c_templates_tera/CPPLINT.cfg new file mode 100644 index 0000000000..bbd1caf057 --- /dev/null +++ b/third_party/acados/acados_template/c_templates_tera/CPPLINT.cfg @@ -0,0 +1 @@ +exclude_files=[main, acados_solver, acados_solver_sfun, Makefile, model].*\.? diff --git a/third_party/acados/acados_template/c_templates_tera/Makefile.in b/third_party/acados/acados_template/c_templates_tera/Makefile.in new file mode 100644 index 0000000000..d45be0a9c7 --- /dev/null +++ b/third_party/acados/acados_template/c_templates_tera/Makefile.in @@ -0,0 +1,406 @@ +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + +{%- if solver_options.qp_solver %} + {%- set qp_solver = solver_options.qp_solver %} +{%- else %} + {%- set qp_solver = "FULL_CONDENSING_HPIPM" %} +{%- endif %} + +{%- if solver_options.hessian_approx %} + {%- set hessian_approx = solver_options.hessian_approx %} +{%- elif solver_options.sens_hess %} + {%- set hessian_approx = "EXACT" %} +{%- else %} + {%- set hessian_approx = "GAUSS_NEWTON" %} +{%- endif %} + +{%- if constraints.constr_type %} + {%- set constr_type = constraints.constr_type %} +{%- else %} + {%- set constr_type = "NONE" %} +{%- endif %} + +{%- if constraints.constr_type_e %} + {%- set constr_type_e = constraints.constr_type_e %} +{%- else %} + {%- set constr_type_e = "NONE" %} +{%- endif %} + +{%- if cost.cost_type %} + {%- set cost_type = cost.cost_type %} +{%- else %} + {%- set cost_type = "NONE" %} +{%- endif %} + +{%- if cost.cost_type_e %} + {%- set cost_type_e = cost.cost_type_e %} +{%- else %} + {%- set cost_type_e = "NONE" %} +{%- endif %} + +{%- if cost.cost_type_0 %} + {%- set cost_type_0 = cost.cost_type_0 %} +{%- else %} + {%- set cost_type_0 = "NONE" %} +{%- endif %} + +{%- if dims.nh %} + {%- set dims_nh = dims.nh %} +{%- else %} + {%- set dims_nh = 0 %} +{%- endif %} + +{%- if dims.nphi %} + {%- set dims_nphi = dims.nphi %} +{%- else %} + {%- set dims_nphi = 0 %} +{%- endif %} + +{%- if dims.nh_e %} + {%- set dims_nh_e = dims.nh_e %} +{%- else %} + {%- set dims_nh_e = 0 %} +{%- endif %} + +{%- if dims.nphi_e %} + {%- set dims_nphi_e = dims.nphi_e %} +{%- else %} + {%- set dims_nphi_e = 0 %} +{%- endif %} +{%- if solver_options.model_external_shared_lib_dir %} + {%- set model_external_shared_lib_dir = solver_options.model_external_shared_lib_dir %} +{%- endif %} +{%- if solver_options.model_external_shared_lib_name %} + {%- set model_external_shared_lib_name = solver_options.model_external_shared_lib_name %} +{%- endif %} + +{# control operator #} +{%- if os and os == "pc" %} + {%- set control = "&" %} +{%- else %} + {%- set control = ";" %} +{%- endif %} + +{# acados linking libraries and flags #} +{%- if acados_link_libs and os and os == "pc" %} + {%- set link_libs = acados_link_libs.qpoases ~ " " ~ acados_link_libs.hpmpc ~ " " ~ acados_link_libs.osqp -%} + {%- set openmp_flag = acados_link_libs.openmp %} +{%- else %} + {%- set openmp_flag = " " %} + {%- if qp_solver == "FULL_CONDENSING_QPOASES" %} + {%- set link_libs = "-lqpOASES_e" %} + {%- else %} + {%- set link_libs = "" %} + {%- endif %} +{%- endif %} + +# define sources and use make's implicit rules to generate object files (*.o) + +# model +MODEL_SRC= +{%- if solver_options.integrator_type == "ERK" %} +MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_expl_ode_fun.c +MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_expl_vde_forw.c + {%- if hessian_approx == "EXACT" %} +MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_expl_ode_hess.c + {%- endif %} +{%- elif solver_options.integrator_type == "IRK" %} +MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun.c +MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_z.c +MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_jac_x_xdot_u_z.c + {%- if hessian_approx == "EXACT" %} +MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_hess.c + {%- endif %} +{%- elif solver_options.integrator_type == "LIFTED_IRK" %} +MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun.c +MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_u.c + {%- if hessian_approx == "EXACT" %} +MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_hess.c + {%- endif %} +{%- elif solver_options.integrator_type == "GNSF" %} + {% if model.gnsf.purely_linear != 1 %} +MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.c +MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_fun_jac_y.c +MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.c + {% if model.gnsf.nontrivial_f_LO == 1 %} +MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c + {%- endif %} + {%- endif %} +MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.c +{%- elif solver_options.integrator_type == "DISCRETE" %} + {%- if model.dyn_ext_fun_type == "casadi" %} +MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun.c +MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac.c + {%- if hessian_approx == "EXACT" %} +MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac_hess.c + {%- endif %} + {%- else %} +MODEL_SRC+= {{ model.name }}_model/{{ model.dyn_source_discrete }} + {%- endif %} +{%- endif %} +MODEL_OBJ := $(MODEL_SRC:.c=.o) + +# optimal control problem - mostly CasADi exports +OCP_SRC= +{%- if constr_type == "BGP" and dims_nphi > 0 %} +OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_phi_constraint.c +{%- endif %} +{%- if constr_type_e == "BGP" and dims_nphi_e > 0 %} +OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_phi_e_constraint.c +{%- endif %} + +{%- if constr_type == "BGH" and dims_nh > 0 %} +OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt.c +OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun.c + {%- if hessian_approx == "EXACT" %} +OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt_hess.c + {%- endif %} +{%- endif %} + +{%- if constr_type_e == "BGH" and dims_nh_e > 0 %} +OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt.c +OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun.c + {%- if hessian_approx == "EXACT" %} +OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess.c + {%- endif %} +{%- endif %} + +{%- if cost_type_0 == "NONLINEAR_LS" %} +OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_fun.c +OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_fun_jac_ut_xt.c +OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_hess.c +{%- elif cost_type_0 == "EXTERNAL" %} + {%- if cost.cost_ext_fun_type_0 == "casadi" %} +OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun.c +OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac.c +OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac_hess.c + {%- else %} +OCP_SRC+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost_0 }} + {%- endif %} +{%- endif %} +{%- if cost_type == "NONLINEAR_LS" %} +OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_fun.c +OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_fun_jac_ut_xt.c +OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_hess.c +{%- elif cost_type == "EXTERNAL" %} + {%- if cost.cost_ext_fun_type == "casadi" %} +OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun.c +OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac.c +OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac_hess.c + {%- elif cost.cost_source_ext_cost != cost.cost_source_ext_cost_0 %} +OCP_SRC+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost }} + {%- endif %} +{%- endif %} +{%- if cost_type_e == "NONLINEAR_LS" %} +OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_fun.c +OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_fun_jac_ut_xt.c +OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_hess.c +{%- elif cost_type_e == "EXTERNAL" %} + {%- if cost.cost_ext_fun_type_e == "casadi" %} +OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun.c +OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac.c +OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac_hess.c + {%- elif cost.cost_source_ext_cost_e != cost.cost_source_ext_cost_0 %} +OCP_SRC+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost_e }} + {%- endif %} +{%- endif %} +OCP_SRC+= acados_solver_{{ model.name }}.c +OCP_OBJ := $(OCP_SRC:.c=.o) + +# for sim solver +SIM_SRC= acados_sim_solver_{{ model.name }}.c +SIM_OBJ := $(SIM_SRC:.c=.o) + +# for target example +EX_SRC= main_{{ model.name }}.c +EX_OBJ := $(EX_SRC:.c=.o) +EX_EXE := $(EX_SRC:.c=) + +# for target example_sim +EX_SIM_SRC= main_sim_{{ model.name }}.c +EX_SIM_OBJ := $(EX_SIM_SRC:.c=.o) +EX_SIM_EXE := $(EX_SIM_SRC:.c=) + +# combine model, sim and ocp object files +OBJ= +OBJ+= $(MODEL_OBJ) +{%- if solver_options.integrator_type != "DISCRETE" %} +OBJ+= $(SIM_OBJ) +{%- endif %} +OBJ+= $(OCP_OBJ) + +EXTERNAL_DIR= +EXTERNAL_LIB= + +{%- if model_external_shared_lib_dir and model_external_shared_lib_name %} +EXTERNAL_DIR+= {{ model_external_shared_lib_dir }} +EXTERNAL_LIB+= {{ model_external_shared_lib_name }} +{%- endif %} + +INCLUDE_PATH = {{ acados_include_path }} +LIB_PATH = {{ acados_lib_path }} + +# preprocessor flags for make's implicit rules +{%- if qp_solver == "FULL_CONDENSING_QPOASES" %} +CPPFLAGS += -DACADOS_WITH_QPOASES +{%- endif %} +{%- if qp_solver == "PARTIAL_CONDENSING_OSQP" %} +CPPFLAGS += -DACADOS_WITH_OSQP +{%- endif %} +{%- if qp_solver == "PARTIAL_CONDENSING_QPDUNES" %} +CPPFLAGS += -DACADOS_WITH_QPDUNES +{%- endif %} +CPPFLAGS+= -I$(INCLUDE_PATH) +CPPFLAGS+= -I$(INCLUDE_PATH)/acados +CPPFLAGS+= -I$(INCLUDE_PATH)/blasfeo/include +CPPFLAGS+= -I$(INCLUDE_PATH)/hpipm/include + {%- if qp_solver == "FULL_CONDENSING_QPOASES" %} +CPPFLAGS+= -I $(INCLUDE_PATH)/qpOASES_e/ + {%- endif %} + +{# c-compiler flags #} +# define the c-compiler flags for make's implicit rules +CFLAGS = -fPIC -std=c99 {{ openmp_flag }} #-fno-diagnostics-show-line-numbers -g +# # Debugging +# CFLAGS += -g3 + +# linker flags +LDFLAGS+= -L$(LIB_PATH) + +# link to libraries +LDLIBS+= -lacados +LDLIBS+= -lhpipm +LDLIBS+= -lblasfeo +LDLIBS+= -lm +LDLIBS+= {{ link_libs }} + +# libraries +LIBACADOS_SOLVER=libacados_solver_{{ model.name }}.so +LIBACADOS_OCP_SOLVER=libacados_ocp_solver_{{ model.name }}.so +LIBACADOS_SIM_SOLVER=lib$(SIM_SRC:.c=.so) + +# virtual targets +.PHONY : all clean + +#all: clean example_sim example shared_lib +{% if solver_options.integrator_type == "DISCRETE" -%} +all: clean example +shared_lib: ocp_shared_lib +{%- else %} +all: clean example_sim example +shared_lib: bundled_shared_lib ocp_shared_lib sim_shared_lib +{%- endif %} + +# some linker targets +example: $(EX_OBJ) $(OBJ) + $(CC) $^ -o $(EX_EXE) $(LDFLAGS) $(LDLIBS) + +example_sim: $(EX_SIM_OBJ) $(MODEL_OBJ) $(SIM_OBJ) + $(CC) $^ -o $(EX_SIM_EXE) $(LDFLAGS) $(LDLIBS) + +{% if solver_options.integrator_type != "DISCRETE" -%} +bundled_shared_lib: $(OBJ) + $(CC) -shared $^ -o $(LIBACADOS_SOLVER) $(LDFLAGS) $(LDLIBS) +{%- endif %} + +ocp_shared_lib: $(OCP_OBJ) $(MODEL_OBJ) + $(CC) -shared $^ -o $(LIBACADOS_OCP_SOLVER) $(LDFLAGS) $(LDLIBS) \ + -L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB) + +sim_shared_lib: $(SIM_OBJ) $(MODEL_OBJ) + $(CC) -shared $^ -o $(LIBACADOS_SIM_SOLVER) $(LDFLAGS) $(LDLIBS) + + +# Cython targets +ocp_cython_c: ocp_shared_lib + cython \ + -o acados_ocp_solver_pyx.c \ + -I $(INCLUDE_PATH)/../interfaces/acados_template/acados_template \ + $(INCLUDE_PATH)/../interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx \ + -I {{ code_export_directory }} \ + +ocp_cython_o: ocp_cython_c + $(CC) $(ACADOS_FLAGS) -c -O2 \ + -fPIC \ + -o acados_ocp_solver_pyx.o \ + -I /usr/include/python3.8 \ + -I $(INCLUDE_PATH)/blasfeo/include/ \ + -I $(INCLUDE_PATH)/hpipm/include/ \ + -I $(INCLUDE_PATH) \ + -I {{ cython_include_dirs }} \ + acados_ocp_solver_pyx.c \ + +ocp_cython: ocp_cython_o + $(CC) $(ACADOS_FLAGS) -shared \ + -o acados_ocp_solver_pyx.so \ + -Wl,-rpath=$(LIB_PATH) \ + acados_ocp_solver_pyx.o \ + $(abspath .)/libacados_ocp_solver_{{ model.name }}.so \ + $(LDFLAGS) $(LDLIBS) + +{%- if os and os == "pc" %} + +clean: + del \Q *.o 2>nul + del \Q *.so 2>nul + del \Q main_{{ model.name }} 2>nul + +clean_ocp_shared_lib: + del \Q libacados_ocp_solver_{{ model.name }}.so 2>nul + del \Q acados_solver_{{ model.name }}.o 2>nul + +clean_ocp_cython: + del \Q libacados_ocp_solver_{{ model.name }}.so 2>nul + del \Q acados_solver_{{ model.name }}.o 2>nul + del \Q acados_ocp_solver_pyx.so 2>nul + del \Q acados_ocp_solver_pyx.o 2>nul + +{%- else %} + +clean: + $(RM) $(OBJ) $(EX_OBJ) $(EX_SIM_OBJ) + $(RM) $(LIBACADOS_SOLVER) $(LIBACADOS_OCP_SOLVER) $(LIBACADOS_SIM_SOLVER) + $(RM) $(EX_EXE) $(EX_SIM_EXE) + +clean_ocp_shared_lib: + $(RM) $(LIBACADOS_OCP_SOLVER) + $(RM) $(OCP_OBJ) + +clean_ocp_cython: + $(RM) libacados_ocp_solver_{{ model.name }}.so + $(RM) acados_solver_{{ model.name }}.o + $(RM) acados_ocp_solver_pyx.so + $(RM) acados_ocp_solver_pyx.o + +{%- endif %} diff --git a/third_party/acados/acados_template/c_templates_tera/acados_mex_create.in.c b/third_party/acados/acados_template/c_templates_tera/acados_mex_create.in.c new file mode 100644 index 0000000000..e67a51567a --- /dev/null +++ b/third_party/acados/acados_template/c_templates_tera/acados_mex_create.in.c @@ -0,0 +1,387 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +// standard +#include +#include +#include + +// acados +#include "acados/utils/print.h" +#include "acados_c/ocp_nlp_interface.h" +#include "acados_solver_{{ model.name }}.h" + +// mex +#include "mex.h" + + +void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) +{ + long long *l_ptr; + int status = 0; + + // create solver + {{ model.name }}_solver_capsule *acados_ocp_capsule = {{ model.name }}_acados_create_capsule(); + + status = {{ model.name }}_acados_create(acados_ocp_capsule); + + if (status) + { + mexPrintf("{{ model.name }}_acados_create() returned status %d.\n", status); + } + mexPrintf("{{ model.name }}_acados_create() -> success!\n"); + + // get pointers to nlp solver related objects + ocp_nlp_plan_t *nlp_plan = {{ model.name }}_acados_get_nlp_plan(acados_ocp_capsule); + ocp_nlp_config *nlp_config = {{ model.name }}_acados_get_nlp_config(acados_ocp_capsule); + ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims(acados_ocp_capsule); + ocp_nlp_in *nlp_in = {{ model.name }}_acados_get_nlp_in(acados_ocp_capsule); + ocp_nlp_out *nlp_out = {{ model.name }}_acados_get_nlp_out(acados_ocp_capsule); + ocp_nlp_solver *nlp_solver = {{ model.name }}_acados_get_nlp_solver(acados_ocp_capsule); + void *nlp_opts = {{ model.name }}_acados_get_nlp_opts(acados_ocp_capsule); + + // mexPrintf("acados: got pointer to objectes!\n"); + + // field names of output struct + #define FIELDS_OCP 9 + #define FIELDS_EXT_FUN 25 + #define MAX_FIELDS 25 + char *fieldnames[MAX_FIELDS]; + + for (int i = 0; i < MAX_FIELDS; i++) + { + fieldnames[i] = (char*) mxMalloc(50); + } + + memcpy(fieldnames[0],"config",sizeof("config")); + memcpy(fieldnames[1],"dims",sizeof("dims")); + memcpy(fieldnames[2],"opts",sizeof("opts")); + memcpy(fieldnames[3],"in",sizeof("in")); + memcpy(fieldnames[4],"out",sizeof("out")); + memcpy(fieldnames[5],"solver",sizeof("solver")); + memcpy(fieldnames[6],"sens_out",sizeof("sens_out")); + memcpy(fieldnames[7],"plan",sizeof("plan")); + memcpy(fieldnames[8],"capsule",sizeof("capsule")); + + // create output struct - C_ocp + plhs[0] = mxCreateStructMatrix(1, 1, 9, (const char **) fieldnames); + + // MEX: config, dims, opts, in, out, solver, sens_out, plan + // plan + mxArray *plan_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL); + l_ptr = mxGetData(plan_mat); + l_ptr[0] = (long long) nlp_plan; + mxSetField(plhs[0], 0, "plan", plan_mat); + + // config + mxArray *config_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL); + l_ptr = mxGetData(config_mat); + l_ptr[0] = (long long) nlp_config; + mxSetField(plhs[0], 0, "config", config_mat); + + // dims + mxArray *dims_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL); + l_ptr = mxGetData(dims_mat); + l_ptr[0] = (long long) nlp_dims; + mxSetField(plhs[0], 0, "dims", dims_mat); + + // opts + mxArray *opts_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL); + l_ptr = mxGetData(opts_mat); + l_ptr[0] = (long long) nlp_opts; + mxSetField(plhs[0], 0, "opts", opts_mat); + + // in + mxArray *in_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL); + l_ptr = mxGetData(in_mat); + l_ptr[0] = (long long) nlp_in; + mxSetField(plhs[0], 0, "in", in_mat); + + // out + mxArray *out_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL); + l_ptr = mxGetData(out_mat); + l_ptr[0] = (long long) nlp_out; + mxSetField(plhs[0], 0, "out", out_mat); + + // solver + mxArray *solver_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL); + l_ptr = mxGetData(solver_mat); + l_ptr[0] = (long long) nlp_solver; + mxSetField(plhs[0], 0, "solver", solver_mat); + + // TODO: sens_out not actually implemented in templates.. + // sens_out + mxArray *sens_out_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL); + l_ptr = mxGetData(sens_out_mat); + l_ptr[0] = (long long) 1; + mxSetField(plhs[0], 0, "sens_out", sens_out_mat); + + // capsule + mxArray *capsule_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL); + l_ptr = mxGetData(capsule_mat); + l_ptr[0] = (long long) acados_ocp_capsule; + mxSetField(plhs[0], 0, "capsule", capsule_mat); + + /* store external function pointers */ + // dyn + memcpy(fieldnames[0],"expl_ode_fun",sizeof("expl_ode_fun")); + memcpy(fieldnames[1],"forw_vde",sizeof("forw_vde")); + memcpy(fieldnames[2],"hess_vde",sizeof("hess_vde")); + memcpy(fieldnames[3],"impl_dae_fun",sizeof("impl_dae_fun")); + memcpy(fieldnames[4],"impl_dae_fun_jac_x_xdot_z",sizeof("impl_dae_fun_jac_x_xdot_z")); + memcpy(fieldnames[5],"impl_dae_jac_x_xdot_u_z",sizeof("impl_dae_jac_x_xdot_u_z")); + memcpy(fieldnames[6],"impl_dae_hess",sizeof("impl_dae_hess")); + + memcpy(fieldnames[7],"gnsf_phi_fun",sizeof("gnsf_phi_fun")); + memcpy(fieldnames[8],"gnsf_phi_fun_jac_y",sizeof("gnsf_phi_fun_jac_y")); + memcpy(fieldnames[9],"gnsf_phi_jac_y_uhat",sizeof("gnsf_phi_jac_y_uhat")); + memcpy(fieldnames[10],"gnsf_f_lo_jac_x1_x1dot_u_z",sizeof("gnsf_f_lo_jac_x1_x1dot_u_z")); + memcpy(fieldnames[11],"gnsf_get_matrices_fun",sizeof("gnsf_get_matrices_fun")); + + memcpy(fieldnames[12],"disc_phi_fun",sizeof("disc_phi_fun")); + memcpy(fieldnames[13],"disc_phi_fun_jac",sizeof("disc_phi_fun_jac")); + memcpy(fieldnames[14],"disc_phi_fun_jac_hess",sizeof("disc_phi_fun_jac_hess")); + + // cost + memcpy(fieldnames[15],"cost_y_fun",sizeof("cost_y_fun")); + memcpy(fieldnames[16],"cost_y_fun_jac_ut_xt",sizeof("cost_y_fun_jac_ut_xt")); + memcpy(fieldnames[17],"cost_y_hess",sizeof("cost_y_hess")); + memcpy(fieldnames[18],"ext_cost_fun",sizeof("ext_cost_fun")); + memcpy(fieldnames[19],"ext_cost_fun_jac",sizeof("ext_cost_fun_jac")); + memcpy(fieldnames[20],"ext_cost_fun_jac_hess",sizeof("ext_cost_fun_jac_hess")); + + // constraints + memcpy(fieldnames[21],"phi_constraint",sizeof("phi_constraint")); + memcpy(fieldnames[22],"nl_constr_h_fun_jac",sizeof("nl_constr_h_fun_jac")); + memcpy(fieldnames[23],"nl_constr_h_fun",sizeof("nl_constr_h_fun")); + memcpy(fieldnames[24],"nl_constr_h_fun_jac_hess",sizeof("nl_constr_h_fun_jac_hess")); + + + // create output struct - C_ocp_ext_fun + plhs[1] = mxCreateStructMatrix(1, 1, FIELDS_EXT_FUN, (const char **) fieldnames); + + + for (int i = 0; i < FIELDS_EXT_FUN; i++) + { + mxFree( fieldnames[i] ); + } + +/* dynamics */ + mxArray *expl_ode_fun_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL); + mxArray *forw_vde_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL); + mxArray *hess_vde_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL); + mxArray *impl_dae_fun_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL); + mxArray *impl_dae_fun_jac_x_xdot_z_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL); + mxArray *impl_dae_jac_x_xdot_u_z_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL); + mxArray *impl_dae_hess_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL); + + mxArray *gnsf_phi_fun_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL); + mxArray *gnsf_phi_fun_jac_y_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL); + mxArray *gnsf_phi_jac_y_uhat_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL); + mxArray *gnsf_f_lo_jac_x1_x1dot_u_z_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL); + mxArray *gnsf_get_matrices_fun_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL); + + mxArray *disc_phi_fun_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL); + mxArray *disc_phi_fun_jac_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL); + mxArray *disc_phi_fun_jac_hess_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL); + +{% if solver_options.integrator_type == "ERK" %} + {# TODO: remove _casadi from these names.. #} + l_ptr = mxGetData(forw_vde_mat); + l_ptr[0] = (long long) acados_ocp_capsule->forw_vde_casadi; + l_ptr = mxGetData(expl_ode_fun_mat); + l_ptr[0] = (long long) acados_ocp_capsule->expl_ode_fun; +{% if solver_options.hessian_approx == "EXACT" %} + l_ptr = mxGetData(hess_vde_mat); + l_ptr[0] = (long long) acados_ocp_capsule->hess_vde_casadi; +{%- endif %} +{% elif solver_options.integrator_type == "IRK" %} + l_ptr = mxGetData(impl_dae_fun_mat); + l_ptr[0] = (long long) acados_ocp_capsule->impl_dae_fun; + l_ptr = mxGetData(impl_dae_fun_jac_x_xdot_z_mat); + l_ptr[0] = (long long) acados_ocp_capsule->impl_dae_fun_jac_x_xdot_z; + l_ptr = mxGetData(impl_dae_jac_x_xdot_u_z_mat); + l_ptr[0] = (long long) acados_ocp_capsule->impl_dae_jac_x_xdot_u_z; +{% if solver_options.hessian_approx == "EXACT" %} + l_ptr = mxGetData(impl_dae_hess_mat); + l_ptr[0] = (long long) acados_ocp_capsule->impl_dae_hess; +{%- endif %} +{% elif solver_options.integrator_type == "GNSF" %} + {% if model.gnsf.purely_linear != 1 %} + l_ptr = mxGetData(gnsf_phi_fun_mat); + l_ptr[0] = (long long) acados_ocp_capsule->gnsf_phi_fun; + l_ptr = mxGetData(gnsf_phi_fun_jac_y_mat); + l_ptr[0] = (long long) acados_ocp_capsule->gnsf_phi_fun_jac_y; + l_ptr = mxGetData(gnsf_phi_jac_y_uhat_mat); + l_ptr[0] = (long long) acados_ocp_capsule->gnsf_phi_jac_y_uhat; + {% if model.gnsf.nontrivial_f_LO == 1 %} + l_ptr = mxGetData(gnsf_f_lo_jac_x1_x1dot_u_z_mat); + l_ptr[0] = (long long) acados_ocp_capsule->gnsf_f_lo_jac_x1_x1dot_u_z; + {%- endif %} + {%- endif %} + l_ptr = mxGetData(gnsf_get_matrices_fun_mat); + l_ptr[0] = (long long) acados_ocp_capsule->gnsf_get_matrices_fun; +{% elif solver_options.integrator_type == "DISCRETE" %} + l_ptr = mxGetData(disc_phi_fun_mat); + l_ptr[0] = (long long) acados_ocp_capsule->discr_dyn_phi_fun; + l_ptr = mxGetData(disc_phi_fun_jac_mat); + l_ptr[0] = (long long) acados_ocp_capsule->discr_dyn_phi_fun_jac_ut_xt; +{% if solver_options.hessian_approx == "EXACT" %} + l_ptr = mxGetData(disc_phi_fun_jac_hess_mat); + l_ptr[0] = (long long) acados_ocp_capsule->discr_dyn_phi_fun_jac_ut_xt_hess; +{%- endif %} +{%- endif %} + mxSetField(plhs[1], 0, "expl_ode_fun", expl_ode_fun_mat); + mxSetField(plhs[1], 0, "forw_vde", forw_vde_mat); + mxSetField(plhs[1], 0, "hess_vde", hess_vde_mat); + + mxSetField(plhs[1], 0, "gnsf_phi_fun", gnsf_phi_fun_mat); + mxSetField(plhs[1], 0, "gnsf_phi_fun_jac_y", gnsf_phi_fun_jac_y_mat); + mxSetField(plhs[1], 0, "gnsf_phi_jac_y_uhat", gnsf_phi_jac_y_uhat_mat); + mxSetField(plhs[1], 0, "gnsf_f_lo_jac_x1_x1dot_u_z", gnsf_f_lo_jac_x1_x1dot_u_z_mat); + mxSetField(plhs[1], 0, "gnsf_get_matrices_fun", gnsf_get_matrices_fun_mat); + + mxSetField(plhs[1], 0, "impl_dae_fun", impl_dae_fun_mat); + mxSetField(plhs[1], 0, "impl_dae_fun_jac_x_xdot_z", impl_dae_fun_jac_x_xdot_z_mat); + mxSetField(plhs[1], 0, "impl_dae_jac_x_xdot_u_z", impl_dae_jac_x_xdot_u_z_mat); + mxSetField(plhs[1], 0, "impl_dae_hess", impl_dae_hess_mat); + + mxSetField(plhs[1], 0, "disc_phi_fun", disc_phi_fun_mat); + mxSetField(plhs[1], 0, "disc_phi_fun_jac", disc_phi_fun_jac_mat); + mxSetField(plhs[1], 0, "disc_phi_fun_jac_hess", disc_phi_fun_jac_hess_mat); +/* constaints */ + mxArray *phi_constraint_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL); + l_ptr = mxGetData(phi_constraint_mat); +{%- if constraints.constr_type == "BGP" %} + l_ptr[0] = (long long) acados_ocp_capsule->phi_constraint; +{% endif %} +{% if constraints.constr_type_e == "BGP" %} + l_ptr[1] = (long long) &acados_ocp_capsule->phi_e_constraint; +{% endif %} + mxSetField(plhs[1], 0, "phi_constraint", phi_constraint_mat); + + mxArray *nl_constr_h_fun_jac_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL); + l_ptr = mxGetData(nl_constr_h_fun_jac_mat); +{% if constraints.constr_type == "BGH" and dims.nh > 0 %} + l_ptr[0] = (long long) acados_ocp_capsule->nl_constr_h_fun_jac; +{% endif %} +{% if constraints.constr_type_e == "BGH" and dims.nh_e > 0 %} + l_ptr[1] = (long long) &acados_ocp_capsule->nl_constr_h_e_fun_jac; +{%- endif %} + mxSetField(plhs[1], 0, "nl_constr_h_fun_jac", nl_constr_h_fun_jac_mat); + + mxArray *nl_constr_h_fun_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL); + l_ptr = mxGetData(nl_constr_h_fun_mat); +{% if constraints.constr_type == "BGH" and dims.nh > 0 %} + l_ptr[0] = (long long) acados_ocp_capsule->nl_constr_h_fun; +{% endif %} +{% if constraints.constr_type_e == "BGH" and dims.nh_e > 0 %} + l_ptr[1] = (long long) &acados_ocp_capsule->nl_constr_h_e_fun; +{%- endif %} + mxSetField(plhs[1], 0, "nl_constr_h_fun", nl_constr_h_fun_mat); + + mxArray *nl_constr_h_fun_jac_hess_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL); + l_ptr = mxGetData(nl_constr_h_fun_jac_hess_mat); +{% if constraints.constr_type == "BGH" and dims.nh > 0 and solver_options.hessian_approx == "EXACT" %} + l_ptr[0] = (long long) acados_ocp_capsule->nl_constr_h_fun_jac_hess; +{% endif %} +{% if constraints.constr_type_e == "BGH" and dims.nh_e > 0 and solver_options.hessian_approx == "EXACT" %} + l_ptr[1] = (long long) &acados_ocp_capsule->nl_constr_h_e_fun_jac_hess; +{%- endif %} + mxSetField(plhs[1], 0, "nl_constr_h_fun_jac_hess", nl_constr_h_fun_jac_hess_mat); + +/* cost */ + mxArray *cost_y_fun_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL); + l_ptr = mxGetData(cost_y_fun_mat); +{% if cost.cost_type == "NONLINEAR_LS" %} + l_ptr[0] = (long long) acados_ocp_capsule->cost_y_fun; +{% endif %} +{% if cost.cost_type_e == "NONLINEAR_LS" %} + l_ptr[1] = (long long) &acados_ocp_capsule->cost_y_e_fun; +{%- endif %} + mxSetField(plhs[1], 0, "cost_y_fun", cost_y_fun_mat); + + mxArray *cost_y_fun_jac_ut_xt_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL); + l_ptr = mxGetData(cost_y_fun_jac_ut_xt_mat); +{% if cost.cost_type == "NONLINEAR_LS" %} + l_ptr[0] = (long long) acados_ocp_capsule->cost_y_fun_jac_ut_xt; +{% endif %} +{% if cost.cost_type_e == "NONLINEAR_LS" %} + l_ptr[1] = (long long) &acados_ocp_capsule->cost_y_e_fun_jac_ut_xt; +{%- endif %} + mxSetField(plhs[1], 0, "cost_y_fun_jac_ut_xt", cost_y_fun_jac_ut_xt_mat); + + mxArray *cost_y_hess_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL); + l_ptr = mxGetData(cost_y_hess_mat); +{% if cost.cost_type == "NONLINEAR_LS" %} + l_ptr[0] = (long long) acados_ocp_capsule->cost_y_hess; +{% endif %} +{% if cost.cost_type_e == "NONLINEAR_LS" %} + l_ptr[1] = (long long) &acados_ocp_capsule->cost_y_e_hess; +{%- endif %} + mxSetField(plhs[1], 0, "cost_y_hess", cost_y_hess_mat); + + mxArray *ext_cost_fun_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL); + l_ptr = mxGetData(ext_cost_fun_mat); +{% if cost.cost_type == "EXTERNAL" %} + l_ptr[0] = (long long) acados_ocp_capsule->ext_cost_fun; +{% endif -%} +{% if cost.cost_type_e == "EXTERNAL" %} + l_ptr[1] = (long long) &acados_ocp_capsule->ext_cost_e_fun; +{%- endif %} + mxSetField(plhs[1], 0, "ext_cost_fun", ext_cost_fun_mat); + + mxArray *ext_cost_fun_jac_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL); + l_ptr = mxGetData(ext_cost_fun_jac_mat); +{% if cost.cost_type == "EXTERNAL" %} + l_ptr[0] = (long long) acados_ocp_capsule->ext_cost_fun_jac; +{% endif -%} +{% if cost.cost_type_e == "EXTERNAL" %} + l_ptr[1] = (long long) &acados_ocp_capsule->ext_cost_e_fun_jac; +{%- endif %} + mxSetField(plhs[1], 0, "ext_cost_fun_jac", ext_cost_fun_jac_mat); + + mxArray *ext_cost_fun_jac_hess_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL); + l_ptr = mxGetData(ext_cost_fun_jac_hess_mat); +{% if cost.cost_type == "EXTERNAL" %} + l_ptr[0] = (long long) acados_ocp_capsule->ext_cost_fun_jac_hess; +{% endif -%} +{% if cost.cost_type_e == "EXTERNAL" %} + l_ptr[1] = (long long) &acados_ocp_capsule->ext_cost_e_fun_jac_hess; +{%- endif %} + mxSetField(plhs[1], 0, "ext_cost_fun_jac_hess", ext_cost_fun_jac_hess_mat); + + + return; +} diff --git a/third_party/acados/acados_template/c_templates_tera/acados_mex_free.in.c b/third_party/acados/acados_template/c_templates_tera/acados_mex_free.in.c new file mode 100644 index 0000000000..560adb0b98 --- /dev/null +++ b/third_party/acados/acados_template/c_templates_tera/acados_mex_free.in.c @@ -0,0 +1,70 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + +// system +#include +#include +#include +// acados +#include "acados_solver_{{ model.name }}.h" + +// mex +#include "mex.h" + + +void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) +{ + int status = 0; + long long *ptr; + + // mexPrintf("\nin mex_acados_free\n"); + const mxArray *C_ocp = prhs[0]; + // capsule + ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "capsule" ) ); + {{ model.name }}_solver_capsule *capsule = ({{ model.name }}_solver_capsule *) ptr[0]; + + status = {{ model.name }}_acados_free(capsule); + if (status) + { + mexPrintf("{{ model.name }}_acados_free() returned status %d.\n", status); + } + + status = {{ model.name }}_acados_free_capsule(capsule); + if (status) + { + mexPrintf("{{ model.name }}_acados_free_capsule() returned status %d.\n", status); + } + + return; +} + diff --git a/third_party/acados/acados_template/c_templates_tera/acados_mex_set.in.c b/third_party/acados/acados_template/c_templates_tera/acados_mex_set.in.c new file mode 100644 index 0000000000..f8e1e5e445 --- /dev/null +++ b/third_party/acados/acados_template/c_templates_tera/acados_mex_set.in.c @@ -0,0 +1,570 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +// standard +#include +#include +#include + +// acados +#include "acados/utils/print.h" +#include "acados_c/ocp_nlp_interface.h" +#include "acados_solver_{{ model.name }}.h" + +// mex +#include "mex.h" +#include "mex_macros.h" + + +void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) +{ + + long long *ptr; + int acados_size; + mxArray *mex_field; + char fun_name[20] = "ocp_set"; + char buffer [500]; // for error messages + + /* RHS */ + int min_nrhs = 6; + + char *ext_fun_type = mxArrayToString( prhs[0] ); + char *ext_fun_type_e = mxArrayToString( prhs[1] ); + + // C ocp + const mxArray *C_ocp = prhs[2]; + // capsule + ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "capsule" ) ); + {{ model.name }}_solver_capsule *capsule = ({{ model.name }}_solver_capsule *) ptr[0]; + // plan + ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "plan" ) ); + ocp_nlp_plan_t *plan = (ocp_nlp_plan_t *) ptr[0]; + // config + ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "config" ) ); + ocp_nlp_config *config = (ocp_nlp_config *) ptr[0]; + // dims + ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "dims" ) ); + ocp_nlp_dims *dims = (ocp_nlp_dims *) ptr[0]; + // opts + ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "opts" ) ); + void *opts = (void *) ptr[0]; + // in + ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "in" ) ); + ocp_nlp_in *in = (ocp_nlp_in *) ptr[0]; + // out + ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "out" ) ); + ocp_nlp_out *out = (ocp_nlp_out *) ptr[0]; + // solver + ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "solver" ) ); + ocp_nlp_solver *solver = (ocp_nlp_solver *) ptr[0]; + + const mxArray *C_ext_fun_pointers = prhs[3]; + // field + char *field = mxArrayToString( prhs[4] ); + // value + double *value = mxGetPr( prhs[5] ); + + // for checks + int matlab_size = (int) mxGetNumberOfElements( prhs[5] ); + int nrow = (int) mxGetM( prhs[5] ); + int ncol = (int) mxGetN( prhs[5] ); + + int N = dims->N; + int nu = dims->nu[0]; + int nx = dims->nx[0]; + + // stage + int s0, se; + if (nrhs==min_nrhs) + { + s0 = 0; + se = N; + } + else if (nrhs==min_nrhs+1) + { + s0 = mxGetScalar( prhs[6] ); + if (s0 > N) + { + sprintf(buffer, "ocp_set: N < specified stage = %d\n", s0); + mexErrMsgTxt(buffer); + } + se = s0 + 1; + } + else + { + sprintf(buffer, "ocp_set: wrong nrhs: %d\n", nrhs); + mexErrMsgTxt(buffer); + } + + /* Set value */ + // constraints + if (!strcmp(field, "constr_x0")) + { + acados_size = nx; + MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); + ocp_nlp_constraints_model_set(config, dims, in, 0, "lbx", value); + ocp_nlp_constraints_model_set(config, dims, in, 0, "ubx", value); + } + else if (!strcmp(field, "constr_C")) + { + for (int ii=s0; iinlp_cost[ii] == LINEAR_LS) || (plan->nlp_cost[ii] == NONLINEAR_LS)) + { + acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "y_ref"); + MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); + ocp_nlp_cost_model_set(config, dims, in, ii, "y_ref", value); + } + else + { + MEX_FIELD_NOT_SUPPORTED_FOR_COST_STAGE(fun_name, field, plan->nlp_cost[ii], ii); + } + } + } + else if (!strcmp(field, "cost_y_ref_e")) + { + acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, N, "y_ref"); + MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); + ocp_nlp_cost_model_set(config, dims, in, N, "y_ref", value); + } + else if (!strcmp(field, "cost_Vu")) + { + for (int ii=s0; iinlp_cost[ii] == LINEAR_LS) || (plan->nlp_cost[ii] == NONLINEAR_LS)) + { + int ny = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "y_ref"); + int nu = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "u"); + acados_size = ny * nu; + MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); + ocp_nlp_cost_model_set(config, dims, in, ii, "Vu", value); + } + else + { + MEX_FIELD_NOT_SUPPORTED_FOR_COST_STAGE(fun_name, field, plan->nlp_cost[ii], ii); + } + } + } + else if (!strcmp(field, "cost_Vx")) + { + for (int ii=s0; iinlp_cost[ii] == LINEAR_LS) || (plan->nlp_cost[ii] == NONLINEAR_LS)) + { + int ny = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "y_ref"); + int nx = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "x"); + acados_size = ny * nx; + MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); + ocp_nlp_cost_model_set(config, dims, in, ii, "Vx", value); + } + else + { + MEX_FIELD_NOT_SUPPORTED_FOR_COST_STAGE(fun_name, field, plan->nlp_cost[ii], ii); + } + } + } + else if (!strcmp(field, "cost_W")) + { + for (int ii=s0; iinlp_cost[ii] == LINEAR_LS) || (plan->nlp_cost[ii] == NONLINEAR_LS)) + { + int ny = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "y_ref"); + acados_size = ny * ny; + MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); + ocp_nlp_cost_model_set(config, dims, in, ii, "W", value); + } + else + { + MEX_FIELD_NOT_SUPPORTED_FOR_COST_STAGE(fun_name, field, plan->nlp_cost[ii], ii); + } + } + } + else if (!strcmp(field, "cost_Z")) + { + acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "cost_Z"); + MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); + for (int ii=s0; iisim_solver_plan[0]; + sim_solver_t type = sim_plan.sim_solver; + if (type == IRK) + { + int nz = ocp_nlp_dims_get_from_attr(config, dims, out, 0, "z"); + if (nrhs!=min_nrhs) + MEX_SETTER_NO_STAGE_SUPPORT(fun_name, field) + + acados_size = N*nz; + MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); + for (int ii=0; iisim_solver_plan[0]; + sim_solver_t type = sim_plan.sim_solver; + if (type == IRK) + { + int nx = ocp_nlp_dims_get_from_attr(config, dims, out, 0, "x"); + if (nrhs!=min_nrhs) + MEX_SETTER_NO_STAGE_SUPPORT(fun_name, field) + + acados_size = N*nx; + MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); + for (int ii=0; iisim_solver_plan[0]; + sim_solver_t type = sim_plan.sim_solver; + if (type == GNSF) + { + int nout = ocp_nlp_dims_get_from_attr(config, dims, out, 0, "init_gnsf_phi"); + + if (nrhs!=min_nrhs) + MEX_SETTER_NO_STAGE_SUPPORT(fun_name, field) + + acados_size = N*nout; + MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); + for (int ii=0; iinlp_solver == SQP && rti_phase != 0) + { + MEX_FIELD_ONLY_SUPPORTED_FOR_SOLVER(fun_name, field, "sqp_rti") + } + ocp_nlp_solver_opts_set(config, opts, "rti_phase", &rti_phase); + } + else if (!strcmp(field, "qp_warm_start")) + { + acados_size = 1; + MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); + int qp_warm_start = (int) value[0]; + ocp_nlp_solver_opts_set(config, opts, "qp_warm_start", &qp_warm_start); + } + else if (!strcmp(field, "warm_start_first_qp")) + { + acados_size = 1; + MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); + int warm_start_first_qp = (int) value[0]; + ocp_nlp_solver_opts_set(config, opts, "warm_start_first_qp", &warm_start_first_qp); + } + else if (!strcmp(field, "print_level")) + { + acados_size = 1; + MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); + int print_level = (int) value[0]; + ocp_nlp_solver_opts_set(config, opts, "print_level", &print_level); + } + else + { + MEX_FIELD_NOT_SUPPORTED_SUGGEST(fun_name, field, "p, constr_x0,\ + constr_lbx, constr_ubx, constr_C, constr_D, constr_lg, constr_ug, constr_lh, constr_uh\ + constr_lbu, constr_ubu, cost_y_ref[_e],\ + cost_Vu, cost_Vx, cost_Vz, cost_W, cost_Z, cost_Zl, cost_Zu, cost_z,\ + cost_zl, cost_zu, init_x, init_u, init_z, init_xdot, init_gnsf_phi,\ + init_pi, nlp_solver_max_iter, qp_warm_start, warm_start_first_qp, print_level"); + } + + return; +} + diff --git a/third_party/acados/acados_template/c_templates_tera/acados_mex_solve.in.c b/third_party/acados/acados_template/c_templates_tera/acados_mex_solve.in.c new file mode 100644 index 0000000000..c673d4bf22 --- /dev/null +++ b/third_party/acados/acados_template/c_templates_tera/acados_mex_solve.in.c @@ -0,0 +1,59 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + +// system +#include +#include +#include +// acados +#include "acados_solver_{{ model.name }}.h" + +// mex +#include "mex.h" + + + +void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) +{ + // C_ocp + long long *ptr; + const mxArray *C_ocp = prhs[0]; + + // capsule + ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "capsule" ) ); + {{ model.name }}_solver_capsule *capsule = ({{ model.name }}_solver_capsule *) ptr[0]; + + // solve + {{ model.name }}_acados_solve(capsule); + +} diff --git a/third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.c b/third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.c new file mode 100644 index 0000000000..f2e75058c1 --- /dev/null +++ b/third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.c @@ -0,0 +1,508 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + +{%- if solver_options.hessian_approx %} + {%- set hessian_approx = solver_options.hessian_approx %} +{%- elif solver_options.sens_hess %} + {%- set hessian_approx = "EXACT" %} +{%- else %} + {%- set hessian_approx = "GAUSS_NEWTON" %} +{%- endif %} +// standard +#include +#include + +// acados +#include "acados_c/external_function_interface.h" +#include "acados_c/sim_interface.h" +#include "acados_c/external_function_interface.h" + +#include "acados/sim/sim_common.h" +#include "acados/utils/external_function_generic.h" +#include "acados/utils/print.h" + + +// example specific +#include "{{ model.name }}_model/{{ model.name }}_model.h" +#include "acados_sim_solver_{{ model.name }}.h" + + +// ** solver data ** + +sim_solver_capsule * {{ model.name }}_acados_sim_solver_create_capsule() +{ + void* capsule_mem = malloc(sizeof(sim_solver_capsule)); + sim_solver_capsule *capsule = (sim_solver_capsule *) capsule_mem; + + return capsule; +} + + +int {{ model.name }}_acados_sim_solver_free_capsule(sim_solver_capsule * capsule) +{ + free(capsule); + return 0; +} + + +int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule) +{ + // initialize + const int nx = {{ model.name | upper }}_NX; + const int nu = {{ model.name | upper }}_NU; + const int nz = {{ model.name | upper }}_NZ; + const int np = {{ model.name | upper }}_NP; + bool tmp_bool; + + {#// double Tsim = {{ solver_options.tf / dims.N }};#} + double Tsim = {{ solver_options.Tsim }}; + + {% if solver_options.integrator_type == "IRK" %} + capsule->sim_impl_dae_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); + capsule->sim_impl_dae_fun_jac_x_xdot_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); + capsule->sim_impl_dae_jac_x_xdot_u_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); + + // external functions (implicit model) + capsule->sim_impl_dae_fun->casadi_fun = &{{ model.name }}_impl_dae_fun; + capsule->sim_impl_dae_fun->casadi_work = &{{ model.name }}_impl_dae_fun_work; + capsule->sim_impl_dae_fun->casadi_sparsity_in = &{{ model.name }}_impl_dae_fun_sparsity_in; + capsule->sim_impl_dae_fun->casadi_sparsity_out = &{{ model.name }}_impl_dae_fun_sparsity_out; + capsule->sim_impl_dae_fun->casadi_n_in = &{{ model.name }}_impl_dae_fun_n_in; + capsule->sim_impl_dae_fun->casadi_n_out = &{{ model.name }}_impl_dae_fun_n_out; + external_function_param_casadi_create(capsule->sim_impl_dae_fun, np); + + capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_fun = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z; + capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_work = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_work; + capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_sparsity_in = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_sparsity_in; + capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_sparsity_out = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_sparsity_out; + capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_n_in = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_in; + capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_n_out = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_out; + external_function_param_casadi_create(capsule->sim_impl_dae_fun_jac_x_xdot_z, np); + + // external_function_param_casadi impl_dae_jac_x_xdot_u_z; + capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_fun = &{{ model.name }}_impl_dae_jac_x_xdot_u_z; + capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_work = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_work; + capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_sparsity_in = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_sparsity_in; + capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_sparsity_out = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_sparsity_out; + capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_n_in = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_n_in; + capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_n_out = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_n_out; + external_function_param_casadi_create(capsule->sim_impl_dae_jac_x_xdot_u_z, np); + +{%- if hessian_approx == "EXACT" %} + capsule->sim_impl_dae_hess = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); + // external_function_param_casadi impl_dae_jac_x_xdot_u_z; + capsule->sim_impl_dae_hess->casadi_fun = &{{ model.name }}_impl_dae_hess; + capsule->sim_impl_dae_hess->casadi_work = &{{ model.name }}_impl_dae_hess_work; + capsule->sim_impl_dae_hess->casadi_sparsity_in = &{{ model.name }}_impl_dae_hess_sparsity_in; + capsule->sim_impl_dae_hess->casadi_sparsity_out = &{{ model.name }}_impl_dae_hess_sparsity_out; + capsule->sim_impl_dae_hess->casadi_n_in = &{{ model.name }}_impl_dae_hess_n_in; + capsule->sim_impl_dae_hess->casadi_n_out = &{{ model.name }}_impl_dae_hess_n_out; + external_function_param_casadi_create(capsule->sim_impl_dae_hess, np); +{%- endif %} + + {% elif solver_options.integrator_type == "ERK" %} + // explicit ode + capsule->sim_forw_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); + capsule->sim_expl_ode_fun_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); + + capsule->sim_forw_vde_casadi->casadi_fun = &{{ model.name }}_expl_vde_forw; + capsule->sim_forw_vde_casadi->casadi_n_in = &{{ model.name }}_expl_vde_forw_n_in; + capsule->sim_forw_vde_casadi->casadi_n_out = &{{ model.name }}_expl_vde_forw_n_out; + capsule->sim_forw_vde_casadi->casadi_sparsity_in = &{{ model.name }}_expl_vde_forw_sparsity_in; + capsule->sim_forw_vde_casadi->casadi_sparsity_out = &{{ model.name }}_expl_vde_forw_sparsity_out; + capsule->sim_forw_vde_casadi->casadi_work = &{{ model.name }}_expl_vde_forw_work; + external_function_param_casadi_create(capsule->sim_forw_vde_casadi, np); + + capsule->sim_expl_ode_fun_casadi->casadi_fun = &{{ model.name }}_expl_ode_fun; + capsule->sim_expl_ode_fun_casadi->casadi_n_in = &{{ model.name }}_expl_ode_fun_n_in; + capsule->sim_expl_ode_fun_casadi->casadi_n_out = &{{ model.name }}_expl_ode_fun_n_out; + capsule->sim_expl_ode_fun_casadi->casadi_sparsity_in = &{{ model.name }}_expl_ode_fun_sparsity_in; + capsule->sim_expl_ode_fun_casadi->casadi_sparsity_out = &{{ model.name }}_expl_ode_fun_sparsity_out; + capsule->sim_expl_ode_fun_casadi->casadi_work = &{{ model.name }}_expl_ode_fun_work; + external_function_param_casadi_create(capsule->sim_expl_ode_fun_casadi, np); + +{%- if hessian_approx == "EXACT" %} + capsule->sim_expl_ode_hess = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); + // external_function_param_casadi impl_dae_jac_x_xdot_u_z; + capsule->sim_expl_ode_hess->casadi_fun = &{{ model.name }}_expl_ode_hess; + capsule->sim_expl_ode_hess->casadi_work = &{{ model.name }}_expl_ode_hess_work; + capsule->sim_expl_ode_hess->casadi_sparsity_in = &{{ model.name }}_expl_ode_hess_sparsity_in; + capsule->sim_expl_ode_hess->casadi_sparsity_out = &{{ model.name }}_expl_ode_hess_sparsity_out; + capsule->sim_expl_ode_hess->casadi_n_in = &{{ model.name }}_expl_ode_hess_n_in; + capsule->sim_expl_ode_hess->casadi_n_out = &{{ model.name }}_expl_ode_hess_n_out; + external_function_param_casadi_create(capsule->sim_expl_ode_hess, np); +{%- endif %} + + {% elif solver_options.integrator_type == "GNSF" -%} + {% if model.gnsf.purely_linear != 1 %} + capsule->sim_gnsf_phi_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); + capsule->sim_gnsf_phi_fun_jac_y = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); + capsule->sim_gnsf_phi_jac_y_uhat = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); + {% if model.gnsf.nontrivial_f_LO == 1 %} + capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); + {%- endif %} + {%- endif %} + capsule->sim_gnsf_get_matrices_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); + + {% if model.gnsf.purely_linear != 1 %} + capsule->sim_gnsf_phi_fun->casadi_fun = &{{ model.name }}_gnsf_phi_fun; + capsule->sim_gnsf_phi_fun->casadi_n_in = &{{ model.name }}_gnsf_phi_fun_n_in; + capsule->sim_gnsf_phi_fun->casadi_n_out = &{{ model.name }}_gnsf_phi_fun_n_out; + capsule->sim_gnsf_phi_fun->casadi_sparsity_in = &{{ model.name }}_gnsf_phi_fun_sparsity_in; + capsule->sim_gnsf_phi_fun->casadi_sparsity_out = &{{ model.name }}_gnsf_phi_fun_sparsity_out; + capsule->sim_gnsf_phi_fun->casadi_work = &{{ model.name }}_gnsf_phi_fun_work; + external_function_param_casadi_create(capsule->sim_gnsf_phi_fun, np); + + capsule->sim_gnsf_phi_fun_jac_y->casadi_fun = &{{ model.name }}_gnsf_phi_fun_jac_y; + capsule->sim_gnsf_phi_fun_jac_y->casadi_n_in = &{{ model.name }}_gnsf_phi_fun_jac_y_n_in; + capsule->sim_gnsf_phi_fun_jac_y->casadi_n_out = &{{ model.name }}_gnsf_phi_fun_jac_y_n_out; + capsule->sim_gnsf_phi_fun_jac_y->casadi_sparsity_in = &{{ model.name }}_gnsf_phi_fun_jac_y_sparsity_in; + capsule->sim_gnsf_phi_fun_jac_y->casadi_sparsity_out = &{{ model.name }}_gnsf_phi_fun_jac_y_sparsity_out; + capsule->sim_gnsf_phi_fun_jac_y->casadi_work = &{{ model.name }}_gnsf_phi_fun_jac_y_work; + external_function_param_casadi_create(capsule->sim_gnsf_phi_fun_jac_y, np); + + capsule->sim_gnsf_phi_jac_y_uhat->casadi_fun = &{{ model.name }}_gnsf_phi_jac_y_uhat; + capsule->sim_gnsf_phi_jac_y_uhat->casadi_n_in = &{{ model.name }}_gnsf_phi_jac_y_uhat_n_in; + capsule->sim_gnsf_phi_jac_y_uhat->casadi_n_out = &{{ model.name }}_gnsf_phi_jac_y_uhat_n_out; + capsule->sim_gnsf_phi_jac_y_uhat->casadi_sparsity_in = &{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_in; + capsule->sim_gnsf_phi_jac_y_uhat->casadi_sparsity_out = &{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_out; + capsule->sim_gnsf_phi_jac_y_uhat->casadi_work = &{{ model.name }}_gnsf_phi_jac_y_uhat_work; + external_function_param_casadi_create(capsule->sim_gnsf_phi_jac_y_uhat, np); + + {% if model.gnsf.nontrivial_f_LO == 1 %} + capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_fun = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz; + capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_n_in = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_in; + capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_n_out = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_out; + capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_sparsity_in = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_in; + capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_sparsity_out = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_out; + capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_work = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_work; + external_function_param_casadi_create(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z, np); + {%- endif %} + {%- endif %} + + capsule->sim_gnsf_get_matrices_fun->casadi_fun = &{{ model.name }}_gnsf_get_matrices_fun; + capsule->sim_gnsf_get_matrices_fun->casadi_n_in = &{{ model.name }}_gnsf_get_matrices_fun_n_in; + capsule->sim_gnsf_get_matrices_fun->casadi_n_out = &{{ model.name }}_gnsf_get_matrices_fun_n_out; + capsule->sim_gnsf_get_matrices_fun->casadi_sparsity_in = &{{ model.name }}_gnsf_get_matrices_fun_sparsity_in; + capsule->sim_gnsf_get_matrices_fun->casadi_sparsity_out = &{{ model.name }}_gnsf_get_matrices_fun_sparsity_out; + capsule->sim_gnsf_get_matrices_fun->casadi_work = &{{ model.name }}_gnsf_get_matrices_fun_work; + external_function_param_casadi_create(capsule->sim_gnsf_get_matrices_fun, np); + {% endif %} + + // sim plan & config + sim_solver_plan_t plan; + plan.sim_solver = {{ solver_options.integrator_type }}; + + // create correct config based on plan + sim_config * {{ model.name }}_sim_config = sim_config_create(plan); + capsule->acados_sim_config = {{ model.name }}_sim_config; + + // sim dims + void *{{ model.name }}_sim_dims = sim_dims_create({{ model.name }}_sim_config); + capsule->acados_sim_dims = {{ model.name }}_sim_dims; + sim_dims_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, "nx", &nx); + sim_dims_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, "nu", &nu); + sim_dims_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, "nz", &nz); +{% if solver_options.integrator_type == "GNSF" %} + int gnsf_nx1 = {{ dims.gnsf_nx1 }}; + int gnsf_nz1 = {{ dims.gnsf_nz1 }}; + int gnsf_nout = {{ dims.gnsf_nout }}; + int gnsf_ny = {{ dims.gnsf_ny }}; + int gnsf_nuhat = {{ dims.gnsf_nuhat }}; + + sim_dims_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, "nx1", &gnsf_nx1); + sim_dims_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, "nz1", &gnsf_nz1); + sim_dims_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, "nout", &gnsf_nout); + sim_dims_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, "ny", &gnsf_ny); + sim_dims_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, "nuhat", &gnsf_nuhat); +{% endif %} + + // sim opts + sim_opts *{{ model.name }}_sim_opts = sim_opts_create({{ model.name }}_sim_config, {{ model.name }}_sim_dims); + capsule->acados_sim_opts = {{ model.name }}_sim_opts; + int tmp_int = {{ solver_options.sim_method_newton_iter }}; + sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "newton_iter", &tmp_int); + sim_collocation_type collocation_type = {{ solver_options.collocation_type }}; + sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "collocation_type", &collocation_type); + +{% if problem_class == "SIM" %} + tmp_int = {{ solver_options.sim_method_num_stages }}; + sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "num_stages", &tmp_int); + tmp_int = {{ solver_options.sim_method_num_steps }}; + sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "num_steps", &tmp_int); + + // options that are not available to AcadosOcpSolver + // (in OCP they will be determined by other options, like exact_hessian) + tmp_bool = {{ solver_options.sens_forw }}; + sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "sens_forw", &tmp_bool); + tmp_bool = {{ solver_options.sens_adj }}; + sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "sens_adj", &tmp_bool); + tmp_bool = {{ solver_options.sens_algebraic }}; + sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "sens_algebraic", &tmp_bool); + tmp_bool = {{ solver_options.sens_hess }}; + sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "sens_hess", &tmp_bool); + tmp_bool = {{ solver_options.output_z }}; + sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "output_z", &tmp_bool); + +{% else %} {# num_stages and num_steps of first shooting interval are used #} + tmp_int = {{ solver_options.sim_method_num_stages[0] }}; + sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "num_stages", &tmp_int); + tmp_int = {{ solver_options.sim_method_num_steps[0] }}; + sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "num_steps", &tmp_int); + tmp_bool = {{ solver_options.sim_method_jac_reuse[0] }}; + sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "jac_reuse", &tmp_bool); +{% endif %} + + // sim in / out + sim_in *{{ model.name }}_sim_in = sim_in_create({{ model.name }}_sim_config, {{ model.name }}_sim_dims); + capsule->acados_sim_in = {{ model.name }}_sim_in; + sim_out *{{ model.name }}_sim_out = sim_out_create({{ model.name }}_sim_config, {{ model.name }}_sim_dims); + capsule->acados_sim_out = {{ model.name }}_sim_out; + + sim_in_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, + {{ model.name }}_sim_in, "T", &Tsim); + + // model functions +{%- if solver_options.integrator_type == "IRK" %} + {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model, + "impl_ode_fun", capsule->sim_impl_dae_fun); + {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model, + "impl_ode_fun_jac_x_xdot", capsule->sim_impl_dae_fun_jac_x_xdot_z); + {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model, + "impl_ode_jac_x_xdot_u", capsule->sim_impl_dae_jac_x_xdot_u_z); +{%- if hessian_approx == "EXACT" %} + {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model, + "impl_dae_hess", capsule->sim_impl_dae_hess); +{%- endif %} + +{%- elif solver_options.integrator_type == "ERK" %} + {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model, + "expl_vde_for", capsule->sim_forw_vde_casadi); + {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model, + "expl_ode_fun", capsule->sim_expl_ode_fun_casadi); +{%- if hessian_approx == "EXACT" %} + {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model, + "expl_ode_hess", capsule->sim_expl_ode_hess); +{%- endif %} +{%- elif solver_options.integrator_type == "GNSF" %} + {% if model.gnsf.purely_linear != 1 %} + {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model, + "phi_fun", capsule->sim_gnsf_phi_fun); + {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model, + "phi_fun_jac_y", capsule->sim_gnsf_phi_fun_jac_y); + {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model, + "phi_jac_y_uhat", capsule->sim_gnsf_phi_jac_y_uhat); + {% if model.gnsf.nontrivial_f_LO == 1 %} + {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model, + "f_lo_jac_x1_x1dot_u_z", capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z); + {%- endif %} + {%- endif %} + {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model, + "gnsf_get_matrices_fun", capsule->sim_gnsf_get_matrices_fun); +{%- endif %} + + // sim solver + sim_solver *{{ model.name }}_sim_solver = sim_solver_create({{ model.name }}_sim_config, + {{ model.name }}_sim_dims, {{ model.name }}_sim_opts); + capsule->acados_sim_solver = {{ model.name }}_sim_solver; + +{% if dims.np > 0 %} + /* initialize parameter values */ + double* p = calloc(np, sizeof(double)); + {% for item in parameter_values %} + {%- if item != 0 %} + p[{{ loop.index0 }}] = {{ item }}; + {%- endif %} + {%- endfor %} + + {{ model.name }}_acados_sim_update_params(capsule, p, np); + free(p); +{% endif %}{# if dims.np #} + + /* initialize input */ + // x + double x0[{{ dims.nx }}]; + for (int ii = 0; ii < {{ dims.nx }}; ii++) + x0[ii] = 0.0; + + sim_in_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, + {{ model.name }}_sim_in, "x", x0); + + + // u + double u0[{{ dims.nu }}]; + for (int ii = 0; ii < {{ dims.nu }}; ii++) + u0[ii] = 0.0; + + sim_in_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, + {{ model.name }}_sim_in, "u", u0); + + // S_forw + double S_forw[{{ dims.nx * (dims.nx + dims.nu) }}]; + for (int ii = 0; ii < {{ dims.nx * (dims.nx + dims.nu) }}; ii++) + S_forw[ii] = 0.0; + for (int ii = 0; ii < {{ dims.nx }}; ii++) + S_forw[ii + ii * {{ dims.nx }} ] = 1.0; + + + sim_in_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, + {{ model.name }}_sim_in, "S_forw", S_forw); + + int status = sim_precompute({{ model.name }}_sim_solver, {{ model.name }}_sim_in, {{ model.name }}_sim_out); + + return status; +} + + +int {{ model.name }}_acados_sim_solve(sim_solver_capsule *capsule) +{ + // integrate dynamics using acados sim_solver + int status = sim_solve(capsule->acados_sim_solver, + capsule->acados_sim_in, capsule->acados_sim_out); + if (status != 0) + printf("error in {{ model.name }}_acados_sim_solve()! Exiting.\n"); + + return status; +} + + +int {{ model.name }}_acados_sim_free(sim_solver_capsule *capsule) +{ + // free memory + sim_solver_destroy(capsule->acados_sim_solver); + sim_in_destroy(capsule->acados_sim_in); + sim_out_destroy(capsule->acados_sim_out); + sim_opts_destroy(capsule->acados_sim_opts); + sim_dims_destroy(capsule->acados_sim_dims); + sim_config_destroy(capsule->acados_sim_config); + + // free external function +{%- if solver_options.integrator_type == "IRK" %} + external_function_param_casadi_free(capsule->sim_impl_dae_fun); + external_function_param_casadi_free(capsule->sim_impl_dae_fun_jac_x_xdot_z); + external_function_param_casadi_free(capsule->sim_impl_dae_jac_x_xdot_u_z); +{%- if hessian_approx == "EXACT" %} + external_function_param_casadi_free(capsule->sim_impl_dae_hess); +{%- endif %} +{%- elif solver_options.integrator_type == "ERK" %} + external_function_param_casadi_free(capsule->sim_forw_vde_casadi); + external_function_param_casadi_free(capsule->sim_expl_ode_fun_casadi); +{%- if hessian_approx == "EXACT" %} + external_function_param_casadi_free(capsule->sim_expl_ode_hess); +{%- endif %} +{%- elif solver_options.integrator_type == "GNSF" %} + {% if model.gnsf.purely_linear != 1 %} + external_function_param_casadi_free(capsule->sim_gnsf_phi_fun); + external_function_param_casadi_free(capsule->sim_gnsf_phi_fun_jac_y); + external_function_param_casadi_free(capsule->sim_gnsf_phi_jac_y_uhat); + {% if model.gnsf.nontrivial_f_LO == 1 %} + external_function_param_casadi_free(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z); + {%- endif %} + {%- endif %} + external_function_param_casadi_free(capsule->sim_gnsf_get_matrices_fun); +{% endif %} + + return 0; +} + + +int {{ model.name }}_acados_sim_update_params(sim_solver_capsule *capsule, double *p, int np) +{ + int status = 0; + int casadi_np = {{ model.name | upper }}_NP; + + if (casadi_np != np) { + printf("{{ model.name }}_acados_sim_update_params: trying to set %i parameters for external functions." + " External function has %i parameters. Exiting.\n", np, casadi_np); + exit(1); + } + +{%- if solver_options.integrator_type == "ERK" %} + capsule->sim_forw_vde_casadi[0].set_param(capsule->sim_forw_vde_casadi, p); + capsule->sim_expl_ode_fun_casadi[0].set_param(capsule->sim_expl_ode_fun_casadi, p); +{%- if hessian_approx == "EXACT" %} + capsule->sim_expl_ode_hess[0].set_param(capsule->sim_expl_ode_hess, p); +{%- endif %} +{%- elif solver_options.integrator_type == "IRK" %} + capsule->sim_impl_dae_fun[0].set_param(capsule->sim_impl_dae_fun, p); + capsule->sim_impl_dae_fun_jac_x_xdot_z[0].set_param(capsule->sim_impl_dae_fun_jac_x_xdot_z, p); + capsule->sim_impl_dae_jac_x_xdot_u_z[0].set_param(capsule->sim_impl_dae_jac_x_xdot_u_z, p); +{%- if hessian_approx == "EXACT" %} + capsule->sim_impl_dae_hess[0].set_param(capsule->sim_impl_dae_hess, p); +{%- endif %} +{%- elif solver_options.integrator_type == "GNSF" %} + {% if model.gnsf.purely_linear != 1 %} + capsule->sim_gnsf_phi_fun[0].set_param(capsule->sim_gnsf_phi_fun, p); + capsule->sim_gnsf_phi_fun_jac_y[0].set_param(capsule->sim_gnsf_phi_fun_jac_y, p); + capsule->sim_gnsf_phi_jac_y_uhat[0].set_param(capsule->sim_gnsf_phi_jac_y_uhat, p); + {% if model.gnsf.nontrivial_f_LO == 1 %} + capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z[0].set_param(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z, p); + {%- endif %} + {%- endif %} + capsule->sim_gnsf_get_matrices_fun[0].set_param(capsule->sim_gnsf_get_matrices_fun, p); +{% endif %} + + return status; +} + +/* getters pointers to C objects*/ +sim_config * {{ model.name }}_acados_get_sim_config(sim_solver_capsule *capsule) +{ + return capsule->acados_sim_config; +}; + +sim_in * {{ model.name }}_acados_get_sim_in(sim_solver_capsule *capsule) +{ + return capsule->acados_sim_in; +}; + +sim_out * {{ model.name }}_acados_get_sim_out(sim_solver_capsule *capsule) +{ + return capsule->acados_sim_out; +}; + +void * {{ model.name }}_acados_get_sim_dims(sim_solver_capsule *capsule) +{ + return capsule->acados_sim_dims; +}; + +sim_opts * {{ model.name }}_acados_get_sim_opts(sim_solver_capsule *capsule) +{ + return capsule->acados_sim_opts; +}; + +sim_solver * {{ model.name }}_acados_get_sim_solver(sim_solver_capsule *capsule) +{ + return capsule->acados_sim_solver; +}; + diff --git a/third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.h b/third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.h new file mode 100644 index 0000000000..7306491baf --- /dev/null +++ b/third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.h @@ -0,0 +1,103 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + +#ifndef ACADOS_SIM_{{ model.name }}_H_ +#define ACADOS_SIM_{{ model.name }}_H_ + +#include "acados_c/sim_interface.h" +#include "acados_c/external_function_interface.h" + +#define {{ model.name | upper }}_NX {{ dims.nx }} +#define {{ model.name | upper }}_NZ {{ dims.nz }} +#define {{ model.name | upper }}_NU {{ dims.nu }} +#define {{ model.name | upper }}_NP {{ dims.np }} + +#ifdef __cplusplus +extern "C" { +#endif + + +// ** capsule for solver data ** +typedef struct sim_solver_capsule +{ + // acados objects + sim_in *acados_sim_in; + sim_out *acados_sim_out; + sim_solver *acados_sim_solver; + sim_opts *acados_sim_opts; + sim_config *acados_sim_config; + void *acados_sim_dims; + + /* external functions */ + // ERK + external_function_param_casadi * sim_forw_vde_casadi; + external_function_param_casadi * sim_expl_ode_fun_casadi; + external_function_param_casadi * sim_expl_ode_hess; + + // IRK + external_function_param_casadi * sim_impl_dae_fun; + external_function_param_casadi * sim_impl_dae_fun_jac_x_xdot_z; + external_function_param_casadi * sim_impl_dae_jac_x_xdot_u_z; + external_function_param_casadi * sim_impl_dae_hess; + + // GNSF + external_function_param_casadi * sim_gnsf_phi_fun; + external_function_param_casadi * sim_gnsf_phi_fun_jac_y; + external_function_param_casadi * sim_gnsf_phi_jac_y_uhat; + external_function_param_casadi * sim_gnsf_f_lo_jac_x1_x1dot_u_z; + external_function_param_casadi * sim_gnsf_get_matrices_fun; + +} sim_solver_capsule; + + +ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_sim_create(sim_solver_capsule *capsule); +ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_sim_solve(sim_solver_capsule *capsule); +ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_sim_free(sim_solver_capsule *capsule); +ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_sim_update_params(sim_solver_capsule *capsule, double *value, int np); + +ACADOS_SYMBOL_EXPORT sim_config * {{ model.name }}_acados_get_sim_config(sim_solver_capsule *capsule); +ACADOS_SYMBOL_EXPORT sim_in * {{ model.name }}_acados_get_sim_in(sim_solver_capsule *capsule); +ACADOS_SYMBOL_EXPORT sim_out * {{ model.name }}_acados_get_sim_out(sim_solver_capsule *capsule); +ACADOS_SYMBOL_EXPORT void * {{ model.name }}_acados_get_sim_dims(sim_solver_capsule *capsule); +ACADOS_SYMBOL_EXPORT sim_opts * {{ model.name }}_acados_get_sim_opts(sim_solver_capsule *capsule); +ACADOS_SYMBOL_EXPORT sim_solver * {{ model.name }}_acados_get_sim_solver(sim_solver_capsule *capsule); + + +ACADOS_SYMBOL_EXPORT sim_solver_capsule * {{ model.name }}_acados_sim_solver_create_capsule(void); +ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_sim_solver_free_capsule(sim_solver_capsule *capsule); + +#ifdef __cplusplus +} +#endif + +#endif // ACADOS_SIM_{{ model.name }}_H_ diff --git a/third_party/acados/acados_template/c_templates_tera/acados_sim_solver_sfun.in.c b/third_party/acados/acados_template/c_templates_tera/acados_sim_solver_sfun.in.c new file mode 100644 index 0000000000..68a6a3f80f --- /dev/null +++ b/third_party/acados/acados_template/c_templates_tera/acados_sim_solver_sfun.in.c @@ -0,0 +1,233 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + +#define S_FUNCTION_NAME acados_sim_solver_sfunction_{{ model.name }} +#define S_FUNCTION_LEVEL 2 + +#define MDL_START + +// acados +// #include "acados/utils/print.h" +#include "acados_c/ocp_nlp_interface.h" +#include "acados_c/external_function_interface.h" + +// example specific +#include "{{ model.name }}_model/{{ model.name }}_model.h" +#include "acados_sim_solver_{{ model.name }}.h" + +#include "simstruc.h" + +#define SAMPLINGTIME {{ solver_options.Tsim }} + + +static void mdlInitializeSizes (SimStruct *S) +{ + // specify the number of continuous and discrete states + ssSetNumContStates(S, 0); + ssSetNumDiscStates(S, 0); + + {# compute number of input ports #} + {%- set n_inputs = 1 %} {# x0 #} + {%- if dims.nu > 0 %} {# u0 -#} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif %} + {%- if dims.np > 0 %} {# parameters #} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif %} + + // specify the number of input ports + if ( !ssSetNumInputPorts(S, {{ n_inputs }}) ) + return; + + // specify the number of output ports + if ( !ssSetNumOutputPorts(S, 1) ) + return; + + // specify dimension information for the input ports + {%- set i_input = 0 %} + // x0 + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nx }}); + + {%- if dims.nu > 0 %} + {%- set i_input = i_input + 1 %} + // u0 + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nu }}); + {%- endif %} + + {%- if dims.np > 0 %} + {%- set i_input = i_input + 1 %} + // parameters + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.np }}); + {%- endif %} + + // specify dimension information for the output ports + ssSetOutputPortVectorDimension(S, 0, {{ dims.nx }} ); // xnext + + // specify the direct feedthrough status + // should be set to 1 for all inputs used in mdlOutputs + {%- for i in range(end=n_inputs) %} + ssSetInputPortDirectFeedThrough(S, {{ i }}, 1); + {%- endfor %} + + // one sample time + ssSetNumSampleTimes(S, 1); +} + + +#if defined(MATLAB_MEX_FILE) + +#define MDL_SET_INPUT_PORT_DIMENSION_INFO +#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO + +static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo) +{ + if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) ) + return; +} + +static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo) +{ + if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) ) + return; +} + +#endif /* MATLAB_MEX_FILE */ + + +static void mdlInitializeSampleTimes(SimStruct *S) +{ + ssSetSampleTime(S, 0, SAMPLINGTIME); + ssSetOffsetTime(S, 0, 0.0); +} + + +static void mdlStart(SimStruct *S) +{ + sim_solver_capsule *capsule = {{ model.name }}_acados_sim_solver_create_capsule(); + {{ model.name }}_acados_sim_create(capsule); + + ssSetUserData(S, (void*)capsule); +} + +static void mdlOutputs(SimStruct *S, int_T tid) +{ + sim_solver_capsule *capsule = ssGetUserData(S); + + sim_config *acados_sim_config = {{ model.name }}_acados_get_sim_config(capsule); + sim_in *acados_sim_in = {{ model.name }}_acados_get_sim_in(capsule); + sim_out *acados_sim_out = {{ model.name }}_acados_get_sim_out(capsule); + void *acados_sim_dims = {{ model.name }}_acados_get_sim_dims(capsule); + // sim_opts * {{ model.name }}_acados_get_sim_opts(capsule); + // sim_solver * {{ model.name }}_acados_get_sim_solver(capsule); + + InputRealPtrsType in_sign; + {% set input_sizes = [dims.nx, dims.nu, dims.np] %} + + // local buffer + {%- set buffer_size = input_sizes | sort | last %} + real_t buffer[{{ buffer_size }}]; + + + /* go through inputs */ + {%- set i_input = 0 %} + // initial condition + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + for (int i = 0; i < {{ dims.nx }}; i++) + buffer[i] = (double)(*in_sign[i]); + + sim_in_set(acados_sim_config, acados_sim_dims, + acados_sim_in, "x", buffer); + + + // ssPrintf("\nin acados sim:\n"); + // for (int i = 0; i < {{ dims.nx }}; i++) ssPrintf("x0[%d] = %f\n", i, buffer[i]); + // ssPrintf("\n"); + +{% if dims.nu > 0 %} + // control input - u + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + + for (int i = 0; i < {{ dims.nu }}; i++) + buffer[i] = (double)(*in_sign[i]); + + sim_in_set(acados_sim_config, acados_sim_dims, + acados_sim_in, "u", buffer); +{%- endif %} + + +{% if dims.np > 0 %} + // parameters + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + + for (int i = 0; i < {{ dims.np }}; i++) + buffer[i] = (double)(*in_sign[i]); + + // update value of parameters + {{ model.name }}_acados_sim_update_params(capsule, buffer, {{ dims.np }}); +{%- endif %} + + + /* call solver */ + int acados_status = {{ model.name }}_acados_sim_solve(capsule); + + + /* set outputs */ + real_t *out_x = ssGetOutputPortRealSignal(S, 0); + + // get simulated state + sim_out_get(acados_sim_config, acados_sim_dims, acados_sim_out, + "xn", (void *) out_x); + + // ssPrintf("\nacados sim solve: returned %d\n", acados_status); + // for (int i = 0; i < {{ dims.nx }}; i++) ssPrintf("x_sim[%d] = %f\n", i, out_x[i]); + // ssPrintf("\n"); + +} + + +static void mdlTerminate(SimStruct *S) +{ + sim_solver_capsule *capsule = ssGetUserData(S); + + {{ model.name }}_acados_sim_free(capsule); + {{ model.name }}_acados_sim_solver_free_capsule(capsule); +} + + +#ifdef MATLAB_MEX_FILE +#include "simulink.c" +#else +#include "cg_sfun.h" +#endif diff --git a/third_party/acados/acados_template/c_templates_tera/acados_solver.in.c b/third_party/acados/acados_template/c_templates_tera/acados_solver.in.c new file mode 100644 index 0000000000..0af8127709 --- /dev/null +++ b/third_party/acados/acados_template/c_templates_tera/acados_solver.in.c @@ -0,0 +1,2495 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + +// standard +#include +#include +#include +// acados +// #include "acados/utils/print.h" +#include "acados_c/ocp_nlp_interface.h" +#include "acados_c/external_function_interface.h" + +// example specific +#include "{{ model.name }}_model/{{ model.name }}_model.h" +{% if constraints.constr_type == "BGP" and dims.nphi %} +#include "{{ model.name }}_constraints/{{ model.name }}_phi_constraint.h" +{% endif %} +{% if constraints.constr_type_e == "BGP" and dims.nphi_e > 0 %} +#include "{{ model.name }}_constraints/{{ model.name }}_phi_e_constraint.h" +{% endif %} +{% if constraints.constr_type == "BGH" and dims.nh > 0 %} +#include "{{ model.name }}_constraints/{{ model.name }}_h_constraint.h" +{% endif %} +{% if constraints.constr_type_e == "BGH" and dims.nh_e > 0 %} +#include "{{ model.name }}_constraints/{{ model.name }}_h_e_constraint.h" +{% endif %} +{%- if cost.cost_type == "NONLINEAR_LS" %} +#include "{{ model.name }}_cost/{{ model.name }}_cost_y_fun.h" +{%- elif cost.cost_type == "EXTERNAL" %} +#include "{{ model.name }}_cost/{{ model.name }}_external_cost.h" +{%- endif %} +{%- if cost.cost_type_0 == "NONLINEAR_LS" %} +#include "{{ model.name }}_cost/{{ model.name }}_cost_y_0_fun.h" +{%- elif cost.cost_type_0 == "EXTERNAL" %} +#include "{{ model.name }}_cost/{{ model.name }}_external_cost_0.h" +{%- endif %} +{%- if cost.cost_type_e == "NONLINEAR_LS" %} +#include "{{ model.name }}_cost/{{ model.name }}_cost_y_e_fun.h" +{%- elif cost.cost_type_e == "EXTERNAL" %} +#include "{{ model.name }}_cost/{{ model.name }}_external_cost_e.h" +{%- endif %} + +#include "acados_solver_{{ model.name }}.h" + +#define NX {{ model.name | upper }}_NX +#define NZ {{ model.name | upper }}_NZ +#define NU {{ model.name | upper }}_NU +#define NP {{ model.name | upper }}_NP +#define NBX {{ model.name | upper }}_NBX +#define NBX0 {{ model.name | upper }}_NBX0 +#define NBU {{ model.name | upper }}_NBU +#define NSBX {{ model.name | upper }}_NSBX +#define NSBU {{ model.name | upper }}_NSBU +#define NSH {{ model.name | upper }}_NSH +#define NSG {{ model.name | upper }}_NSG +#define NSPHI {{ model.name | upper }}_NSPHI +#define NSHN {{ model.name | upper }}_NSHN +#define NSGN {{ model.name | upper }}_NSGN +#define NSPHIN {{ model.name | upper }}_NSPHIN +#define NSBXN {{ model.name | upper }}_NSBXN +#define NS {{ model.name | upper }}_NS +#define NSN {{ model.name | upper }}_NSN +#define NG {{ model.name | upper }}_NG +#define NBXN {{ model.name | upper }}_NBXN +#define NGN {{ model.name | upper }}_NGN +#define NY0 {{ model.name | upper }}_NY0 +#define NY {{ model.name | upper }}_NY +#define NYN {{ model.name | upper }}_NYN +// #define N {{ model.name | upper }}_N +#define NH {{ model.name | upper }}_NH +#define NPHI {{ model.name | upper }}_NPHI +#define NHN {{ model.name | upper }}_NHN +#define NPHIN {{ model.name | upper }}_NPHIN +#define NR {{ model.name | upper }}_NR + + +// ** solver data ** + +{{ model.name }}_solver_capsule * {{ model.name }}_acados_create_capsule(void) +{ + void* capsule_mem = malloc(sizeof({{ model.name }}_solver_capsule)); + {{ model.name }}_solver_capsule *capsule = ({{ model.name }}_solver_capsule *) capsule_mem; + + return capsule; +} + + +int {{ model.name }}_acados_free_capsule({{ model.name }}_solver_capsule *capsule) +{ + free(capsule); + return 0; +} + + +int {{ model.name }}_acados_create({{ model.name }}_solver_capsule* capsule) +{ + int N_shooting_intervals = {{ model.name | upper }}_N; + double* new_time_steps = NULL; // NULL -> don't alter the code generated time-steps + return {{ model.name }}_acados_create_with_discretization(capsule, N_shooting_intervals, new_time_steps); +} + + +int {{ model.name }}_acados_update_time_steps({{ model.name }}_solver_capsule* capsule, int N, double* new_time_steps) +{ + if (N != capsule->nlp_solver_plan->N) { + fprintf(stderr, "{{ model.name }}_acados_update_time_steps: given number of time steps (= %d) " \ + "differs from the currently allocated number of " \ + "time steps (= %d)!\n" \ + "Please recreate with new discretization and provide a new vector of time_stamps!\n", + N, capsule->nlp_solver_plan->N); + return 1; + } + + ocp_nlp_config * nlp_config = capsule->nlp_config; + ocp_nlp_dims * nlp_dims = capsule->nlp_dims; + ocp_nlp_in * nlp_in = capsule->nlp_in; + + for (int i = 0; i < N; i++) + { + ocp_nlp_in_set(nlp_config, nlp_dims, nlp_in, i, "Ts", &new_time_steps[i]); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "scaling", &new_time_steps[i]); + } + return 0; +} + +/** + * Internal function for {{ model.name }}_acados_create: step 1 + */ +void {{ model.name }}_acados_create_1_set_plan(ocp_nlp_plan_t* nlp_solver_plan, const int N) +{ + assert(N == nlp_solver_plan->N); + + /************************************************ + * plan + ************************************************/ + + {%- if solver_options.nlp_solver_type == "SQP" %} + nlp_solver_plan->nlp_solver = SQP; + {%- else %} + nlp_solver_plan->nlp_solver = SQP_RTI; + {%- endif %} + + nlp_solver_plan->ocp_qp_solver_plan.qp_solver = {{ solver_options.qp_solver }}; + + nlp_solver_plan->nlp_cost[0] = {{ cost.cost_type_0 }}; + for (int i = 1; i < N; i++) + nlp_solver_plan->nlp_cost[i] = {{ cost.cost_type }}; + + nlp_solver_plan->nlp_cost[N] = {{ cost.cost_type_e }}; + + for (int i = 0; i < N; i++) + { + {%- if solver_options.integrator_type == "DISCRETE" %} + nlp_solver_plan->nlp_dynamics[i] = DISCRETE_MODEL; + // discrete dynamics does not need sim solver option, this field is ignored + nlp_solver_plan->sim_solver_plan[i].sim_solver = INVALID_SIM_SOLVER; + {%- else %} + nlp_solver_plan->nlp_dynamics[i] = CONTINUOUS_MODEL; + nlp_solver_plan->sim_solver_plan[i].sim_solver = {{ solver_options.integrator_type }}; + {%- endif %} + } + + for (int i = 0; i < N; i++) + { + {%- if constraints.constr_type == "BGP" %} + nlp_solver_plan->nlp_constraints[i] = BGP; + {%- else -%} + nlp_solver_plan->nlp_constraints[i] = BGH; + {%- endif %} + } + + {%- if constraints.constr_type_e == "BGP" %} + nlp_solver_plan->nlp_constraints[N] = BGP; + {%- else %} + nlp_solver_plan->nlp_constraints[N] = BGH; + {%- endif %} + +{%- if solver_options.hessian_approx == "EXACT" %} + {%- if solver_options.regularize_method == "NO_REGULARIZE" %} + nlp_solver_plan->regularization = NO_REGULARIZE; + {%- elif solver_options.regularize_method == "MIRROR" %} + nlp_solver_plan->regularization = MIRROR; + {%- elif solver_options.regularize_method == "PROJECT" %} + nlp_solver_plan->regularization = PROJECT; + {%- elif solver_options.regularize_method == "PROJECT_REDUC_HESS" %} + nlp_solver_plan->regularization = PROJECT_REDUC_HESS; + {%- elif solver_options.regularize_method == "CONVEXIFY" %} + nlp_solver_plan->regularization = CONVEXIFY; + {%- endif %} +{%- endif %} +} + + +/** + * Internal function for {{ model.name }}_acados_create: step 2 + */ +ocp_nlp_dims* {{ model.name }}_acados_create_2_create_and_set_dimensions({{ model.name }}_solver_capsule* capsule) +{ + ocp_nlp_plan_t* nlp_solver_plan = capsule->nlp_solver_plan; + const int N = nlp_solver_plan->N; + ocp_nlp_config* nlp_config = capsule->nlp_config; + + /************************************************ + * dimensions + ************************************************/ + #define NINTNP1MEMS 17 + int* intNp1mem = (int*)malloc( (N+1)*sizeof(int)*NINTNP1MEMS ); + + int* nx = intNp1mem + (N+1)*0; + int* nu = intNp1mem + (N+1)*1; + int* nbx = intNp1mem + (N+1)*2; + int* nbu = intNp1mem + (N+1)*3; + int* nsbx = intNp1mem + (N+1)*4; + int* nsbu = intNp1mem + (N+1)*5; + int* nsg = intNp1mem + (N+1)*6; + int* nsh = intNp1mem + (N+1)*7; + int* nsphi = intNp1mem + (N+1)*8; + int* ns = intNp1mem + (N+1)*9; + int* ng = intNp1mem + (N+1)*10; + int* nh = intNp1mem + (N+1)*11; + int* nphi = intNp1mem + (N+1)*12; + int* nz = intNp1mem + (N+1)*13; + int* ny = intNp1mem + (N+1)*14; + int* nr = intNp1mem + (N+1)*15; + int* nbxe = intNp1mem + (N+1)*16; + + for (int i = 0; i < N+1; i++) + { + // common + nx[i] = NX; + nu[i] = NU; + nz[i] = NZ; + ns[i] = NS; + // cost + ny[i] = NY; + // constraints + nbx[i] = NBX; + nbu[i] = NBU; + nsbx[i] = NSBX; + nsbu[i] = NSBU; + nsg[i] = NSG; + nsh[i] = NSH; + nsphi[i] = NSPHI; + ng[i] = NG; + nh[i] = NH; + nphi[i] = NPHI; + nr[i] = NR; + nbxe[i] = 0; + } + + // for initial state + nbx[0] = NBX0; + nsbx[0] = 0; + ns[0] = NS - NSBX; + nbxe[0] = {{ dims.nbxe_0 }}; + ny[0] = NY0; + + // terminal - common + nu[N] = 0; + nz[N] = 0; + ns[N] = NSN; + // cost + ny[N] = NYN; + // constraint + nbx[N] = NBXN; + nbu[N] = 0; + ng[N] = NGN; + nh[N] = NHN; + nphi[N] = NPHIN; + nr[N] = {{ dims.nr_e }}; + + nsbx[N] = NSBXN; + nsbu[N] = 0; + nsg[N] = NSGN; + nsh[N] = NSHN; + nsphi[N] = NSPHIN; + + /* create and set ocp_nlp_dims */ + ocp_nlp_dims * nlp_dims = ocp_nlp_dims_create(nlp_config); + + ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "nx", nx); + ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "nu", nu); + ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "nz", nz); + ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "ns", ns); + + for (int i = 0; i <= N; i++) + { + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nbx", &nbx[i]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nbu", &nbu[i]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsbx", &nsbx[i]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsbu", &nsbu[i]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "ng", &ng[i]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsg", &nsg[i]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nbxe", &nbxe[i]); + } + +{%- if cost.cost_type_0 == "NONLINEAR_LS" or cost.cost_type_0 == "LINEAR_LS" %} + ocp_nlp_dims_set_cost(nlp_config, nlp_dims, 0, "ny", &ny[0]); +{%- endif %} + +{%- if cost.cost_type == "NONLINEAR_LS" or cost.cost_type == "LINEAR_LS" %} + for (int i = 1; i < N; i++) + ocp_nlp_dims_set_cost(nlp_config, nlp_dims, i, "ny", &ny[i]); +{%- endif %} + + for (int i = 0; i < N; i++) + { + {%- if constraints.constr_type == "BGH" and dims.nh > 0 %} + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nh", &nh[i]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsh", &nsh[i]); + {%- elif constraints.constr_type == "BGP" and dims.nphi > 0 %} + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nr", &nr[i]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nphi", &nphi[i]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsphi", &nsphi[i]); + {%- endif %} + } + +{%- if constraints.constr_type_e == "BGH" %} + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nh", &nh[N]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nsh", &nsh[N]); +{%- elif constraints.constr_type_e == "BGP" %} + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nr", &nr[N]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nphi", &nphi[N]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nsphi", &nsphi[N]); +{%- endif %} +{%- if cost.cost_type_e == "NONLINEAR_LS" or cost.cost_type_e == "LINEAR_LS" %} + ocp_nlp_dims_set_cost(nlp_config, nlp_dims, N, "ny", &ny[N]); +{%- endif %} + free(intNp1mem); + +{%- if solver_options.integrator_type == "GNSF" -%} + // GNSF specific dimensions + int gnsf_nx1 = {{ dims.gnsf_nx1 }}; + int gnsf_nz1 = {{ dims.gnsf_nz1 }}; + int gnsf_nout = {{ dims.gnsf_nout }}; + int gnsf_ny = {{ dims.gnsf_ny }}; + int gnsf_nuhat = {{ dims.gnsf_nuhat }}; + + for (int i = 0; i < N; i++) + { + if (nlp_solver_plan->sim_solver_plan[i].sim_solver == GNSF) + { + ocp_nlp_dims_set_dynamics(nlp_config, nlp_dims, i, "gnsf_nx1", &gnsf_nx1); + ocp_nlp_dims_set_dynamics(nlp_config, nlp_dims, i, "gnsf_nz1", &gnsf_nz1); + ocp_nlp_dims_set_dynamics(nlp_config, nlp_dims, i, "gnsf_nout", &gnsf_nout); + ocp_nlp_dims_set_dynamics(nlp_config, nlp_dims, i, "gnsf_ny", &gnsf_ny); + ocp_nlp_dims_set_dynamics(nlp_config, nlp_dims, i, "gnsf_nuhat", &gnsf_nuhat); + } + } +{%- endif %} +return nlp_dims; +} + + +/** + * Internal function for {{ model.name }}_acados_create: step 3 + */ +void {{ model.name }}_acados_create_3_create_and_set_functions({{ model.name }}_solver_capsule* capsule) +{ + const int N = capsule->nlp_solver_plan->N; + ocp_nlp_config* nlp_config = capsule->nlp_config; + + /************************************************ + * external functions + ************************************************/ + +#define MAP_CASADI_FNC(__CAPSULE_FNC__, __MODEL_BASE_FNC__) do{ \ + capsule->__CAPSULE_FNC__.casadi_fun = & __MODEL_BASE_FNC__ ;\ + capsule->__CAPSULE_FNC__.casadi_n_in = & __MODEL_BASE_FNC__ ## _n_in; \ + capsule->__CAPSULE_FNC__.casadi_n_out = & __MODEL_BASE_FNC__ ## _n_out; \ + capsule->__CAPSULE_FNC__.casadi_sparsity_in = & __MODEL_BASE_FNC__ ## _sparsity_in; \ + capsule->__CAPSULE_FNC__.casadi_sparsity_out = & __MODEL_BASE_FNC__ ## _sparsity_out; \ + capsule->__CAPSULE_FNC__.casadi_work = & __MODEL_BASE_FNC__ ## _work; \ + external_function_param_casadi_create(&capsule->__CAPSULE_FNC__ , {{ dims.np }}); \ + }while(false) + +{% if constraints.constr_type == "BGP" %} + // constraints.constr_type == "BGP" + capsule->phi_constraint = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N; i++) + { + // nonlinear part of convex-composite constraint + MAP_CASADI_FNC(phi_constraint[i], {{ model.name }}_phi_constraint); + } +{%- endif %} + +{%- if constraints.constr_type_e == "BGP" %} + // constraints.constr_type_e == "BGP" + // nonlinear part of convex-composite constraint + MAP_CASADI_FNC(phi_e_constraint, {{ model.name }}_phi_e_constraint); +{%- endif %} + +{%- if constraints.constr_type == "BGH" and dims.nh > 0 %} + // constraints.constr_type == "BGH" and dims.nh > 0 + capsule->nl_constr_h_fun_jac = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N; i++) { + MAP_CASADI_FNC(nl_constr_h_fun_jac[i], {{ model.name }}_constr_h_fun_jac_uxt_zt); + } + capsule->nl_constr_h_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N; i++) { + MAP_CASADI_FNC(nl_constr_h_fun[i], {{ model.name }}_constr_h_fun); + } + {% if solver_options.hessian_approx == "EXACT" %} + capsule->nl_constr_h_fun_jac_hess = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N; i++) { + MAP_CASADI_FNC(nl_constr_h_fun_jac_hess[i], {{ model.name }}_constr_h_fun_jac_uxt_zt_hess); + } + {% endif %} +{% endif %} + +{%- if constraints.constr_type_e == "BGH" and dims.nh_e > 0 %} + MAP_CASADI_FNC(nl_constr_h_e_fun_jac, {{ model.name }}_constr_h_e_fun_jac_uxt_zt); + MAP_CASADI_FNC(nl_constr_h_e_fun, {{ model.name }}_constr_h_e_fun); + + {%- if solver_options.hessian_approx == "EXACT" %} + MAP_CASADI_FNC(nl_constr_h_e_fun_jac_hess, {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess); + {% endif %} +{%- endif %} + +{% if solver_options.integrator_type == "ERK" %} + // explicit ode + capsule->forw_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N; i++) { + MAP_CASADI_FNC(forw_vde_casadi[i], {{ model.name }}_expl_vde_forw); + } + + capsule->expl_ode_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N; i++) { + MAP_CASADI_FNC(expl_ode_fun[i], {{ model.name }}_expl_ode_fun); + } + + {%- if solver_options.hessian_approx == "EXACT" %} + capsule->hess_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N; i++) { + MAP_CASADI_FNC(hess_vde_casadi[i], {{ model.name }}_expl_ode_hess); + } + {%- endif %} + +{% elif solver_options.integrator_type == "IRK" %} + // implicit dae + capsule->impl_dae_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N; i++) { + MAP_CASADI_FNC(impl_dae_fun[i], {{ model.name }}_impl_dae_fun); + } + + capsule->impl_dae_fun_jac_x_xdot_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N; i++) { + MAP_CASADI_FNC(impl_dae_fun_jac_x_xdot_z[i], {{ model.name }}_impl_dae_fun_jac_x_xdot_z); + } + + capsule->impl_dae_jac_x_xdot_u_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N; i++) { + MAP_CASADI_FNC(impl_dae_jac_x_xdot_u_z[i], {{ model.name }}_impl_dae_jac_x_xdot_u_z); + } + + {%- if solver_options.hessian_approx == "EXACT" %} + capsule->impl_dae_hess = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N; i++) { + MAP_CASADI_FNC(impl_dae_hess[i], {{ model.name }}_impl_dae_hess); + } + {%- endif %} +{% elif solver_options.integrator_type == "LIFTED_IRK" %} + // external functions (implicit model) + capsule->impl_dae_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N; i++) { + MAP_CASADI_FNC(impl_dae_fun[i], {{ model.name }}_impl_dae_fun); + } + + capsule->impl_dae_fun_jac_x_xdot_u = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N; i++) { + MAP_CASADI_FNC(impl_dae_fun_jac_x_xdot_u[i], {{ model.name }}_impl_dae_fun_jac_x_xdot_u); + } + +{% elif solver_options.integrator_type == "GNSF" %} + {% if model.gnsf.purely_linear != 1 %} + capsule->gnsf_phi_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N; i++) { + MAP_CASADI_FNC(gnsf_phi_fun[i], {{ model.name }}_gnsf_phi_fun); + } + + capsule->gnsf_phi_fun_jac_y = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N; i++) { + MAP_CASADI_FNC(gnsf_phi_fun_jac_y[i], {{ model.name }}_gnsf_phi_fun_jac_y); + } + + capsule->gnsf_phi_jac_y_uhat = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N; i++) { + MAP_CASADI_FNC(gnsf_phi_jac_y_uhat[i], {{ model.name }}_gnsf_phi_jac_y_uhat); + } + + {% if model.gnsf.nontrivial_f_LO == 1 %} + capsule->gnsf_f_lo_jac_x1_x1dot_u_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N; i++) { + MAP_CASADI_FNC(gnsf_f_lo_jac_x1_x1dot_u_z[i], {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz); + } + {%- endif %} + {%- endif %} + capsule->gnsf_get_matrices_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N; i++) { + MAP_CASADI_FNC(gnsf_get_matrices_fun[i], {{ model.name }}_gnsf_get_matrices_fun); + } +{% elif solver_options.integrator_type == "DISCRETE" %} + // discrete dynamics + capsule->discr_dyn_phi_fun = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }})*N); + for (int i = 0; i < N; i++) + { + {%- if model.dyn_ext_fun_type == "casadi" %} + MAP_CASADI_FNC(discr_dyn_phi_fun[i], {{ model.name }}_dyn_disc_phi_fun); + {%- else %} + capsule->discr_dyn_phi_fun[i].fun = &{{ model.dyn_disc_fun }}; + external_function_param_{{ model.dyn_ext_fun_type }}_create(&capsule->discr_dyn_phi_fun[i], {{ dims.np }}); + {%- endif %} + } + + capsule->discr_dyn_phi_fun_jac_ut_xt = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }})*N); + for (int i = 0; i < N; i++) + { + {%- if model.dyn_ext_fun_type == "casadi" %} + MAP_CASADI_FNC(discr_dyn_phi_fun_jac_ut_xt[i], {{ model.name }}_dyn_disc_phi_fun_jac); + {%- else %} + capsule->discr_dyn_phi_fun_jac_ut_xt[i].fun = &{{ model.dyn_disc_fun_jac }}; + external_function_param_{{ model.dyn_ext_fun_type }}_create(&capsule->discr_dyn_phi_fun_jac_ut_xt[i], {{ dims.np }}); + {%- endif %} + } + + {%- if solver_options.hessian_approx == "EXACT" %} + capsule->discr_dyn_phi_fun_jac_ut_xt_hess = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }})*N); + for (int i = 0; i < N; i++) + { + {%- if model.dyn_ext_fun_type == "casadi" %} + MAP_CASADI_FNC(discr_dyn_phi_fun_jac_ut_xt_hess[i], {{ model.name }}_dyn_disc_phi_fun_jac_hess); + {%- else %} + capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i].fun = &{{ model.dyn_disc_fun_jac_hess }}; + external_function_param_{{ model.dyn_ext_fun_type }}_create(&capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i], {{ dims.np }}); + {%- endif %} + } + {%- endif %} +{%- endif %} + + +{%- if cost.cost_type_0 == "NONLINEAR_LS" %} + // nonlinear least squares function + MAP_CASADI_FNC(cost_y_0_fun, {{ model.name }}_cost_y_0_fun); + MAP_CASADI_FNC(cost_y_0_fun_jac_ut_xt, {{ model.name }}_cost_y_0_fun_jac_ut_xt); + MAP_CASADI_FNC(cost_y_0_hess, {{ model.name }}_cost_y_0_hess); + +{%- elif cost.cost_type_0 == "EXTERNAL" %} + // external cost + {%- if cost.cost_ext_fun_type_0 == "casadi" %} + MAP_CASADI_FNC(ext_cost_0_fun, {{ model.name }}_cost_ext_cost_0_fun); + {%- else %} + capsule->ext_cost_0_fun.fun = &{{ cost.cost_function_ext_cost_0 }}; + external_function_param_{{ cost.cost_ext_fun_type_0 }}_create(&capsule->ext_cost_0_fun, {{ dims.np }}); + {%- endif %} + + // external cost + {%- if cost.cost_ext_fun_type_0 == "casadi" %} + MAP_CASADI_FNC(ext_cost_0_fun_jac, {{ model.name }}_cost_ext_cost_0_fun_jac); + {%- else %} + capsule->ext_cost_0_fun_jac.fun = &{{ cost.cost_function_ext_cost_0 }}; + external_function_param_{{ cost.cost_ext_fun_type_0 }}_create(&capsule->ext_cost_0_fun_jac, {{ dims.np }}); + {%- endif %} + + // external cost + {%- if cost.cost_ext_fun_type_0 == "casadi" %} + MAP_CASADI_FNC(ext_cost_0_fun_jac_hess, {{ model.name }}_cost_ext_cost_0_fun_jac_hess); + {%- else %} + capsule->ext_cost_0_fun_jac_hess.fun = &{{ cost.cost_function_ext_cost_0 }}; + external_function_param_{{ cost.cost_ext_fun_type_0 }}_create(&capsule->ext_cost_0_fun_jac_hess, {{ dims.np }}); + {%- endif %} +{%- endif %} + +{%- if cost.cost_type == "NONLINEAR_LS" %} + // nonlinear least squares cost + capsule->cost_y_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N-1; i++) + { + MAP_CASADI_FNC(cost_y_fun[i], {{ model.name }}_cost_y_fun); + } + + capsule->cost_y_fun_jac_ut_xt = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N-1; i++) + { + MAP_CASADI_FNC(cost_y_fun_jac_ut_xt[i], {{ model.name }}_cost_y_fun_jac_ut_xt); + } + + capsule->cost_y_hess = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N-1; i++) + { + MAP_CASADI_FNC(cost_y_hess[i], {{ model.name }}_cost_y_hess); + } +{%- elif cost.cost_type == "EXTERNAL" %} + // external cost + capsule->ext_cost_fun = (external_function_param_{{ cost.cost_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ cost.cost_ext_fun_type }})*N); + for (int i = 0; i < N-1; i++) + { + {%- if cost.cost_ext_fun_type == "casadi" %} + MAP_CASADI_FNC(ext_cost_fun[i], {{ model.name }}_cost_ext_cost_fun); + {%- else %} + capsule->ext_cost_fun[i].fun = &{{ cost.cost_function_ext_cost }}; + external_function_param_{{ cost.cost_ext_fun_type }}_create(&capsule->ext_cost_fun[i], {{ dims.np }}); + {%- endif %} + } + + capsule->ext_cost_fun_jac = (external_function_param_{{ cost.cost_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ cost.cost_ext_fun_type }})*N); + for (int i = 0; i < N-1; i++) + { + {%- if cost.cost_ext_fun_type == "casadi" %} + MAP_CASADI_FNC(ext_cost_fun_jac[i], {{ model.name }}_cost_ext_cost_fun_jac); + {%- else %} + capsule->ext_cost_fun_jac[i].fun = &{{ cost.cost_function_ext_cost }}; + external_function_param_{{ cost.cost_ext_fun_type }}_create(&capsule->ext_cost_fun_jac[i], {{ dims.np }}); + {%- endif %} + } + + capsule->ext_cost_fun_jac_hess = (external_function_param_{{ cost.cost_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ cost.cost_ext_fun_type }})*N); + for (int i = 0; i < N-1; i++) + { + {%- if cost.cost_ext_fun_type == "casadi" %} + MAP_CASADI_FNC(ext_cost_fun_jac_hess[i], {{ model.name }}_cost_ext_cost_fun_jac_hess); + {%- else %} + capsule->ext_cost_fun_jac_hess[i].fun = &{{ cost.cost_function_ext_cost }}; + external_function_param_{{ cost.cost_ext_fun_type }}_create(&capsule->ext_cost_fun_jac_hess[i], {{ dims.np }}); + {%- endif %} + } +{%- endif %} + +{%- if cost.cost_type_e == "NONLINEAR_LS" %} + // nonlinear least square function + MAP_CASADI_FNC(cost_y_e_fun, {{ model.name }}_cost_y_e_fun); + MAP_CASADI_FNC(cost_y_e_fun_jac_ut_xt, {{ model.name }}_cost_y_e_fun_jac_ut_xt); + MAP_CASADI_FNC(cost_y_e_hess, {{ model.name }}_cost_y_e_hess); +{%- elif cost.cost_type_e == "EXTERNAL" %} + // external cost - function + {%- if cost.cost_ext_fun_type_e == "casadi" %} + MAP_CASADI_FNC(ext_cost_e_fun, {{ model.name }}_cost_ext_cost_e_fun); + {% else %} + capsule->ext_cost_e_fun.fun = &{{ cost.cost_function_ext_cost_e }}; + external_function_param_{{ cost.cost_ext_fun_type_e }}_create(&capsule->ext_cost_e_fun, {{ dims.np }}); + {%- endif %} + + // external cost - jacobian + {%- if cost.cost_ext_fun_type_e == "casadi" %} + MAP_CASADI_FNC(ext_cost_e_fun_jac, {{ model.name }}_cost_ext_cost_e_fun_jac); + {%- else %} + capsule->ext_cost_e_fun_jac.fun = &{{ cost.cost_function_ext_cost_e }}; + external_function_param_{{ cost.cost_ext_fun_type_e }}_create(&capsule->ext_cost_e_fun_jac, {{ dims.np }}); + {%- endif %} + + // external cost - hessian + {%- if cost.cost_ext_fun_type_e == "casadi" %} + MAP_CASADI_FNC(ext_cost_e_fun_jac_hess, {{ model.name }}_cost_ext_cost_e_fun_jac_hess); + {%- else %} + capsule->ext_cost_e_fun_jac_hess.fun = &{{ cost.cost_function_ext_cost_e }}; + external_function_param_{{ cost.cost_ext_fun_type_e }}_create(&capsule->ext_cost_e_fun_jac_hess, {{ dims.np }}); + {%- endif %} +{%- endif %} + +#undef MAP_CASADI_FNC +} + + +/** + * Internal function for {{ model.name }}_acados_create: step 4 + */ +void {{ model.name }}_acados_create_4_set_default_parameters({{ model.name }}_solver_capsule* capsule) { +{%- if dims.np > 0 %} + const int N = capsule->nlp_solver_plan->N; + // initialize parameters to nominal value + double* p = calloc(NP, sizeof(double)); + {%- for item in parameter_values %} + {%- if item != 0 %} + p[{{ loop.index0 }}] = {{ item }}; + {%- endif %} + {%- endfor %} + + for (int i = 0; i <= N; i++) { + {{ model.name }}_acados_update_params(capsule, i, p, NP); + } + free(p); +{%- else %} + // no parameters defined +{%- endif %}{# if dims.np #} +} + + +/** + * Internal function for {{ model.name }}_acados_create: step 5 + */ +void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule* capsule, const int N, double* new_time_steps) +{ + assert(N == capsule->nlp_solver_plan->N); + ocp_nlp_config* nlp_config = capsule->nlp_config; + ocp_nlp_dims* nlp_dims = capsule->nlp_dims; + + /************************************************ + * nlp_in + ************************************************/ +// ocp_nlp_in * nlp_in = ocp_nlp_in_create(nlp_config, nlp_dims); +// capsule->nlp_in = nlp_in; + ocp_nlp_in * nlp_in = capsule->nlp_in; + + // set up time_steps + {% set all_equal = true -%} + {%- set val = solver_options.time_steps[0] %} + {%- for j in range(start=1, end=dims.N) %} + {%- if val != solver_options.time_steps[j] %} + {%- set_global all_equal = false %} + {%- break %} + {%- endif %} + {%- endfor %} + + if (new_time_steps) { + {{ model.name }}_acados_update_time_steps(capsule, N, new_time_steps); + } else { + {%- if all_equal == true -%} + // all time_steps are identical + double time_step = {{ solver_options.time_steps[0] }}; + for (int i = 0; i < N; i++) + { + ocp_nlp_in_set(nlp_config, nlp_dims, nlp_in, i, "Ts", &time_step); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "scaling", &time_step); + } + {%- else -%} + // time_steps are different + double* time_steps = malloc(N*sizeof(double)); + {%- for j in range(end=dims.N) %} + time_steps[{{ j }}] = {{ solver_options.time_steps[j] }}; + {%- endfor %} + {{ model.name }}_acados_update_time_steps(capsule, N, time_steps); + free(time_steps); + {%- endif %} + } + + /**** Dynamics ****/ + for (int i = 0; i < N; i++) + { + {%- if solver_options.integrator_type == "ERK" %} + ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "expl_vde_forw", &capsule->forw_vde_casadi[i]); + ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "expl_ode_fun", &capsule->expl_ode_fun[i]); + {%- if solver_options.hessian_approx == "EXACT" %} + ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "expl_ode_hess", &capsule->hess_vde_casadi[i]); + {%- endif %} + {% elif solver_options.integrator_type == "IRK" %} + ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "impl_dae_fun", &capsule->impl_dae_fun[i]); + ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, + "impl_dae_fun_jac_x_xdot_z", &capsule->impl_dae_fun_jac_x_xdot_z[i]); + ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, + "impl_dae_jac_x_xdot_u", &capsule->impl_dae_jac_x_xdot_u_z[i]); + {%- if solver_options.hessian_approx == "EXACT" %} + ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "impl_dae_hess", &capsule->impl_dae_hess[i]); + {%- endif %} + {% elif solver_options.integrator_type == "LIFTED_IRK" %} + ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "impl_dae_fun", &capsule->impl_dae_fun[i]); + ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, + "impl_dae_fun_jac_x_xdot_u", &capsule->impl_dae_fun_jac_x_xdot_u[i]); + {% elif solver_options.integrator_type == "GNSF" %} + {% if model.gnsf.purely_linear != 1 %} + ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "phi_fun", &capsule->gnsf_phi_fun[i]); + ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "phi_fun_jac_y", &capsule->gnsf_phi_fun_jac_y[i]); + ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "phi_jac_y_uhat", &capsule->gnsf_phi_jac_y_uhat[i]); + {% if model.gnsf.nontrivial_f_LO == 1 %} + ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "f_lo_jac_x1_x1dot_u_z", + &capsule->gnsf_f_lo_jac_x1_x1dot_u_z[i]); + {%- endif %} + {%- endif %} + ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "gnsf_get_matrices_fun", + &capsule->gnsf_get_matrices_fun[i]); + {% elif solver_options.integrator_type == "DISCRETE" %} + ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "disc_dyn_fun", &capsule->discr_dyn_phi_fun[i]); + ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "disc_dyn_fun_jac", + &capsule->discr_dyn_phi_fun_jac_ut_xt[i]); + {%- if solver_options.hessian_approx == "EXACT" %} + ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "disc_dyn_fun_jac_hess", + &capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i]); + {%- endif %} + {%- endif %} + } + + /**** Cost ****/ +{%- if cost.cost_type_0 == "NONLINEAR_LS" or cost.cost_type_0 == "LINEAR_LS" %} + {%- if dims.ny_0 > 0 %} + double* W_0 = calloc(NY0*NY0, sizeof(double)); + // change only the non-zero elements: + {%- for j in range(end=dims.ny_0) %} + {%- for k in range(end=dims.ny_0) %} + {%- if cost.W_0[j][k] != 0 %} + W_0[{{ j }}+(NY0) * {{ k }}] = {{ cost.W_0[j][k] }}; + {%- endif %} + {%- endfor %} + {%- endfor %} + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "W", W_0); + free(W_0); + + double* yref_0 = calloc(NY0, sizeof(double)); + // change only the non-zero elements: + {%- for j in range(end=dims.ny_0) %} + {%- if cost.yref_0[j] != 0 %} + yref_0[{{ j }}] = {{ cost.yref_0[j] }}; + {%- endif %} + {%- endfor %} + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", yref_0); + free(yref_0); + {%- endif %} +{%- endif %} + +{%- if cost.cost_type == "NONLINEAR_LS" or cost.cost_type == "LINEAR_LS" %} + {%- if dims.ny > 0 %} + double* W = calloc(NY*NY, sizeof(double)); + // change only the non-zero elements: + {%- for j in range(end=dims.ny) %} + {%- for k in range(end=dims.ny) %} + {%- if cost.W[j][k] != 0 %} + W[{{ j }}+(NY) * {{ k }}] = {{ cost.W[j][k] }}; + {%- endif %} + {%- endfor %} + {%- endfor %} + + double* yref = calloc(NY, sizeof(double)); + // change only the non-zero elements: + {%- for j in range(end=dims.ny) %} + {%- if cost.yref[j] != 0 %} + yref[{{ j }}] = {{ cost.yref[j] }}; + {%- endif %} + {%- endfor %} + + for (int i = 1; i < N; i++) + { + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "W", W); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "yref", yref); + } + free(W); + free(yref); + {%- endif %} +{%- endif %} + +{%- if cost.cost_type_0 == "LINEAR_LS" %} + double* Vx_0 = calloc(NY0*NX, sizeof(double)); + // change only the non-zero elements: + {%- for j in range(end=dims.ny_0) %} + {%- for k in range(end=dims.nx) %} + {%- if cost.Vx_0[j][k] != 0 %} + Vx_0[{{ j }}+(NY0) * {{ k }}] = {{ cost.Vx_0[j][k] }}; + {%- endif %} + {%- endfor %} + {%- endfor %} + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "Vx", Vx_0); + free(Vx_0); + + {%- if dims.ny_0 > 0 and dims.nu > 0 %} + double* Vu_0 = calloc(NY0*NU, sizeof(double)); + // change only the non-zero elements: + {%- for j in range(end=dims.ny_0) %} + {%- for k in range(end=dims.nu) %} + {%- if cost.Vu_0[j][k] != 0 %} + Vu_0[{{ j }}+(NY0) * {{ k }}] = {{ cost.Vu_0[j][k] }}; + {%- endif %} + {%- endfor %} + {%- endfor %} + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "Vu", Vu_0); + free(Vu_0); + {%- endif %} + + {%- if dims.ny_0 > 0 and dims.nz > 0 %} + double* Vz_0 = calloc(NY0*NZ, sizeof(double)); + // change only the non-zero elements: + {% for j in range(end=dims.ny_0) %} + {%- for k in range(end=dims.nz) %} + {%- if cost.Vz_0[j][k] != 0 %} + Vz_0[{{ j }}+(NY0) * {{ k }}] = {{ cost.Vz_0[j][k] }}; + {%- endif %} + {%- endfor %} + {%- endfor %} + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "Vz", Vz_0); + free(Vz_0); + {%- endif %} +{%- endif %}{# LINEAR LS #} + + +{%- if cost.cost_type == "LINEAR_LS" %} + double* Vx = calloc(NY*NX, sizeof(double)); + // change only the non-zero elements: + {%- for j in range(end=dims.ny) %} + {%- for k in range(end=dims.nx) %} + {%- if cost.Vx[j][k] != 0 %} + Vx[{{ j }}+(NY) * {{ k }}] = {{ cost.Vx[j][k] }}; + {%- endif %} + {%- endfor %} + {%- endfor %} + for (int i = 1; i < N; i++) + { + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "Vx", Vx); + } + free(Vx); + + {% if dims.ny > 0 and dims.nu > 0 %} + double* Vu = calloc(NY*NU, sizeof(double)); + // change only the non-zero elements: + {% for j in range(end=dims.ny) %} + {%- for k in range(end=dims.nu) %} + {%- if cost.Vu[j][k] != 0 %} + Vu[{{ j }}+(NY) * {{ k }}] = {{ cost.Vu[j][k] }}; + {%- endif %} + {%- endfor %} + {%- endfor %} + + for (int i = 1; i < N; i++) + { + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "Vu", Vu); + } + free(Vu); + {%- endif %} + + {%- if dims.ny > 0 and dims.nz > 0 %} + double* Vz = calloc(NY*NZ, sizeof(double)); + // change only the non-zero elements: + {% for j in range(end=dims.ny) %} + {%- for k in range(end=dims.nz) %} + {%- if cost.Vz[j][k] != 0 %} + Vz[{{ j }}+(NY) * {{ k }}] = {{ cost.Vz[j][k] }}; + {%- endif %} + {%- endfor %} + {%- endfor %} + + for (int i = 1; i < N; i++) + { + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "Vz", Vz); + } + free(Vz); + {%- endif %} +{%- endif %}{# LINEAR LS #} + + +{%- if cost.cost_type_0 == "NONLINEAR_LS" %} + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "nls_y_fun", &capsule->cost_y_0_fun); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "nls_y_fun_jac", &capsule->cost_y_0_fun_jac_ut_xt); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "nls_y_hess", &capsule->cost_y_0_hess); +{%- elif cost.cost_type_0 == "EXTERNAL" %} + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "ext_cost_fun", &capsule->ext_cost_0_fun); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "ext_cost_fun_jac", &capsule->ext_cost_0_fun_jac); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "ext_cost_fun_jac_hess", &capsule->ext_cost_0_fun_jac_hess); +{%- endif %} + +{%- if cost.cost_type == "NONLINEAR_LS" %} + for (int i = 1; i < N; i++) + { + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "nls_y_fun", &capsule->cost_y_fun[i-1]); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "nls_y_fun_jac", &capsule->cost_y_fun_jac_ut_xt[i-1]); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "nls_y_hess", &capsule->cost_y_hess[i-1]); + } +{%- elif cost.cost_type == "EXTERNAL" %} + for (int i = 1; i < N; i++) + { + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "ext_cost_fun", &capsule->ext_cost_fun[i-1]); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "ext_cost_fun_jac", &capsule->ext_cost_fun_jac[i-1]); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "ext_cost_fun_jac_hess", &capsule->ext_cost_fun_jac_hess[i-1]); + } +{%- endif %} + +{%- if dims.ns > 0 %} + double* zlumem = calloc(4*NS, sizeof(double)); + double* Zl = zlumem+NS*0; + double* Zu = zlumem+NS*1; + double* zl = zlumem+NS*2; + double* zu = zlumem+NS*3; + // change only the non-zero elements: + {%- for j in range(end=dims.ns) %} + {%- if cost.Zl[j] != 0 %} + Zl[{{ j }}] = {{ cost.Zl[j] }}; + {%- endif %} + {%- endfor %} + + {%- for j in range(end=dims.ns) %} + {%- if cost.Zu[j] != 0 %} + Zu[{{ j }}] = {{ cost.Zu[j] }}; + {%- endif %} + {%- endfor %} + + {%- for j in range(end=dims.ns) %} + {%- if cost.zl[j] != 0 %} + zl[{{ j }}] = {{ cost.zl[j] }}; + {%- endif %} + {%- endfor %} + + {%- for j in range(end=dims.ns) %} + {%- if cost.zu[j] != 0 %} + zu[{{ j }}] = {{ cost.zu[j] }}; + {%- endif %} + {%- endfor %} + + for (int i = 0; i < N; i++) + { + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "Zl", Zl); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "Zu", Zu); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "zl", zl); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "zu", zu); + } + free(zlumem); +{%- endif %} + + // terminal cost +{%- if cost.cost_type_e == "LINEAR_LS" or cost.cost_type_e == "NONLINEAR_LS" %} + {%- if dims.ny_e > 0 %} + double* yref_e = calloc(NYN, sizeof(double)); + // change only the non-zero elements: + {%- for j in range(end=dims.ny_e) %} + {%- if cost.yref_e[j] != 0 %} + yref_e[{{ j }}] = {{ cost.yref_e[j] }}; + {%- endif %} + {%- endfor %} + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "yref", yref_e); + free(yref_e); + + double* W_e = calloc(NYN*NYN, sizeof(double)); + // change only the non-zero elements: + {%- for j in range(end=dims.ny_e) %} + {%- for k in range(end=dims.ny_e) %} + {%- if cost.W_e[j][k] != 0 %} + W_e[{{ j }}+(NYN) * {{ k }}] = {{ cost.W_e[j][k] }}; + {%- endif %} + {%- endfor %} + {%- endfor %} + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "W", W_e); + free(W_e); + + {%- if cost.cost_type_e == "LINEAR_LS" %} + double* Vx_e = calloc(NYN*NX, sizeof(double)); + // change only the non-zero elements: + {% for j in range(end=dims.ny_e) %} + {%- for k in range(end=dims.nx) %} + {%- if cost.Vx_e[j][k] != 0 %} + Vx_e[{{ j }}+(NYN) * {{ k }}] = {{ cost.Vx_e[j][k] }}; + {%- endif %} + {%- endfor %} + {%- endfor %} + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "Vx", Vx_e); + free(Vx_e); + {%- endif %} + + {%- if cost.cost_type_e == "NONLINEAR_LS" %} + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_fun", &capsule->cost_y_e_fun); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_fun_jac", &capsule->cost_y_e_fun_jac_ut_xt); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_hess", &capsule->cost_y_e_hess); + {%- endif %} + {%- endif %}{# ny_e > 0 #} + +{%- elif cost.cost_type_e == "EXTERNAL" %} + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "ext_cost_fun", &capsule->ext_cost_e_fun); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "ext_cost_fun_jac", &capsule->ext_cost_e_fun_jac); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "ext_cost_fun_jac_hess", &capsule->ext_cost_e_fun_jac_hess); +{%- endif %} + +{% if dims.ns_e > 0 %} + double* zluemem = calloc(4*NSN, sizeof(double)); + double* Zl_e = zluemem+NSN*0; + double* Zu_e = zluemem+NSN*1; + double* zl_e = zluemem+NSN*2; + double* zu_e = zluemem+NSN*3; + + // change only the non-zero elements: + {% for j in range(end=dims.ns_e) %} + {%- if cost.Zl_e[j] != 0 %} + Zl_e[{{ j }}] = {{ cost.Zl_e[j] }}; + {%- endif %} + {%- endfor %} + + {% for j in range(end=dims.ns_e) %} + {%- if cost.Zu_e[j] != 0 %} + Zu_e[{{ j }}] = {{ cost.Zu_e[j] }}; + {%- endif %} + {%- endfor %} + + {% for j in range(end=dims.ns_e) %} + {%- if cost.zl_e[j] != 0 %} + zl_e[{{ j }}] = {{ cost.zl_e[j] }}; + {%- endif %} + {%- endfor %} + + {% for j in range(end=dims.ns_e) %} + {%- if cost.zu_e[j] != 0 %} + zu_e[{{ j }}] = {{ cost.zu_e[j] }}; + {%- endif %} + {%- endfor %} + + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "Zl", Zl_e); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "Zu", Zu_e); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "zl", zl_e); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "zu", zu_e); + free(zluemem); +{%- endif %} + + /**** Constraints ****/ + + // bounds for initial stage +{%- if dims.nbx_0 > 0 %} + // x0 + int* idxbx0 = malloc(NBX0 * sizeof(int)); + {%- for i in range(end=dims.nbx_0) %} + idxbx0[{{ i }}] = {{ constraints.idxbx_0[i] }}; + {%- endfor %} + + double* lubx0 = calloc(2*NBX0, sizeof(double)); + double* lbx0 = lubx0; + double* ubx0 = lubx0 + NBX0; + // change only the non-zero elements: + {%- for i in range(end=dims.nbx_0) %} + {%- if constraints.lbx_0[i] != 0 %} + lbx0[{{ i }}] = {{ constraints.lbx_0[i] }}; + {%- endif %} + {%- if constraints.ubx_0[i] != 0 %} + ubx0[{{ i }}] = {{ constraints.ubx_0[i] }}; + {%- endif %} + {%- endfor %} + + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxbx", idxbx0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", lbx0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", ubx0); + free(idxbx0); + free(lubx0); +{%- endif %} + +{%- if dims.nbxe_0 > 0 %} + // idxbxe_0 + int* idxbxe_0 = malloc({{ dims.nbxe_0 }} * sizeof(int)); + {% for i in range(end=dims.nbxe_0) %} + idxbxe_0[{{ i }}] = {{ constraints.idxbxe_0[i] }}; + {%- endfor %} + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxbxe", idxbxe_0); + free(idxbxe_0); +{%- endif %} + + /* constraints that are the same for initial and intermediate */ +{%- if dims.nsbx > 0 %} +{# TODO: introduce nsbx0 & move this block down!! #} + // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxsbx", idxsbx); + // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lsbx", lsbx); + // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "usbx", usbx); + + // soft bounds on x + int* idxsbx = malloc(NSBX * sizeof(int)); + {%- for i in range(end=dims.nsbx) %} + idxsbx[{{ i }}] = {{ constraints.idxsbx[i] }}; + {%- endfor %} + + double* lusbx = calloc(2*NSBX, sizeof(double)); + double* lsbx = lusbx; + double* usbx = lusbx + NSBX; + {%- for i in range(end=dims.nsbx) %} + {%- if constraints.lsbx[i] != 0 %} + lsbx[{{ i }}] = {{ constraints.lsbx[i] }}; + {%- endif %} + {%- if constraints.usbx[i] != 0 %} + usbx[{{ i }}] = {{ constraints.usbx[i] }}; + {%- endif %} + {%- endfor %} + + for (int i = 1; i < N; i++) + { + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxsbx", idxsbx); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsbx", lsbx); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "usbx", usbx); + } + free(idxsbx); + free(lusbx); +{%- endif %} + + +{%- if dims.nbu > 0 %} + // u + int* idxbu = malloc(NBU * sizeof(int)); + {% for i in range(end=dims.nbu) %} + idxbu[{{ i }}] = {{ constraints.idxbu[i] }}; + {%- endfor %} + double* lubu = calloc(2*NBU, sizeof(double)); + double* lbu = lubu; + double* ubu = lubu + NBU; + {% for i in range(end=dims.nbu) %} + {%- if constraints.lbu[i] != 0 %} + lbu[{{ i }}] = {{ constraints.lbu[i] }}; + {%- endif %} + {%- if constraints.ubu[i] != 0 %} + ubu[{{ i }}] = {{ constraints.ubu[i] }}; + {%- endif %} + {%- endfor %} + + for (int i = 0; i < N; i++) + { + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxbu", idxbu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lbu", lbu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "ubu", ubu); + } + free(idxbu); + free(lubu); +{%- endif %} + +{%- if dims.nsbu > 0 %} + // set up soft bounds for u + int* idxsbu = malloc(NSBU * sizeof(int)); + {% for i in range(end=dims.nsbu) %} + idxsbu[{{ i }}] = {{ constraints.idxsbu[i] }}; + {%- endfor %} + double* lusbu = calloc(2*NSBU, sizeof(double)); + double* lsbu = lusbu; + double* usbu = lusbu + NSBU; + {% for i in range(end=dims.nsbu) %} + {%- if constraints.lsbu[i] != 0 %} + lsbu[{{ i }}] = {{ constraints.lsbu[i] }}; + {%- endif %} + {%- if constraints.usbu[i] != 0 %} + usbu[{{ i }}] = {{ constraints.usbu[i] }}; + {%- endif %} + {%- endfor %} + for (int i = 0; i < N; i++) + { + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxsbu", idxsbu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsbu", lsbu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "usbu", usbu); + } + free(idxsbu); + free(lusbu); +{%- endif %} + +{% if dims.nsg > 0 %} + // set up soft bounds for general linear constraints + int* idxsg = malloc(NSG * sizeof(int)); + {% for i in range(end=dims.nsg) %} + idxsg[{{ i }}] = {{ constraints.idxsg[i] }}; + {%- endfor %} + double* lusg = calloc(2*NSG, sizeof(double)); + double* lsg = lusg; + double* usg = lusg + NSG; + {% for i in range(end=dims.nsg) %} + {%- if constraints.lsg[i] != 0 %} + lsg[{{ i }}] = {{ constraints.lsg[i] }}; + {%- endif %} + {%- if constraints.usg[i] != 0 %} + usg[{{ i }}] = {{ constraints.usg[i] }}; + {%- endif %} + {%- endfor %} + + for (int i = 0; i < N; i++) + { + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxsg", idxsg); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsg", lsg); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "usg", usg); + } + free(idxsg); + free(lusg); +{%- endif %} + +{% if dims.nsh > 0 %} + // set up soft bounds for nonlinear constraints + int* idxsh = malloc(NSH * sizeof(int)); + {% for i in range(end=dims.nsh) %} + idxsh[{{ i }}] = {{ constraints.idxsh[i] }}; + {%- endfor %} + double* lush = calloc(2*NSH, sizeof(double)); + double* lsh = lush; + double* ush = lush + NSH; + {% for i in range(end=dims.nsh) %} + {%- if constraints.lsh[i] != 0 %} + lsh[{{ i }}] = {{ constraints.lsh[i] }}; + {%- endif %} + {%- if constraints.ush[i] != 0 %} + ush[{{ i }}] = {{ constraints.ush[i] }}; + {%- endif %} + {%- endfor %} + + for (int i = 0; i < N; i++) + { + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxsh", idxsh); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsh", lsh); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "ush", ush); + } + free(idxsh); + free(lush); +{%- endif %} + +{% if dims.nsphi > 0 %} + // set up soft bounds for convex-over-nonlinear constraints + int* idxsphi = malloc(NSPHI * sizeof(int)); + {% for i in range(end=dims.nsphi) %} + idxsphi[{{ i }}] = {{ constraints.idxsphi[i] }}; + {%- endfor %} + double* lusphi = calloc(2*NSPHI, sizeof(double)); + double* lsphi = lusphi; + double* usphi = lusphi + NSPHI; + {% for i in range(end=dims.nsphi) %} + {%- if constraints.lsphi[i] != 0 %} + lsphi[{{ i }}] = {{ constraints.lsphi[i] }}; + {%- endif %} + {%- if constraints.usphi[i] != 0 %} + usphi[{{ i }}] = {{ constraints.usphi[i] }}; + {%- endif %} + {%- endfor %} + + for (int i = 0; i < N; i++) + { + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxsphi", idxsphi); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsphi", lsphi); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "usphi", usphi); + } + free(idxsphi); + free(lusphi); +{%- endif %} + +{% if dims.nbx > 0 %} + // x + int* idxbx = malloc(NBX * sizeof(int)); + {% for i in range(end=dims.nbx) %} + idxbx[{{ i }}] = {{ constraints.idxbx[i] }}; + {%- endfor %} + double* lubx = calloc(2*NBX, sizeof(double)); + double* lbx = lubx; + double* ubx = lubx + NBX; + {% for i in range(end=dims.nbx) %} + {%- if constraints.lbx[i] != 0 %} + lbx[{{ i }}] = {{ constraints.lbx[i] }}; + {%- endif %} + {%- if constraints.ubx[i] != 0 %} + ubx[{{ i }}] = {{ constraints.ubx[i] }}; + {%- endif %} + {%- endfor %} + + for (int i = 1; i < N; i++) + { + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxbx", idxbx); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lbx", lbx); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "ubx", ubx); + } + free(idxbx); + free(lubx); +{%- endif %} + +{% if dims.ng > 0 %} + // set up general constraints for stage 0 to N-1 + double* D = calloc(NG*NU, sizeof(double)); + double* C = calloc(NG*NX, sizeof(double)); + double* lug = calloc(2*NG, sizeof(double)); + double* lg = lug; + double* ug = lug + NG; + + {% for j in range(end=dims.ng) -%} + {% for k in range(end=dims.nu) %} + {%- if constraints.D[j][k] != 0 %} + D[{{ j }}+NG * {{ k }}] = {{ constraints.D[j][k] }}; + {%- endif %} + {%- endfor %} + {%- endfor %} + + {% for j in range(end=dims.ng) -%} + {% for k in range(end=dims.nx) %} + {%- if constraints.C[j][k] != 0 %} + C[{{ j }}+NG * {{ k }}] = {{ constraints.C[j][k] }}; + {%- endif %} + {%- endfor %} + {%- endfor %} + + {% for i in range(end=dims.ng) %} + {%- if constraints.lg[i] != 0 %} + lg[{{ i }}] = {{ constraints.lg[i] }}; + {%- endif %} + {%- endfor %} + + {% for i in range(end=dims.ng) %} + {%- if constraints.ug[i] != 0 %} + ug[{{ i }}] = {{ constraints.ug[i] }}; + {%- endif %} + {%- endfor %} + + for (int i = 0; i < N; i++) + { + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "D", D); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "C", C); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lg", lg); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "ug", ug); + } + free(D); + free(C); + free(lug); +{%- endif %} + +{% if dims.nh > 0 %} + // set up nonlinear constraints for stage 0 to N-1 + double* luh = calloc(2*NH, sizeof(double)); + double* lh = luh; + double* uh = luh + NH; + + {% for i in range(end=dims.nh) %} + {%- if constraints.lh[i] != 0 %} + lh[{{ i }}] = {{ constraints.lh[i] }}; + {%- endif %} + {%- endfor %} + + {% for i in range(end=dims.nh) %} + {%- if constraints.uh[i] != 0 %} + uh[{{ i }}] = {{ constraints.uh[i] }}; + {%- endif %} + {%- endfor %} + + for (int i = 0; i < N; i++) + { + // nonlinear constraints for stages 0 to N-1 + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "nl_constr_h_fun_jac", + &capsule->nl_constr_h_fun_jac[i]); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "nl_constr_h_fun", + &capsule->nl_constr_h_fun[i]); + {% if solver_options.hessian_approx == "EXACT" %} + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, + "nl_constr_h_fun_jac_hess", &capsule->nl_constr_h_fun_jac_hess[i]); + {% endif %} + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lh", lh); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "uh", uh); + } + free(luh); +{%- endif %} + +{% if dims.nphi > 0 and constraints.constr_type == "BGP" %} + // set up convex-over-nonlinear constraints for stage 0 to N-1 + double* luphi = calloc(2*NPHI, sizeof(double)); + double* lphi = luphi; + double* uphi = luphi + NPHI; + {% for i in range(end=dims.nphi) %} + {%- if constraints.lphi[i] != 0 %} + lphi[{{ i }}] = {{ constraints.lphi[i] }}; + {%- endif %} + {%- endfor %} + + {% for i in range(end=dims.nphi) %} + {%- if constraints.uphi[i] != 0 %} + uphi[{{ i }}] = {{ constraints.uphi[i] }}; + {%- endif %} + {%- endfor %} + + for (int i = 0; i < N; i++) + { + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, + "nl_constr_phi_o_r_fun_phi_jac_ux_z_phi_hess_r_jac_ux", &capsule->phi_constraint[i]); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lphi", lphi); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "uphi", uphi); + } + free(luphi); +{%- endif %} + + /* terminal constraints */ +{% if dims.nbx_e > 0 %} + // set up bounds for last stage + // x + int* idxbx_e = malloc(NBXN * sizeof(int)); + {% for i in range(end=dims.nbx_e) %} + idxbx_e[{{ i }}] = {{ constraints.idxbx_e[i] }}; + {%- endfor %} + double* lubx_e = calloc(2*NBXN, sizeof(double)); + double* lbx_e = lubx_e; + double* ubx_e = lubx_e + NBXN; + {% for i in range(end=dims.nbx_e) %} + {%- if constraints.lbx_e[i] != 0 %} + lbx_e[{{ i }}] = {{ constraints.lbx_e[i] }}; + {%- endif %} + {%- if constraints.ubx_e[i] != 0 %} + ubx_e[{{ i }}] = {{ constraints.ubx_e[i] }}; + {%- endif %} + {%- endfor %} + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "idxbx", idxbx_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lbx", lbx_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ubx", ubx_e); + free(idxbx_e); + free(lubx_e); +{%- endif %} + +{% if dims.nsg_e > 0 %} + // set up soft bounds for general linear constraints + int* idxsg_e = calloc(NSGN, sizeof(int)); + {% for i in range(end=dims.nsg_e) %} + idxsg_e[{{ i }}] = {{ constraints.idxsg_e[i] }}; + {%- endfor %} + double* lusg_e = calloc(2*NSGN, sizeof(double)); + double* lsg_e = lusg_e; + double* usg_e = lusg_e + NSGN; + {% for i in range(end=dims.nsg_e) %} + {%- if constraints.lsg_e[i] != 0 %} + lsg_e[{{ i }}] = {{ constraints.lsg_e[i] }}; + {%- endif %} + {%- if constraints.usg_e[i] != 0 %} + usg_e[{{ i }}] = {{ constraints.usg_e[i] }}; + {%- endif %} + {%- endfor %} + + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "idxsg", idxsg_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lsg", lsg_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "usg", usg_e); + free(idxsg_e); + free(lusg_e); +{%- endif %} + +{% if dims.nsh_e > 0 %} + // set up soft bounds for nonlinear constraints + int* idxsh_e = malloc(NSHN * sizeof(int)); + {% for i in range(end=dims.nsh_e) %} + idxsh_e[{{ i }}] = {{ constraints.idxsh_e[i] }}; + {%- endfor %} + double* lush_e = calloc(2*NSHN, sizeof(double)); + double* lsh_e = lush_e; + double* ush_e = lush_e + NSHN; + {% for i in range(end=dims.nsh_e) %} + {%- if constraints.lsh_e[i] != 0 %} + lsh_e[{{ i }}] = {{ constraints.lsh_e[i] }}; + {%- endif %} + {%- if constraints.ush_e[i] != 0 %} + ush_e[{{ i }}] = {{ constraints.ush_e[i] }}; + {%- endif %} + {%- endfor %} + + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "idxsh", idxsh_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lsh", lsh_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ush", ush_e); + free(idxsh_e); + free(lush_e); +{%- endif %} + +{% if dims.nsphi_e > 0 %} + // set up soft bounds for convex-over-nonlinear constraints + int* idxsphi_e = malloc(NSPHIN * sizeof(int)); + {% for i in range(end=dims.nsphi_e) %} + idxsphi_e[{{ i }}] = {{ constraints.idxsphi_e[i] }}; + {%- endfor %} + double* lusphi_e = calloc(2*NSPHIN, sizeof(double)); + double* lsphi_e = lusphi_e; + double* usphi_e = lusphi_e + NSPHIN; + {% for i in range(end=dims.nsphi_e) %} + {%- if constraints.lsphi_e[i] != 0 %} + lsphi_e[{{ i }}] = {{ constraints.lsphi_e[i] }}; + {%- endif %} + {%- if constraints.usphi_e[i] != 0 %} + usphi_e[{{ i }}] = {{ constraints.usphi_e[i] }}; + {%- endif %} + {%- endfor %} + + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "idxsphi", idxsphi_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lsphi", lsphi_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "usphi", usphi_e); + free(idxsphi_e); + free(lusphi_e); +{%- endif %} + +{% if dims.nsbx_e > 0 %} + // soft bounds on x + int* idxsbx_e = malloc(NSBXN * sizeof(int)); + {% for i in range(end=dims.nsbx_e) %} + idxsbx_e[{{ i }}] = {{ constraints.idxsbx_e[i] }}; + {%- endfor %} + double* lusbx_e = calloc(2*NSBXN, sizeof(double)); + double* lsbx_e = lusbx_e; + double* usbx_e = lusbx_e + NSBXN; + {% for i in range(end=dims.nsbx_e) %} + {%- if constraints.lsbx_e[i] != 0 %} + lsbx_e[{{ i }}] = {{ constraints.lsbx_e[i] }}; + {%- endif %} + {%- if constraints.usbx_e[i] != 0 %} + usbx_e[{{ i }}] = {{ constraints.usbx_e[i] }}; + {%- endif %} + {%- endfor %} + + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "idxsbx", idxsbx_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lsbx", lsbx_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "usbx", usbx_e); + free(idxsbx_e); + free(lusbx_e); +{% endif %} + +{% if dims.ng_e > 0 %} + // set up general constraints for last stage + double* C_e = calloc(NGN*NX, sizeof(double)); + double* lug_e = calloc(2*NGN, sizeof(double)); + double* lg_e = lug_e; + double* ug_e = lug_e + NGN; + + {% for j in range(end=dims.ng) %} + {%- for k in range(end=dims.nx) %} + {%- if constraints.C_e[j][k] != 0 %} + C_e[{{ j }}+NG * {{ k }}] = {{ constraints.C_e[j][k] }}; + {%- endif %} + {%- endfor %} + {%- endfor %} + + {% for i in range(end=dims.ng_e) %} + {%- if constraints.lg_e[i] != 0 %} + lg_e[{{ i }}] = {{ constraints.lg_e[i] }}; + {%- endif %} + {%- if constraints.ug_e[i] != 0 %} + ug_e[{{ i }}] = {{ constraints.ug_e[i] }}; + {%- endif %} + {%- endfor %} + + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "C", C_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lg", lg_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ug", ug_e); + free(C_e); + free(lug_e); +{%- endif %} + +{% if dims.nh_e > 0 %} + // set up nonlinear constraints for last stage + double* luh_e = calloc(2*NHN, sizeof(double)); + double* lh_e = luh_e; + double* uh_e = luh_e + NHN; + {% for i in range(end=dims.nh_e) %} + {%- if constraints.lh_e[i] != 0 %} + lh_e[{{ i }}] = {{ constraints.lh_e[i] }}; + {%- endif %} + {%- endfor %} + + {% for i in range(end=dims.nh_e) %} + {%- if constraints.uh_e[i] != 0 %} + uh_e[{{ i }}] = {{ constraints.uh_e[i] }}; + {%- endif %} + {%- endfor %} + + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "nl_constr_h_fun_jac", &capsule->nl_constr_h_e_fun_jac); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "nl_constr_h_fun", &capsule->nl_constr_h_e_fun); + {% if solver_options.hessian_approx == "EXACT" %} + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "nl_constr_h_fun_jac_hess", + &capsule->nl_constr_h_e_fun_jac_hess); + {% endif %} + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lh", lh_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "uh", uh_e); + free(luh_e); +{%- endif %} + +{% if dims.nphi_e > 0 and constraints.constr_type_e == "BGP" %} + // set up convex-over-nonlinear constraints for last stage + double* luphi_e = calloc(2*NPHIN, sizeof(double)); + double* lphi_e = luphi_e; + double* uphi_e = luphi_e + NPHIN; + {% for i in range(end=dims.nphi_e) %} + {%- if constraints.lphi_e[i] != 0 %} + lphi_e[{{ i }}] = {{ constraints.lphi_e[i] }}; + {%- endif %} + {%- if constraints.uphi_e[i] != 0 %} + uphi_e[{{ i }}] = {{ constraints.uphi_e[i] }}; + {%- endif %} + {%- endfor %} + + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lphi", lphi_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "uphi", uphi_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, + "nl_constr_phi_o_r_fun_phi_jac_ux_z_phi_hess_r_jac_ux", &capsule->phi_e_constraint); + free(luphi_e); +{% endif %} +} + + +/** + * Internal function for {{ model.name }}_acados_create: step 6 + */ +void {{ model.name }}_acados_create_6_set_opts({{ model.name }}_solver_capsule* capsule) +{ + const int N = capsule->nlp_solver_plan->N; + ocp_nlp_config* nlp_config = capsule->nlp_config; + ocp_nlp_dims* nlp_dims = capsule->nlp_dims; + void *nlp_opts = capsule->nlp_opts; + + /************************************************ + * opts + ************************************************/ + +{% if solver_options.hessian_approx == "EXACT" %} + bool nlp_solver_exact_hessian = true; + // TODO: this if should not be needed! however, calling the setter with false leads to weird behavior. Investigate! + if (nlp_solver_exact_hessian) + { + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "exact_hess", &nlp_solver_exact_hessian); + } + int exact_hess_dyn = {{ solver_options.exact_hess_dyn }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "exact_hess_dyn", &exact_hess_dyn); + + int exact_hess_cost = {{ solver_options.exact_hess_cost }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "exact_hess_cost", &exact_hess_cost); + + int exact_hess_constr = {{ solver_options.exact_hess_constr }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "exact_hess_constr", &exact_hess_constr); +{%- endif -%} + +{%- if solver_options.globalization == "FIXED_STEP" %} + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "globalization", "fixed_step"); +{%- elif solver_options.globalization == "MERIT_BACKTRACKING" %} + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "globalization", "merit_backtracking"); + + double alpha_min = {{ solver_options.alpha_min }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "alpha_min", &alpha_min); + + double alpha_reduction = {{ solver_options.alpha_reduction }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "alpha_reduction", &alpha_reduction); + + int line_search_use_sufficient_descent = {{ solver_options.line_search_use_sufficient_descent }}; + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "line_search_use_sufficient_descent", &line_search_use_sufficient_descent); + + int globalization_use_SOC = {{ solver_options.globalization_use_SOC }}; + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "globalization_use_SOC", &globalization_use_SOC); + + double eps_sufficient_descent = {{ solver_options.eps_sufficient_descent }}; + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "eps_sufficient_descent", &eps_sufficient_descent); +{%- endif -%} + int full_step_dual = {{ solver_options.full_step_dual }}; + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "full_step_dual", &full_step_dual); + +{%- if dims.nz > 0 %} + // TODO: these options are lower level -> should be encapsulated! maybe through hessian approx option. + bool output_z_val = true; + bool sens_algebraic_val = true; + + for (int i = 0; i < N; i++) + ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_output_z", &output_z_val); + for (int i = 0; i < N; i++) + ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_sens_algebraic", &sens_algebraic_val); +{%- endif %} + +{%- if solver_options.integrator_type != "DISCRETE" %} + + // set collocation type (relevant for implicit integrators) + sim_collocation_type collocation_type = {{ solver_options.collocation_type }}; + for (int i = 0; i < N; i++) + ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_collocation_type", &collocation_type); + + // set up sim_method_num_steps + {%- set all_equal = true %} + {%- set val = solver_options.sim_method_num_steps[0] %} + {%- for j in range(start=1, end=dims.N) %} + {%- if val != solver_options.sim_method_num_steps[j] %} + {%- set_global all_equal = false %} + {%- break %} + {%- endif %} + {%- endfor %} + + {%- if all_equal == true %} + // all sim_method_num_steps are identical + int sim_method_num_steps = {{ solver_options.sim_method_num_steps[0] }}; + for (int i = 0; i < N; i++) + ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_num_steps", &sim_method_num_steps); + {%- else %} + // sim_method_num_steps are different + int* sim_method_num_steps = malloc(N*sizeof(int)); + {%- for j in range(end=dims.N) %} + sim_method_num_steps[{{ j }}] = {{ solver_options.sim_method_num_steps[j] }}; + {%- endfor %} + + for (int i = 0; i < N; i++) + ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_num_steps", &sim_method_num_steps[i]); + free(sim_method_num_steps); + {%- endif %} + + // set up sim_method_num_stages + {%- set all_equal = true %} + {%- set val = solver_options.sim_method_num_stages[0] %} + {%- for j in range(start=1, end=dims.N) %} + {%- if val != solver_options.sim_method_num_stages[j] %} + {%- set_global all_equal = false %} + {%- break %} + {%- endif %} + {%- endfor %} + + {%- if all_equal == true %} + // all sim_method_num_stages are identical + int sim_method_num_stages = {{ solver_options.sim_method_num_stages[0] }}; + for (int i = 0; i < N; i++) + ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_num_stages", &sim_method_num_stages); + {%- else %} + int* sim_method_num_stages = malloc(N*sizeof(int)); + {%- for j in range(end=dims.N) %} + sim_method_num_stages[{{ j }}] = {{ solver_options.sim_method_num_stages[j] }}; + {%- endfor %} + + for (int i = 0; i < N; i++) + ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_num_stages", &sim_method_num_stages[i]); + free(sim_method_num_stages); + {%- endif %} + + int newton_iter_val = {{ solver_options.sim_method_newton_iter }}; + for (int i = 0; i < N; i++) + ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_newton_iter", &newton_iter_val); + + + // set up sim_method_jac_reuse + {%- set all_equal = true %} + {%- set val = solver_options.sim_method_jac_reuse[0] %} + {%- for j in range(start=1, end=dims.N) %} + {%- if val != solver_options.sim_method_jac_reuse[j] %} + {%- set_global all_equal = false %} + {%- break %} + {%- endif %} + {%- endfor %} + {%- if all_equal == true %} + bool tmp_bool = (bool) {{ solver_options.sim_method_jac_reuse[0] }}; + for (int i = 0; i < N; i++) + ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_jac_reuse", &tmp_bool); + {%- else %} + bool* sim_method_jac_reuse = malloc(N*sizeof(bool)); + {%- for j in range(end=dims.N) %} + sim_method_jac_reuse[{{ j }}] = (bool){{ solver_options.sim_method_jac_reuse[j] }}; + {%- endfor %} + + for (int i = 0; i < N; i++) + ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_jac_reuse", &sim_method_jac_reuse[i]); + free(sim_method_jac_reuse); + {%- endif %} + +{%- endif %} + + double nlp_solver_step_length = {{ solver_options.nlp_solver_step_length }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "step_length", &nlp_solver_step_length); + + {%- if solver_options.nlp_solver_warm_start_first_qp %} + int nlp_solver_warm_start_first_qp = {{ solver_options.nlp_solver_warm_start_first_qp }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "warm_start_first_qp", &nlp_solver_warm_start_first_qp); + {%- endif %} + + double levenberg_marquardt = {{ solver_options.levenberg_marquardt }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "levenberg_marquardt", &levenberg_marquardt); + + /* options QP solver */ +{%- if solver_options.qp_solver is starting_with("PARTIAL_CONDENSING") %} + int qp_solver_cond_N; + + {% if solver_options.qp_solver_cond_N -%} + const int qp_solver_cond_N_ori = {{ solver_options.qp_solver_cond_N }}; + qp_solver_cond_N = N < qp_solver_cond_N_ori ? N : qp_solver_cond_N_ori; // use the minimum value here + {%- else %} + // NOTE: there is no condensing happening here! + qp_solver_cond_N = N; + {%- endif %} + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_cond_N", &qp_solver_cond_N); +{%- endif %} + + +{%- if solver_options.qp_solver is containing("HPIPM") %} + // set HPIPM mode: should be done before setting other QP solver options + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_hpipm_mode", "{{ solver_options.hpipm_mode }}"); +{%- endif %} + +{% if solver_options.nlp_solver_type == "SQP" %} + // set SQP specific options + double nlp_solver_tol_stat = {{ solver_options.nlp_solver_tol_stat }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_stat", &nlp_solver_tol_stat); + + double nlp_solver_tol_eq = {{ solver_options.nlp_solver_tol_eq }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_eq", &nlp_solver_tol_eq); + + double nlp_solver_tol_ineq = {{ solver_options.nlp_solver_tol_ineq }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_ineq", &nlp_solver_tol_ineq); + + double nlp_solver_tol_comp = {{ solver_options.nlp_solver_tol_comp }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_comp", &nlp_solver_tol_comp); + + int nlp_solver_max_iter = {{ solver_options.nlp_solver_max_iter }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "max_iter", &nlp_solver_max_iter); + + int initialize_t_slacks = {{ solver_options.initialize_t_slacks }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "initialize_t_slacks", &initialize_t_slacks); +{%- endif %} + + int qp_solver_iter_max = {{ solver_options.qp_solver_iter_max }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_iter_max", &qp_solver_iter_max); + +{# NOTE: qp_solver tolerances must be set after NLP ones, since the setter for NLP tolerances sets the QP tolerances to the sam values. #} + {%- if solver_options.qp_solver_tol_stat %} + double qp_solver_tol_stat = {{ solver_options.qp_solver_tol_stat }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_tol_stat", &qp_solver_tol_stat); + {%- endif -%} + + {%- if solver_options.qp_solver_tol_eq %} + double qp_solver_tol_eq = {{ solver_options.qp_solver_tol_eq }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_tol_eq", &qp_solver_tol_eq); + {%- endif -%} + + {%- if solver_options.qp_solver_tol_ineq %} + double qp_solver_tol_ineq = {{ solver_options.qp_solver_tol_ineq }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_tol_ineq", &qp_solver_tol_ineq); + {%- endif -%} + + {%- if solver_options.qp_solver_tol_comp %} + double qp_solver_tol_comp = {{ solver_options.qp_solver_tol_comp }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_tol_comp", &qp_solver_tol_comp); + {%- endif -%} + + {%- if solver_options.qp_solver_warm_start %} + int qp_solver_warm_start = {{ solver_options.qp_solver_warm_start }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_warm_start", &qp_solver_warm_start); + {%- endif -%} + + int print_level = {{ solver_options.print_level }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "print_level", &print_level); + + + int ext_cost_num_hess = {{ solver_options.ext_cost_num_hess }}; +{%- if cost.cost_type == "EXTERNAL" %} + for (int i = 0; i < N; i++) + { + ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "cost_numerical_hessian", &ext_cost_num_hess); + } +{%- endif %} +{%- if cost.cost_type_e == "EXTERNAL" %} + ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, N, "cost_numerical_hessian", &ext_cost_num_hess); +{%- endif %} +} + + +/** + * Internal function for {{ model.name }}_acados_create: step 7 + */ +void {{ model.name }}_acados_create_7_set_nlp_out({{ model.name }}_solver_capsule* capsule) +{ + const int N = capsule->nlp_solver_plan->N; + ocp_nlp_config* nlp_config = capsule->nlp_config; + ocp_nlp_dims* nlp_dims = capsule->nlp_dims; + ocp_nlp_out* nlp_out = capsule->nlp_out; + + // initialize primal solution + double* xu0 = calloc(NX+NU, sizeof(double)); + double* x0 = xu0; +{% if dims.nbx_0 == dims.nx %} + // initialize with x0 + {% for item in constraints.lbx_0 %} + {%- if item != 0 %} + x0[{{ loop.index0 }}] = {{ item }}; + {%- endif %} + {%- endfor %} +{% else %} + // initialize with zeros +{%- endif %} + + double* u0 = xu0 + NX; + + for (int i = 0; i < N; i++) + { + // x0 + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "x", x0); + // u0 + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "u", u0); + } + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, N, "x", x0); + free(xu0); +} + + +/** + * Internal function for {{ model.name }}_acados_create: step 8 + */ +//void {{ model.name }}_acados_create_8_create_solver({{ model.name }}_solver_capsule* capsule) +//{ +// capsule->nlp_solver = ocp_nlp_solver_create(capsule->nlp_config, capsule->nlp_dims, capsule->nlp_opts); +//} + +/** + * Internal function for {{ model.name }}_acados_create: step 9 + */ +int {{ model.name }}_acados_create_9_precompute({{ model.name }}_solver_capsule* capsule) { + int status = ocp_nlp_precompute(capsule->nlp_solver, capsule->nlp_in, capsule->nlp_out); + + if (status != ACADOS_SUCCESS) { + printf("\nocp_nlp_precompute failed!\n\n"); + exit(1); + } + + return status; +} + + +int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_capsule* capsule, int N, double* new_time_steps) +{ + // If N does not match the number of shooting intervals used for code generation, new_time_steps must be given. + if (N != {{ model.name | upper }}_N && !new_time_steps) { + fprintf(stderr, "{{ model.name }}_acados_create_with_discretization: new_time_steps is NULL " \ + "but the number of shooting intervals (= %d) differs from the number of " \ + "shooting intervals (= %d) during code generation! Please provide a new vector of time_stamps!\n", \ + N, {{ model.name | upper }}_N); + return 1; + } + + // number of expected runtime parameters + capsule->nlp_np = NP; + + // 1) create and set nlp_solver_plan; create nlp_config + capsule->nlp_solver_plan = ocp_nlp_plan_create(N); + {{ model.name }}_acados_create_1_set_plan(capsule->nlp_solver_plan, N); + capsule->nlp_config = ocp_nlp_config_create(*capsule->nlp_solver_plan); + + // 3) create and set dimensions + capsule->nlp_dims = {{ model.name }}_acados_create_2_create_and_set_dimensions(capsule); + {{ model.name }}_acados_create_3_create_and_set_functions(capsule); + + // 4) set default parameters in functions + {{ model.name }}_acados_create_4_set_default_parameters(capsule); + + // 5) create and set nlp_in + capsule->nlp_in = ocp_nlp_in_create(capsule->nlp_config, capsule->nlp_dims); + {{ model.name }}_acados_create_5_set_nlp_in(capsule, N, new_time_steps); + + // 6) create and set nlp_opts + capsule->nlp_opts = ocp_nlp_solver_opts_create(capsule->nlp_config, capsule->nlp_dims); + {{ model.name }}_acados_create_6_set_opts(capsule); + + // 7) create and set nlp_out + // 7.1) nlp_out + capsule->nlp_out = ocp_nlp_out_create(capsule->nlp_config, capsule->nlp_dims); + // 7.2) sens_out + capsule->sens_out = ocp_nlp_out_create(capsule->nlp_config, capsule->nlp_dims); + {{ model.name }}_acados_create_7_set_nlp_out(capsule); + + // 8) create solver + capsule->nlp_solver = ocp_nlp_solver_create(capsule->nlp_config, capsule->nlp_dims, capsule->nlp_opts); + //{{ model.name }}_acados_create_8_create_solver(capsule); + + // 9) do precomputations + int status = {{ model.name }}_acados_create_9_precompute(capsule); + return status; +} + +/** + * This function is for updating an already initialized solver with a different number of qp_cond_N. It is useful for code reuse after code export. + */ +int {{ model.name }}_acados_update_qp_solver_cond_N({{ model.name }}_solver_capsule* capsule, int qp_solver_cond_N) +{ +{%- if solver_options.qp_solver is starting_with("PARTIAL_CONDENSING") %} + // 1) destroy solver + ocp_nlp_solver_destroy(capsule->nlp_solver); + + // 2) set new value for "qp_cond_N" + const int N = capsule->nlp_solver_plan->N; + if(qp_solver_cond_N > N) + printf("Warning: qp_solver_cond_N = %d > N = %d\n", qp_solver_cond_N, N); + ocp_nlp_solver_opts_set(capsule->nlp_config, capsule->nlp_opts, "qp_cond_N", &qp_solver_cond_N); + + // 3) continue with the remaining steps from {{ model.name }}_acados_create_with_discretization(...): + // -> 8) create solver + capsule->nlp_solver = ocp_nlp_solver_create(capsule->nlp_config, capsule->nlp_dims, capsule->nlp_opts); + + // -> 9) do precomputations + int status = {{ model.name }}_acados_create_9_precompute(capsule); + return status; +{%- else %} + printf("\nacados_update_qp_solver_cond_N() failed, since no partial condensing solver is used!\n\n"); + // Todo: what is an adequate behavior here? + exit(1); + return -1; +{%- endif %} +} + + +int {{ model.name }}_acados_reset({{ model.name }}_solver_capsule* capsule) +{ + + // set initialization to all zeros +{# TODO: use guess values / initial state value from json instead?! #} + const int N = capsule->nlp_solver_plan->N; + ocp_nlp_config* nlp_config = capsule->nlp_config; + ocp_nlp_dims* nlp_dims = capsule->nlp_dims; + ocp_nlp_out* nlp_out = capsule->nlp_out; + ocp_nlp_in* nlp_in = capsule->nlp_in; + ocp_nlp_solver* nlp_solver = capsule->nlp_solver; + + int nx, nu, nv, ns, nz, ni, dim; + + double* buffer = calloc(NX+NU+NZ+2*NS+2*NSN+NBX+NBU+NG+NH+NPHI+NBX0+NBXN+NHN+NPHIN+NGN, sizeof(double)); + + for(int i=0; i reset memory + int qp_status; + ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "qp_status", &qp_status); + if (qp_status == 3) + { + // printf("\nin reset qp_status %d -> resetting QP memory\n", qp_status); + ocp_nlp_solver_reset_qp_memory(nlp_solver, nlp_in, nlp_out); + } +{%- endif %} + + free(buffer); + return 0; +} + + + + +int {{ model.name }}_acados_update_params({{ model.name }}_solver_capsule* capsule, int stage, double *p, int np) +{ + int solver_status = 0; + + int casadi_np = {{ dims.np }}; + if (casadi_np != np) { + printf("acados_update_params: trying to set %i parameters for external functions." + " External function has %i parameters. Exiting.\n", np, casadi_np); + exit(1); + } + +{%- if dims.np > 0 %} + const int N = capsule->nlp_solver_plan->N; + if (stage < N && stage >= 0) + { + {%- if solver_options.integrator_type == "IRK" %} + capsule->impl_dae_fun[stage].set_param(capsule->impl_dae_fun+stage, p); + capsule->impl_dae_fun_jac_x_xdot_z[stage].set_param(capsule->impl_dae_fun_jac_x_xdot_z+stage, p); + capsule->impl_dae_jac_x_xdot_u_z[stage].set_param(capsule->impl_dae_jac_x_xdot_u_z+stage, p); + + {%- if solver_options.hessian_approx == "EXACT" %} + capsule->impl_dae_hess[stage].set_param(capsule->impl_dae_hess+stage, p); + {%- endif %} + {% elif solver_options.integrator_type == "LIFTED_IRK" %} + capsule->impl_dae_fun[stage].set_param(capsule->impl_dae_fun+stage, p); + capsule->impl_dae_fun_jac_x_xdot_u[stage].set_param(capsule->impl_dae_fun_jac_x_xdot_u+stage, p); + {% elif solver_options.integrator_type == "ERK" %} + capsule->forw_vde_casadi[stage].set_param(capsule->forw_vde_casadi+stage, p); + capsule->expl_ode_fun[stage].set_param(capsule->expl_ode_fun+stage, p); + + {%- if solver_options.hessian_approx == "EXACT" %} + capsule->hess_vde_casadi[stage].set_param(capsule->hess_vde_casadi+stage, p); + {%- endif %} + {% elif solver_options.integrator_type == "GNSF" %} + {% if model.gnsf.purely_linear != 1 %} + capsule->gnsf_phi_fun[stage].set_param(capsule->gnsf_phi_fun+stage, p); + capsule->gnsf_phi_fun_jac_y[stage].set_param(capsule->gnsf_phi_fun_jac_y+stage, p); + capsule->gnsf_phi_jac_y_uhat[stage].set_param(capsule->gnsf_phi_jac_y_uhat+stage, p); + {% if model.gnsf.nontrivial_f_LO == 1 %} + capsule->gnsf_f_lo_jac_x1_x1dot_u_z[stage].set_param(capsule->gnsf_f_lo_jac_x1_x1dot_u_z+stage, p); + {%- endif %} + {%- endif %} + {% elif solver_options.integrator_type == "DISCRETE" %} + capsule->discr_dyn_phi_fun[stage].set_param(capsule->discr_dyn_phi_fun+stage, p); + capsule->discr_dyn_phi_fun_jac_ut_xt[stage].set_param(capsule->discr_dyn_phi_fun_jac_ut_xt+stage, p); + {%- if solver_options.hessian_approx == "EXACT" %} + capsule->discr_dyn_phi_fun_jac_ut_xt_hess[stage].set_param(capsule->discr_dyn_phi_fun_jac_ut_xt_hess+stage, p); + {% endif %} + {%- endif %}{# integrator_type #} + + // constraints + {% if constraints.constr_type == "BGP" %} + capsule->phi_constraint[stage].set_param(capsule->phi_constraint+stage, p); + {% elif constraints.constr_type == "BGH" and dims.nh > 0 %} + capsule->nl_constr_h_fun_jac[stage].set_param(capsule->nl_constr_h_fun_jac+stage, p); + capsule->nl_constr_h_fun[stage].set_param(capsule->nl_constr_h_fun+stage, p); + {%- if solver_options.hessian_approx == "EXACT" %} + capsule->nl_constr_h_fun_jac_hess[stage].set_param(capsule->nl_constr_h_fun_jac_hess+stage, p); + {%- endif %} + {%- endif %} + + // cost + if (stage == 0) + { + {%- if cost.cost_type_0 == "NONLINEAR_LS" %} + capsule->cost_y_0_fun.set_param(&capsule->cost_y_0_fun, p); + capsule->cost_y_0_fun_jac_ut_xt.set_param(&capsule->cost_y_0_fun_jac_ut_xt, p); + capsule->cost_y_0_hess.set_param(&capsule->cost_y_0_hess, p); + {%- elif cost.cost_type_0 == "EXTERNAL" %} + capsule->ext_cost_0_fun.set_param(&capsule->ext_cost_0_fun, p); + capsule->ext_cost_0_fun_jac.set_param(&capsule->ext_cost_0_fun_jac, p); + capsule->ext_cost_0_fun_jac_hess.set_param(&capsule->ext_cost_0_fun_jac_hess, p); + {% endif %} + } + else // 0 < stage < N + { + {%- if cost.cost_type == "NONLINEAR_LS" %} + capsule->cost_y_fun[stage-1].set_param(capsule->cost_y_fun+stage-1, p); + capsule->cost_y_fun_jac_ut_xt[stage-1].set_param(capsule->cost_y_fun_jac_ut_xt+stage-1, p); + capsule->cost_y_hess[stage-1].set_param(capsule->cost_y_hess+stage-1, p); + {%- elif cost.cost_type == "EXTERNAL" %} + capsule->ext_cost_fun[stage-1].set_param(capsule->ext_cost_fun+stage-1, p); + capsule->ext_cost_fun_jac[stage-1].set_param(capsule->ext_cost_fun_jac+stage-1, p); + capsule->ext_cost_fun_jac_hess[stage-1].set_param(capsule->ext_cost_fun_jac_hess+stage-1, p); + {%- endif %} + } + } + + else // stage == N + { + // terminal shooting node has no dynamics + // cost + {%- if cost.cost_type_e == "NONLINEAR_LS" %} + capsule->cost_y_e_fun.set_param(&capsule->cost_y_e_fun, p); + capsule->cost_y_e_fun_jac_ut_xt.set_param(&capsule->cost_y_e_fun_jac_ut_xt, p); + capsule->cost_y_e_hess.set_param(&capsule->cost_y_e_hess, p); + {%- elif cost.cost_type_e == "EXTERNAL" %} + capsule->ext_cost_e_fun.set_param(&capsule->ext_cost_e_fun, p); + capsule->ext_cost_e_fun_jac.set_param(&capsule->ext_cost_e_fun_jac, p); + capsule->ext_cost_e_fun_jac_hess.set_param(&capsule->ext_cost_e_fun_jac_hess, p); + {% endif %} + // constraints + {% if constraints.constr_type_e == "BGP" %} + capsule->phi_e_constraint.set_param(&capsule->phi_e_constraint, p); + {% elif constraints.constr_type_e == "BGH" and dims.nh_e > 0 %} + capsule->nl_constr_h_e_fun_jac.set_param(&capsule->nl_constr_h_e_fun_jac, p); + capsule->nl_constr_h_e_fun.set_param(&capsule->nl_constr_h_e_fun, p); + {%- if solver_options.hessian_approx == "EXACT" %} + capsule->nl_constr_h_e_fun_jac_hess.set_param(&capsule->nl_constr_h_e_fun_jac_hess, p); + {%- endif %} + {% endif %} + } +{% endif %}{# if dims.np #} + + return solver_status; +} + + + +int {{ model.name }}_acados_solve({{ model.name }}_solver_capsule* capsule) +{ + // solve NLP + int solver_status = ocp_nlp_solve(capsule->nlp_solver, capsule->nlp_in, capsule->nlp_out); + + return solver_status; +} + + +int {{ model.name }}_acados_free({{ model.name }}_solver_capsule* capsule) +{ + // before destroying, keep some info + const int N = capsule->nlp_solver_plan->N; + // free memory + ocp_nlp_solver_opts_destroy(capsule->nlp_opts); + ocp_nlp_in_destroy(capsule->nlp_in); + ocp_nlp_out_destroy(capsule->nlp_out); + ocp_nlp_out_destroy(capsule->sens_out); + ocp_nlp_solver_destroy(capsule->nlp_solver); + ocp_nlp_dims_destroy(capsule->nlp_dims); + ocp_nlp_config_destroy(capsule->nlp_config); + ocp_nlp_plan_destroy(capsule->nlp_solver_plan); + + /* free external function */ + // dynamics +{%- if solver_options.integrator_type == "IRK" %} + for (int i = 0; i < N; i++) + { + external_function_param_casadi_free(&capsule->impl_dae_fun[i]); + external_function_param_casadi_free(&capsule->impl_dae_fun_jac_x_xdot_z[i]); + external_function_param_casadi_free(&capsule->impl_dae_jac_x_xdot_u_z[i]); + {%- if solver_options.hessian_approx == "EXACT" %} + external_function_param_casadi_free(&capsule->impl_dae_hess[i]); + {%- endif %} + } + free(capsule->impl_dae_fun); + free(capsule->impl_dae_fun_jac_x_xdot_z); + free(capsule->impl_dae_jac_x_xdot_u_z); + {%- if solver_options.hessian_approx == "EXACT" %} + free(capsule->impl_dae_hess); + {%- endif %} + +{%- elif solver_options.integrator_type == "LIFTED_IRK" %} + for (int i = 0; i < N; i++) + { + external_function_param_casadi_free(&capsule->impl_dae_fun[i]); + external_function_param_casadi_free(&capsule->impl_dae_fun_jac_x_xdot_u[i]); + } + free(capsule->impl_dae_fun); + free(capsule->impl_dae_fun_jac_x_xdot_u); + +{%- elif solver_options.integrator_type == "ERK" %} + for (int i = 0; i < N; i++) + { + external_function_param_casadi_free(&capsule->forw_vde_casadi[i]); + external_function_param_casadi_free(&capsule->expl_ode_fun[i]); + {%- if solver_options.hessian_approx == "EXACT" %} + external_function_param_casadi_free(&capsule->hess_vde_casadi[i]); + {%- endif %} + } + free(capsule->forw_vde_casadi); + free(capsule->expl_ode_fun); + {%- if solver_options.hessian_approx == "EXACT" %} + free(capsule->hess_vde_casadi); + {%- endif %} + +{%- elif solver_options.integrator_type == "GNSF" %} + for (int i = 0; i < N; i++) + { + {% if model.gnsf.purely_linear != 1 %} + external_function_param_casadi_free(&capsule->gnsf_phi_fun[i]); + external_function_param_casadi_free(&capsule->gnsf_phi_fun_jac_y[i]); + external_function_param_casadi_free(&capsule->gnsf_phi_jac_y_uhat[i]); + {% if model.gnsf.nontrivial_f_LO == 1 %} + external_function_param_casadi_free(&capsule->gnsf_f_lo_jac_x1_x1dot_u_z[i]); + {%- endif %} + {%- endif %} + external_function_param_casadi_free(&capsule->gnsf_get_matrices_fun[i]); + } + {% if model.gnsf.purely_linear != 1 %} + free(capsule->gnsf_phi_fun); + free(capsule->gnsf_phi_fun_jac_y); + free(capsule->gnsf_phi_jac_y_uhat); + {% if model.gnsf.nontrivial_f_LO == 1 %} + free(capsule->gnsf_f_lo_jac_x1_x1dot_u_z); + {%- endif %} + {%- endif %} + free(capsule->gnsf_get_matrices_fun); +{%- elif solver_options.integrator_type == "DISCRETE" %} + for (int i = 0; i < N; i++) + { + external_function_param_{{ model.dyn_ext_fun_type }}_free(&capsule->discr_dyn_phi_fun[i]); + external_function_param_{{ model.dyn_ext_fun_type }}_free(&capsule->discr_dyn_phi_fun_jac_ut_xt[i]); + {%- if solver_options.hessian_approx == "EXACT" %} + external_function_param_{{ model.dyn_ext_fun_type }}_free(&capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i]); + {%- endif %} + } + free(capsule->discr_dyn_phi_fun); + free(capsule->discr_dyn_phi_fun_jac_ut_xt); + {%- if solver_options.hessian_approx == "EXACT" %} + free(capsule->discr_dyn_phi_fun_jac_ut_xt_hess); + {%- endif %} + +{%- endif %} + + // cost +{%- if cost.cost_type_0 == "NONLINEAR_LS" %} + external_function_param_casadi_free(&capsule->cost_y_0_fun); + external_function_param_casadi_free(&capsule->cost_y_0_fun_jac_ut_xt); + external_function_param_casadi_free(&capsule->cost_y_0_hess); +{%- elif cost.cost_type_0 == "EXTERNAL" %} + external_function_param_{{ cost.cost_ext_fun_type_0 }}_free(&capsule->ext_cost_0_fun); + external_function_param_{{ cost.cost_ext_fun_type_0 }}_free(&capsule->ext_cost_0_fun_jac); + external_function_param_{{ cost.cost_ext_fun_type_0 }}_free(&capsule->ext_cost_0_fun_jac_hess); +{%- endif %} +{%- if cost.cost_type == "NONLINEAR_LS" %} + for (int i = 0; i < N - 1; i++) + { + external_function_param_casadi_free(&capsule->cost_y_fun[i]); + external_function_param_casadi_free(&capsule->cost_y_fun_jac_ut_xt[i]); + external_function_param_casadi_free(&capsule->cost_y_hess[i]); + } + free(capsule->cost_y_fun); + free(capsule->cost_y_fun_jac_ut_xt); + free(capsule->cost_y_hess); +{%- elif cost.cost_type == "EXTERNAL" %} + for (int i = 0; i < N - 1; i++) + { + external_function_param_{{ cost.cost_ext_fun_type }}_free(&capsule->ext_cost_fun[i]); + external_function_param_{{ cost.cost_ext_fun_type }}_free(&capsule->ext_cost_fun_jac[i]); + external_function_param_{{ cost.cost_ext_fun_type }}_free(&capsule->ext_cost_fun_jac_hess[i]); + } + free(capsule->ext_cost_fun); + free(capsule->ext_cost_fun_jac); + free(capsule->ext_cost_fun_jac_hess); +{%- endif %} +{%- if cost.cost_type_e == "NONLINEAR_LS" %} + external_function_param_casadi_free(&capsule->cost_y_e_fun); + external_function_param_casadi_free(&capsule->cost_y_e_fun_jac_ut_xt); + external_function_param_casadi_free(&capsule->cost_y_e_hess); +{%- elif cost.cost_type_e == "EXTERNAL" %} + external_function_param_{{ cost.cost_ext_fun_type_e }}_free(&capsule->ext_cost_e_fun); + external_function_param_{{ cost.cost_ext_fun_type_e }}_free(&capsule->ext_cost_e_fun_jac); + external_function_param_{{ cost.cost_ext_fun_type_e }}_free(&capsule->ext_cost_e_fun_jac_hess); +{%- endif %} + + // constraints +{%- if constraints.constr_type == "BGH" and dims.nh > 0 %} + for (int i = 0; i < N; i++) + { + external_function_param_casadi_free(&capsule->nl_constr_h_fun_jac[i]); + external_function_param_casadi_free(&capsule->nl_constr_h_fun[i]); + } + {%- if solver_options.hessian_approx == "EXACT" %} + for (int i = 0; i < N; i++) + { + external_function_param_casadi_free(&capsule->nl_constr_h_fun_jac_hess[i]); + } + {%- endif %} + free(capsule->nl_constr_h_fun_jac); + free(capsule->nl_constr_h_fun); + {%- if solver_options.hessian_approx == "EXACT" %} + free(capsule->nl_constr_h_fun_jac_hess); + {%- endif %} + +{%- elif constraints.constr_type == "BGP" and dims.nphi > 0 %} + for (int i = 0; i < N; i++) + { + external_function_param_casadi_free(&capsule->phi_constraint[i]); + } + free(capsule->phi_constraint); +{%- endif %} + +{%- if constraints.constr_type_e == "BGH" and dims.nh_e > 0 %} + external_function_param_casadi_free(&capsule->nl_constr_h_e_fun_jac); + external_function_param_casadi_free(&capsule->nl_constr_h_e_fun); +{%- if solver_options.hessian_approx == "EXACT" %} + external_function_param_casadi_free(&capsule->nl_constr_h_e_fun_jac_hess); +{%- endif %} +{%- elif constraints.constr_type_e == "BGP" and dims.nphi_e > 0 %} + external_function_param_casadi_free(&capsule->phi_e_constraint); +{%- endif %} + + return 0; +} + +ocp_nlp_in *{{ model.name }}_acados_get_nlp_in({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_in; } +ocp_nlp_out *{{ model.name }}_acados_get_nlp_out({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_out; } +ocp_nlp_out *{{ model.name }}_acados_get_sens_out({{ model.name }}_solver_capsule* capsule) { return capsule->sens_out; } +ocp_nlp_solver *{{ model.name }}_acados_get_nlp_solver({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_solver; } +ocp_nlp_config *{{ model.name }}_acados_get_nlp_config({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_config; } +void *{{ model.name }}_acados_get_nlp_opts({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_opts; } +ocp_nlp_dims *{{ model.name }}_acados_get_nlp_dims({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_dims; } +ocp_nlp_plan_t *{{ model.name }}_acados_get_nlp_plan({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_solver_plan; } + + +void {{ model.name }}_acados_print_stats({{ model.name }}_solver_capsule* capsule) +{ + int sqp_iter, stat_m, stat_n, tmp_int; + ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "sqp_iter", &sqp_iter); + ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "stat_n", &stat_n); + ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "stat_m", &stat_m); + + {% set stat_n_max = 12 %} + double stat[{{ solver_options.nlp_solver_max_iter * stat_n_max }}]; + ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "statistics", stat); + + int nrow = sqp_iter+1 < stat_m ? sqp_iter+1 : stat_m; + +{%- if solver_options.nlp_solver_type == "SQP" %} + printf("iter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter\talpha\n"); + for (int i = 0; i < nrow; i++) + { + for (int j = 0; j < stat_n + 1; j++) + { + if (j == 0 || j == 5 || j == 6) + { + tmp_int = (int) stat[i + j * nrow]; + printf("%d\t", tmp_int); + } + else + { + printf("%e\t", stat[i + j * nrow]); + } + } + printf("\n"); + } +{% else %} + printf("iter\tqp_stat\tqp_iter\n"); + for (int i = 0; i < nrow; i++) + { + for (int j = 0; j < stat_n + 1; j++) + { + tmp_int = (int) stat[i + j * nrow]; + printf("%d\t", tmp_int); + } + printf("\n"); + } +{%- endif %} +} + diff --git a/third_party/acados/acados_template/c_templates_tera/acados_solver.in.h b/third_party/acados/acados_template/c_templates_tera/acados_solver.in.h new file mode 100644 index 0000000000..1c82ef3ba0 --- /dev/null +++ b/third_party/acados/acados_template/c_templates_tera/acados_solver.in.h @@ -0,0 +1,217 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + +#ifndef ACADOS_SOLVER_{{ model.name }}_H_ +#define ACADOS_SOLVER_{{ model.name }}_H_ + +#include "acados/utils/types.h" + +#include "acados_c/ocp_nlp_interface.h" +#include "acados_c/external_function_interface.h" + +#define {{ model.name | upper }}_NX {{ dims.nx }} +#define {{ model.name | upper }}_NZ {{ dims.nz }} +#define {{ model.name | upper }}_NU {{ dims.nu }} +#define {{ model.name | upper }}_NP {{ dims.np }} +#define {{ model.name | upper }}_NBX {{ dims.nbx }} +#define {{ model.name | upper }}_NBX0 {{ dims.nbx_0 }} +#define {{ model.name | upper }}_NBU {{ dims.nbu }} +#define {{ model.name | upper }}_NSBX {{ dims.nsbx }} +#define {{ model.name | upper }}_NSBU {{ dims.nsbu }} +#define {{ model.name | upper }}_NSH {{ dims.nsh }} +#define {{ model.name | upper }}_NSG {{ dims.nsg }} +#define {{ model.name | upper }}_NSPHI {{ dims.nsphi }} +#define {{ model.name | upper }}_NSHN {{ dims.nsh_e }} +#define {{ model.name | upper }}_NSGN {{ dims.nsg_e }} +#define {{ model.name | upper }}_NSPHIN {{ dims.nsphi_e }} +#define {{ model.name | upper }}_NSBXN {{ dims.nsbx_e }} +#define {{ model.name | upper }}_NS {{ dims.ns }} +#define {{ model.name | upper }}_NSN {{ dims.ns_e }} +#define {{ model.name | upper }}_NG {{ dims.ng }} +#define {{ model.name | upper }}_NBXN {{ dims.nbx_e }} +#define {{ model.name | upper }}_NGN {{ dims.ng_e }} +#define {{ model.name | upper }}_NY0 {{ dims.ny_0 }} +#define {{ model.name | upper }}_NY {{ dims.ny }} +#define {{ model.name | upper }}_NYN {{ dims.ny_e }} +#define {{ model.name | upper }}_N {{ dims.N }} +#define {{ model.name | upper }}_NH {{ dims.nh }} +#define {{ model.name | upper }}_NPHI {{ dims.nphi }} +#define {{ model.name | upper }}_NHN {{ dims.nh_e }} +#define {{ model.name | upper }}_NPHIN {{ dims.nphi_e }} +#define {{ model.name | upper }}_NR {{ dims.nr }} + +#ifdef __cplusplus +extern "C" { +#endif + +// ** capsule for solver data ** +typedef struct {{ model.name }}_solver_capsule +{ + // acados objects + ocp_nlp_in *nlp_in; + ocp_nlp_out *nlp_out; + ocp_nlp_out *sens_out; + ocp_nlp_solver *nlp_solver; + void *nlp_opts; + ocp_nlp_plan_t *nlp_solver_plan; + ocp_nlp_config *nlp_config; + ocp_nlp_dims *nlp_dims; + + // number of expected runtime parameters + unsigned int nlp_np; + + /* external functions */ + // dynamics +{% if solver_options.integrator_type == "ERK" %} + external_function_param_casadi *forw_vde_casadi; + external_function_param_casadi *expl_ode_fun; +{% if solver_options.hessian_approx == "EXACT" %} + external_function_param_casadi *hess_vde_casadi; +{%- endif %} +{% elif solver_options.integrator_type == "IRK" %} + external_function_param_casadi *impl_dae_fun; + external_function_param_casadi *impl_dae_fun_jac_x_xdot_z; + external_function_param_casadi *impl_dae_jac_x_xdot_u_z; +{% if solver_options.hessian_approx == "EXACT" %} + external_function_param_casadi *impl_dae_hess; +{%- endif %} +{% elif solver_options.integrator_type == "LIFTED_IRK" %} + external_function_param_casadi *impl_dae_fun; + external_function_param_casadi *impl_dae_fun_jac_x_xdot_u; +{% elif solver_options.integrator_type == "GNSF" %} + external_function_param_casadi *gnsf_phi_fun; + external_function_param_casadi *gnsf_phi_fun_jac_y; + external_function_param_casadi *gnsf_phi_jac_y_uhat; + external_function_param_casadi *gnsf_f_lo_jac_x1_x1dot_u_z; + external_function_param_casadi *gnsf_get_matrices_fun; +{% elif solver_options.integrator_type == "DISCRETE" %} + external_function_param_{{ model.dyn_ext_fun_type }} *discr_dyn_phi_fun; + external_function_param_{{ model.dyn_ext_fun_type }} *discr_dyn_phi_fun_jac_ut_xt; +{%- if solver_options.hessian_approx == "EXACT" %} + external_function_param_{{ model.dyn_ext_fun_type }} *discr_dyn_phi_fun_jac_ut_xt_hess; +{%- endif %} +{%- endif %} + + + // cost +{% if cost.cost_type == "NONLINEAR_LS" %} + external_function_param_casadi *cost_y_fun; + external_function_param_casadi *cost_y_fun_jac_ut_xt; + external_function_param_casadi *cost_y_hess; +{%- elif cost.cost_type == "EXTERNAL" %} + external_function_param_{{ cost.cost_ext_fun_type }} *ext_cost_fun; + external_function_param_{{ cost.cost_ext_fun_type }} *ext_cost_fun_jac; + external_function_param_{{ cost.cost_ext_fun_type }} *ext_cost_fun_jac_hess; +{% endif %} + +{% if cost.cost_type_0 == "NONLINEAR_LS" %} + external_function_param_casadi cost_y_0_fun; + external_function_param_casadi cost_y_0_fun_jac_ut_xt; + external_function_param_casadi cost_y_0_hess; +{% elif cost.cost_type_0 == "EXTERNAL" %} + external_function_param_{{ cost.cost_ext_fun_type_0 }} ext_cost_0_fun; + external_function_param_{{ cost.cost_ext_fun_type_0 }} ext_cost_0_fun_jac; + external_function_param_{{ cost.cost_ext_fun_type_0 }} ext_cost_0_fun_jac_hess; +{%- endif %} + +{% if cost.cost_type_e == "NONLINEAR_LS" %} + external_function_param_casadi cost_y_e_fun; + external_function_param_casadi cost_y_e_fun_jac_ut_xt; + external_function_param_casadi cost_y_e_hess; +{% elif cost.cost_type_e == "EXTERNAL" %} + external_function_param_{{ cost.cost_ext_fun_type_e }} ext_cost_e_fun; + external_function_param_{{ cost.cost_ext_fun_type_e }} ext_cost_e_fun_jac; + external_function_param_{{ cost.cost_ext_fun_type_e }} ext_cost_e_fun_jac_hess; +{%- endif %} + + // constraints +{%- if constraints.constr_type == "BGP" %} + external_function_param_casadi *phi_constraint; +{% elif constraints.constr_type == "BGH" and dims.nh > 0 %} + external_function_param_casadi *nl_constr_h_fun_jac; + external_function_param_casadi *nl_constr_h_fun; + external_function_param_casadi *nl_constr_h_fun_jac_hess; +{%- endif %} + + +{% if constraints.constr_type_e == "BGP" %} + external_function_param_casadi phi_e_constraint; +{% elif constraints.constr_type_e == "BGH" and dims.nh_e > 0 %} + external_function_param_casadi nl_constr_h_e_fun_jac; + external_function_param_casadi nl_constr_h_e_fun; + external_function_param_casadi nl_constr_h_e_fun_jac_hess; +{%- endif %} + +} {{ model.name }}_solver_capsule; + +ACADOS_SYMBOL_EXPORT {{ model.name }}_solver_capsule * {{ model.name }}_acados_create_capsule(void); +ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_free_capsule({{ model.name }}_solver_capsule *capsule); + +ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_create({{ model.name }}_solver_capsule * capsule); + +ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_reset({{ model.name }}_solver_capsule* capsule); + +/** + * Generic version of {{ model.name }}_acados_create which allows to use a different number of shooting intervals than + * the number used for code generation. If new_time_steps=NULL and n_time_steps matches the number used for code + * generation, the time-steps from code generation is used. + */ +ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_capsule * capsule, int n_time_steps, double* new_time_steps); +/** + * Update the time step vector. Number N must be identical to the currently set number of shooting nodes in the + * nlp_solver_plan. Returns 0 if no error occurred and a otherwise a value other than 0. + */ +ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_update_time_steps({{ model.name }}_solver_capsule * capsule, int N, double* new_time_steps); +/** + * This function is used for updating an already initialized solver with a different number of qp_cond_N. + */ +ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_update_qp_solver_cond_N({{ model.name }}_solver_capsule * capsule, int qp_solver_cond_N); +ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_update_params({{ model.name }}_solver_capsule * capsule, int stage, double *value, int np); +ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_solve({{ model.name }}_solver_capsule * capsule); +ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_free({{ model.name }}_solver_capsule * capsule); +ACADOS_SYMBOL_EXPORT void {{ model.name }}_acados_print_stats({{ model.name }}_solver_capsule * capsule); + +ACADOS_SYMBOL_EXPORT ocp_nlp_in *{{ model.name }}_acados_get_nlp_in({{ model.name }}_solver_capsule * capsule); +ACADOS_SYMBOL_EXPORT ocp_nlp_out *{{ model.name }}_acados_get_nlp_out({{ model.name }}_solver_capsule * capsule); +ACADOS_SYMBOL_EXPORT ocp_nlp_out *{{ model.name }}_acados_get_sens_out({{ model.name }}_solver_capsule * capsule); +ACADOS_SYMBOL_EXPORT ocp_nlp_solver *{{ model.name }}_acados_get_nlp_solver({{ model.name }}_solver_capsule * capsule); +ACADOS_SYMBOL_EXPORT ocp_nlp_config *{{ model.name }}_acados_get_nlp_config({{ model.name }}_solver_capsule * capsule); +ACADOS_SYMBOL_EXPORT void *{{ model.name }}_acados_get_nlp_opts({{ model.name }}_solver_capsule * capsule); +ACADOS_SYMBOL_EXPORT ocp_nlp_dims *{{ model.name }}_acados_get_nlp_dims({{ model.name }}_solver_capsule * capsule); +ACADOS_SYMBOL_EXPORT ocp_nlp_plan_t *{{ model.name }}_acados_get_nlp_plan({{ model.name }}_solver_capsule * capsule); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_SOLVER_{{ model.name }}_H_ diff --git a/third_party/acados/acados_template/c_templates_tera/acados_solver.in.pxd b/third_party/acados/acados_template/c_templates_tera/acados_solver.in.pxd new file mode 100644 index 0000000000..09f8755cbe --- /dev/null +++ b/third_party/acados/acados_template/c_templates_tera/acados_solver.in.pxd @@ -0,0 +1,62 @@ +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + +cimport acados_solver_common + +cdef extern from "acados_solver_{{ model.name }}.h": + ctypedef struct nlp_solver_capsule "{{ model.name }}_solver_capsule": + pass + + nlp_solver_capsule * acados_create_capsule "{{ model.name }}_acados_create_capsule"() + int acados_free_capsule "{{ model.name }}_acados_free_capsule"(nlp_solver_capsule *capsule) + + int acados_create "{{ model.name }}_acados_create"(nlp_solver_capsule * capsule) + + int acados_create_with_discretization "{{ model.name }}_acados_create_with_discretization"(nlp_solver_capsule * capsule, int n_time_steps, double* new_time_steps) + int acados_update_time_steps "{{ model.name }}_acados_update_time_steps"(nlp_solver_capsule * capsule, int N, double* new_time_steps) + int acados_update_qp_solver_cond_N "{{ model.name }}_acados_update_qp_solver_cond_N"(nlp_solver_capsule * capsule, int qp_solver_cond_N) + + int acados_update_params "{{ model.name }}_acados_update_params"(nlp_solver_capsule * capsule, int stage, double *value, int np_) + int acados_solve "{{ model.name }}_acados_solve"(nlp_solver_capsule * capsule) + int acados_reset "{{ model.name }}_acados_reset"(nlp_solver_capsule * capsule) + int acados_free "{{ model.name }}_acados_free"(nlp_solver_capsule * capsule) + void acados_print_stats "{{ model.name }}_acados_print_stats"(nlp_solver_capsule * capsule) + + acados_solver_common.ocp_nlp_in *acados_get_nlp_in "{{ model.name }}_acados_get_nlp_in"(nlp_solver_capsule * capsule) + acados_solver_common.ocp_nlp_out *acados_get_nlp_out "{{ model.name }}_acados_get_nlp_out"(nlp_solver_capsule * capsule) + acados_solver_common.ocp_nlp_out *acados_get_sens_out "{{ model.name }}_acados_get_sens_out"(nlp_solver_capsule * capsule) + acados_solver_common.ocp_nlp_solver *acados_get_nlp_solver "{{ model.name }}_acados_get_nlp_solver"(nlp_solver_capsule * capsule) + acados_solver_common.ocp_nlp_config *acados_get_nlp_config "{{ model.name }}_acados_get_nlp_config"(nlp_solver_capsule * capsule) + void *acados_get_nlp_opts "{{ model.name }}_acados_get_nlp_opts"(nlp_solver_capsule * capsule) + acados_solver_common.ocp_nlp_dims *acados_get_nlp_dims "{{ model.name }}_acados_get_nlp_dims"(nlp_solver_capsule * capsule) + acados_solver_common.ocp_nlp_plan *acados_get_nlp_plan "{{ model.name }}_acados_get_nlp_plan"(nlp_solver_capsule * capsule) diff --git a/third_party/acados/acados_template/c_templates_tera/acados_solver_sfun.in.c b/third_party/acados/acados_template/c_templates_tera/acados_solver_sfun.in.c new file mode 100644 index 0000000000..96e0983de6 --- /dev/null +++ b/third_party/acados/acados_template/c_templates_tera/acados_solver_sfun.in.c @@ -0,0 +1,782 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + +#define S_FUNCTION_NAME acados_solver_sfunction_{{ model.name }} +#define S_FUNCTION_LEVEL 2 + +#define MDL_START + +// acados +// #include "acados/utils/print.h" +#include "acados_c/sim_interface.h" +#include "acados_c/external_function_interface.h" + +// example specific +#include "{{ model.name }}_model/{{ model.name }}_model.h" +#include "acados_solver_{{ model.name }}.h" + +#include "simstruc.h" + +{% if simulink_opts.samplingtime == "t0" -%} +#define SAMPLINGTIME {{ solver_options.time_steps[0] }} +{%- elif simulink_opts.samplingtime == "-1" -%} +#define SAMPLINGTIME -1 +{%- else -%} + {{ throw(message = "simulink_opts.samplingtime must be '-1' or 't0', got val") }} +{%- endif %} + +static void mdlInitializeSizes (SimStruct *S) +{ + // specify the number of continuous and discrete states + ssSetNumContStates(S, 0); + ssSetNumDiscStates(S, 0); + + {%- for key, val in simulink_opts.inputs -%} + {%- if val != 0 and val != 1 -%} + {{ throw(message = "simulink_opts.inputs must be 0 or 1, got val") }} + {%- endif -%} + {%- endfor -%} + + {#- compute number of input ports #} + {%- set n_inputs = 0 -%} + {%- if dims.nbx_0 > 0 and simulink_opts.inputs.lbx_0 -%} {#- lbx_0 #} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif -%} + {%- if dims.nbx_0 > 0 and simulink_opts.inputs.ubx_0 -%} {#- ubx_0 #} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif -%} + {%- if dims.np > 0 and simulink_opts.inputs.parameter_traj -%} {#- parameter_traj #} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif -%} + {%- if dims.ny_0 > 0 and simulink_opts.inputs.y_ref_0 -%} {#- y_ref_0 -#} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif -%} + {%- if dims.ny > 0 and dims.N > 1 and simulink_opts.inputs.y_ref -%} {#- y_ref -#} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif -%} + {%- if dims.ny_e > 0 and dims.N > 0 and simulink_opts.inputs.y_ref_e -%} {#- y_ref_e #} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif -%} + {%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.lbx -%} {#- lbx #} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif -%} + {%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.ubx -%} {#- ubx #} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif -%} + {%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.lbx_e -%} {#- lbx_e #} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif -%} + {%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.ubx_e -%} {#- ubx_e #} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif -%} + {%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.lbu -%} {#- lbu #} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif -%} + {%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.ubu -%} {#- ubu #} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif -%} + {%- if dims.ng > 0 and simulink_opts.inputs.lg -%} {#- lg #} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif -%} + {%- if dims.ng > 0 and simulink_opts.inputs.ug -%} {#- ug #} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif -%} + {%- if dims.nh > 0 and simulink_opts.inputs.lh -%} {#- lh #} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif -%} + {%- if dims.nh > 0 and simulink_opts.inputs.uh -%} {#- uh #} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif -%} + + {%- for key, val in simulink_opts.inputs -%} + {%- if val != 0 and val != 1 -%} + {{ throw(message = "simulink_opts.inputs must be 0 or 1, got val") }} + {%- endif -%} + {%- endfor -%} + {%- if dims.ny_0 > 0 and simulink_opts.inputs.cost_W_0 %} {#- cost_W_0 #} + {%- set n_inputs = n_inputs + 1 %} + {%- endif -%} + {%- if dims.ny > 0 and simulink_opts.inputs.cost_W %} {#- cost_W #} + {%- set n_inputs = n_inputs + 1 %} + {%- endif -%} + {%- if dims.ny_e > 0 and simulink_opts.inputs.cost_W_e %} {#- cost_W_e #} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif -%} + + {%- if simulink_opts.inputs.reset_solver -%} {#- reset_solver #} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif -%} + + {%- if simulink_opts.inputs.x_init -%} {#- x_init #} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif -%} + + {%- if simulink_opts.inputs.u_init -%} {#- u_init #} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif -%} + + // specify the number of input ports + if ( !ssSetNumInputPorts(S, {{ n_inputs }}) ) + return; + + // specify the number of output ports + {%- set_global n_outputs = 0 %} + {%- for key, val in simulink_opts.outputs %} + {%- if val == 1 %} + {%- set_global n_outputs = n_outputs + val %} + {%- elif val != 0 %} + {{ throw(message = "simulink_opts.outputs must be 0 or 1, got val") }} + {%- endif %} + {%- endfor %} + if ( !ssSetNumOutputPorts(S, {{ n_outputs }}) ) + return; + + // specify dimension information for the input ports + {%- set i_input = -1 %}{# note here i_input is 0-based #} + {%- if dims.nbx_0 > 0 and simulink_opts.inputs.lbx_0 -%} {#- lbx_0 #} + {%- set i_input = i_input + 1 %} + // lbx_0 + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nbx_0 }}); + {%- endif %} + {%- if dims.nbx_0 > 0 and simulink_opts.inputs.ubx_0 -%} {#- ubx_0 #} + {%- set i_input = i_input + 1 %} + // ubx_0 + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nbx_0 }}); + {%- endif %} + + {%- if dims.np > 0 and simulink_opts.inputs.parameter_traj -%} {#- parameter_traj #} + {%- set i_input = i_input + 1 %} + // parameters + ssSetInputPortVectorDimension(S, {{ i_input }}, ({{ dims.N }}+1) * {{ dims.np }}); + {%- endif %} + + {%- if dims.ny > 0 and simulink_opts.inputs.y_ref_0 %} + {%- set i_input = i_input + 1 %} + // y_ref_0 + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.ny_0 }}); + {%- endif %} + + {%- if dims.ny > 0 and dims.N > 1 and simulink_opts.inputs.y_ref %} + {%- set i_input = i_input + 1 %} + // y_ref + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ (dims.N-1) * dims.ny }}); + {%- endif %} + + {%- if dims.ny_e > 0 and dims.N > 0 and simulink_opts.inputs.y_ref_e %} + {%- set i_input = i_input + 1 %} + // y_ref_e + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.ny_e }}); + {%- endif %} + + {%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.lbx -%} {#- lbx #} + {%- set i_input = i_input + 1 %} + // lbx + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ (dims.N-1) * dims.nbx }}); + {%- endif %} + {%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.ubx -%} {#- ubx #} + {%- set i_input = i_input + 1 %} + // ubx + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ (dims.N-1) * dims.nbx }}); + {%- endif %} + + {%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.lbx_e -%} {#- lbx_e #} + {%- set i_input = i_input + 1 %} + // lbx_e + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nbx_e }}); + {%- endif %} + {%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.ubx_e -%} {#- ubx_e #} + {%- set i_input = i_input + 1 %} + // ubx_e + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nbx_e }}); + {%- endif %} + + {%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.lbu -%} {#- lbu #} + {%- set i_input = i_input + 1 %} + // lbu + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.N*dims.nbu }}); + {%- endif -%} + {%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.ubu -%} {#- ubu #} + {%- set i_input = i_input + 1 %} + // ubu + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.N*dims.nbu }}); + {%- endif -%} + + + {%- if dims.ng > 0 and simulink_opts.inputs.lg -%} {#- lg #} + {%- set i_input = i_input + 1 %} + // lg + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.ng }}); + {%- endif -%} + {%- if dims.ng > 0 and simulink_opts.inputs.ug -%} {#- ug #} + {%- set i_input = i_input + 1 %} + // ug + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.ng }}); + {%- endif -%} + + {%- if dims.nh > 0 and simulink_opts.inputs.lh -%} {#- lh #} + {%- set i_input = i_input + 1 %} + // lh + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nh }}); + {%- endif -%} + {%- if dims.nh > 0 and simulink_opts.inputs.uh -%} {#- uh #} + {%- set i_input = i_input + 1 %} + // uh + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nh }}); + {%- endif -%} + + {%- if dims.ny_0 > 0 and simulink_opts.inputs.cost_W_0 %} {#- cost_W_0 #} + {%- set i_input = i_input + 1 %} + // cost_W_0 + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.ny_0 * dims.ny_0 }}); + {%- endif %} + + {%- if dims.ny > 0 and simulink_opts.inputs.cost_W %} {#- cost_W #} + {%- set i_input = i_input + 1 %} + // cost_W + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.ny * dims.ny }}); + {%- endif %} + + {%- if dims.ny_e > 0 and simulink_opts.inputs.cost_W_e %} {#- cost_W_e #} + {%- set i_input = i_input + 1 %} + // cost_W_e + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.ny_e * dims.ny_e }}); + {%- endif %} + + {%- if simulink_opts.inputs.reset_solver -%} {#- reset_solver #} + {%- set i_input = i_input + 1 %} + // reset_solver + ssSetInputPortVectorDimension(S, {{ i_input }}, 1); + {%- endif -%} + + {%- if simulink_opts.inputs.x_init -%} {#- x_init #} + {%- set i_input = i_input + 1 %} + // x_init + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nx * (dims.N+1) }}); + {%- endif -%} + + {%- if simulink_opts.inputs.u_init -%} {#- u_init #} + {%- set i_input = i_input + 1 %} + // u_init + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nu * (dims.N) }}); + {%- endif -%} + + /* specify dimension information for the OUTPUT ports */ + {%- set i_output = -1 %}{# note here i_output is 0-based #} + {%- if dims.nu > 0 and simulink_opts.outputs.u0 == 1 %} + {%- set i_output = i_output + 1 %} + ssSetOutputPortVectorDimension(S, {{ i_output }}, {{ dims.nu }} ); + {%- endif %} + + {%- if simulink_opts.outputs.utraj == 1 %} + {%- set i_output = i_output + 1 %} + ssSetOutputPortVectorDimension(S, {{ i_output }}, {{ dims.nu * dims.N }} ); + {%- endif %} + + {%- if simulink_opts.outputs.xtraj == 1 %} + {%- set i_output = i_output + 1 %} + ssSetOutputPortVectorDimension(S, {{ i_output }}, {{ dims.nx * (dims.N+1) }} ); + {%- endif %} + + {%- if simulink_opts.outputs.solver_status == 1 %} + {%- set i_output = i_output + 1 %} + ssSetOutputPortVectorDimension(S, {{ i_output }}, 1 ); + {%- endif %} + + {%- if simulink_opts.outputs.KKT_residual == 1 %} + {%- set i_output = i_output + 1 %} + ssSetOutputPortVectorDimension(S, {{ i_output }}, 1 ); + {%- endif %} + + {%- if dims.N > 0 and simulink_opts.outputs.x1 == 1 %} + {%- set i_output = i_output + 1 %} + ssSetOutputPortVectorDimension(S, {{ i_output }}, {{ dims.nx }} ); // state at shooting node 1 + {%- endif %} + + {%- if simulink_opts.outputs.CPU_time == 1 %} + {%- set i_output = i_output + 1 %} + ssSetOutputPortVectorDimension(S, {{ i_output }}, 1); + {%- endif %} + + {%- if simulink_opts.outputs.CPU_time_sim == 1 %} + {%- set i_output = i_output + 1 %} + ssSetOutputPortVectorDimension(S, {{ i_output }}, 1); + {%- endif %} + + {%- if simulink_opts.outputs.CPU_time_qp == 1 %} + {%- set i_output = i_output + 1 %} + ssSetOutputPortVectorDimension(S, {{ i_output }}, 1); + {%- endif %} + + {%- if simulink_opts.outputs.CPU_time_lin == 1 %} + {%- set i_output = i_output + 1 %} + ssSetOutputPortVectorDimension(S, {{ i_output }}, 1); + {%- endif %} + + {%- if simulink_opts.outputs.sqp_iter == 1 %} + {%- set i_output = i_output + 1 %} + ssSetOutputPortVectorDimension(S, {{ i_output }}, 1 ); + {%- endif %} + + // specify the direct feedthrough status + // should be set to 1 for all inputs used in mdlOutputs + {%- for i in range(end=n_inputs) %} + ssSetInputPortDirectFeedThrough(S, {{ i }}, 1); + {%- endfor %} + + // one sample time + ssSetNumSampleTimes(S, 1); +} + + +#if defined(MATLAB_MEX_FILE) + +#define MDL_SET_INPUT_PORT_DIMENSION_INFO +#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO + +static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo) +{ + if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) ) + return; +} + +static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo) +{ + if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) ) + return; +} + +#endif /* MATLAB_MEX_FILE */ + + +static void mdlInitializeSampleTimes(SimStruct *S) +{ + ssSetSampleTime(S, 0, SAMPLINGTIME); + ssSetOffsetTime(S, 0, 0.0); +} + + +static void mdlStart(SimStruct *S) +{ + {{ model.name }}_solver_capsule *capsule = {{ model.name }}_acados_create_capsule(); + {{ model.name }}_acados_create(capsule); + + ssSetUserData(S, (void*)capsule); +} + + +static void mdlOutputs(SimStruct *S, int_T tid) +{ + {{ model.name }}_solver_capsule *capsule = ssGetUserData(S); + ocp_nlp_config *nlp_config = {{ model.name }}_acados_get_nlp_config(capsule); + ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims(capsule); + ocp_nlp_in *nlp_in = {{ model.name }}_acados_get_nlp_in(capsule); + ocp_nlp_out *nlp_out = {{ model.name }}_acados_get_nlp_out(capsule); + + InputRealPtrsType in_sign; + + {%- set buffer_sizes = [dims.nbx_0, dims.np, dims.nbx, dims.nbu, dims.ng, dims.nh, dims.nx] -%} + + {%- if dims.ny_0 > 0 and simulink_opts.inputs.y_ref_0 %} {# y_ref_0 #} + {%- set buffer_sizes = buffer_sizes | concat(with=(dims.ny_0)) %} + {%- endif %} + {%- if dims.ny > 0 and dims.N > 1 and simulink_opts.inputs.y_ref %} {# y_ref #} + {%- set buffer_sizes = buffer_sizes | concat(with=(dims.ny)) %} + {%- endif %} + {%- if dims.ny_e > 0 and dims.N > 0 and simulink_opts.inputs.y_ref_e %} {# y_ref_e #} + {%- set buffer_sizes = buffer_sizes | concat(with=(dims.ny_e)) %} + {%- endif %} + + {%- if dims.ny_0 > 0 and simulink_opts.inputs.cost_W_0 %} {#- cost_W_0 #} + {%- set buffer_sizes = buffer_sizes | concat(with=(dims.ny_0 * dims.ny_0)) %} + {%- endif %} + {%- if dims.ny > 0 and simulink_opts.inputs.cost_W %} {#- cost_W #} + {%- set buffer_sizes = buffer_sizes | concat(with=(dims.ny * dims.ny)) %} + {%- endif %} + {%- if dims.ny_e > 0 and simulink_opts.inputs.cost_W_e %} {#- cost_W_e #} + {%- set buffer_sizes = buffer_sizes | concat(with=(dims.ny_e * dims.ny_e)) %} + {%- endif %} + + // local buffer + {%- set buffer_size = buffer_sizes | sort | last %} + real_t buffer[{{ buffer_size }}]; + + /* go through inputs */ + {%- set i_input = -1 %} + {%- if dims.nbx_0 > 0 and simulink_opts.inputs.lbx_0 -%} {#- lbx_0 #} + // lbx_0 + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + for (int i = 0; i < {{ dims.nbx_0 }}; i++) + buffer[i] = (double)(*in_sign[i]); + + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", buffer); + {%- endif %} + + {%- if dims.nbx_0 > 0 and simulink_opts.inputs.ubx_0 -%} {#- ubx_0 #} + // ubx_0 + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + for (int i = 0; i < {{ dims.nbx_0 }}; i++) + buffer[i] = (double)(*in_sign[i]); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", buffer); + {%- endif %} + + {%- if dims.np > 0 and simulink_opts.inputs.parameter_traj -%} {#- parameter_traj #} + // parameters - stage-variant !!! + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + + // update value of parameters + for (int ii = 0; ii <= {{ dims.N }}; ii++) + { + for (int jj = 0; jj < {{ dims.np }}; jj++) + buffer[jj] = (double)(*in_sign[ii*{{dims.np}}+jj]); + {{ model.name }}_acados_update_params(capsule, ii, buffer, {{ dims.np }}); + } + {%- endif %} + + {% if dims.ny_0 > 0 and simulink_opts.inputs.y_ref_0 %} + // y_ref_0 + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + + for (int i = 0; i < {{ dims.ny_0 }}; i++) + buffer[i] = (double)(*in_sign[i]); + + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", (void *) buffer); + {%- endif %} + + {% if dims.ny > 0 and dims.N > 1 and simulink_opts.inputs.y_ref %} + // y_ref - for stages 1 to N-1 + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + + for (int ii = 1; ii < {{ dims.N }}; ii++) + { + for (int jj = 0; jj < {{ dims.ny }}; jj++) + buffer[jj] = (double)(*in_sign[(ii-1)*{{ dims.ny }}+jj]); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, ii, "yref", (void *) buffer); + } + {%- endif %} + + {% if dims.ny_e > 0 and dims.N > 0 and simulink_opts.inputs.y_ref_e %} + // y_ref_e + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + + for (int i = 0; i < {{ dims.ny_e }}; i++) + buffer[i] = (double)(*in_sign[i]); + + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, {{ dims.N }}, "yref", (void *) buffer); + {%- endif %} + + {%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.lbx -%} {#- lbx #} + // lbx + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + for (int ii = 1; ii < {{ dims.N }}; ii++) + { + for (int jj = 0; jj < {{ dims.nbx }}; jj++) + buffer[jj] = (double)(*in_sign[(ii-1)*{{ dims.nbx }}+jj]); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lbx", (void *) buffer); + } + {%- endif %} + {%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.ubx -%} {#- ubx #} + // ubx + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + for (int ii = 1; ii < {{ dims.N }}; ii++) + { + for (int jj = 0; jj < {{ dims.nbx }}; jj++) + buffer[jj] = (double)(*in_sign[(ii-1)*{{ dims.nbx }}+jj]); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ubx", (void *) buffer); + } + {%- endif %} + + + {%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.lbx_e -%} {#- lbx_e #} + // lbx_e + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + + for (int i = 0; i < {{ dims.nbx_e }}; i++) + buffer[i] = (double)(*in_sign[i]); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, {{ dims.N }}, "lbx", buffer); + {%- endif %} + {%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.ubx_e -%} {#- ubx_e #} + // ubx_e + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + + for (int i = 0; i < {{ dims.nbx_e }}; i++) + buffer[i] = (double)(*in_sign[i]); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, {{ dims.N }}, "ubx", buffer); + {%- endif %} + + + {%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.lbu -%} {#- lbu #} + // lbu + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + for (int ii = 0; ii < {{ dims.N }}; ii++) + { + for (int jj = 0; jj < {{ dims.nbu }}; jj++) + buffer[jj] = (double)(*in_sign[ii*{{ dims.nbu }}+jj]); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lbu", (void *) buffer); + } + {%- endif -%} + {%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.ubu -%} {#- ubu #} + // ubu + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + for (int ii = 0; ii < {{ dims.N }}; ii++) + { + for (int jj = 0; jj < {{ dims.nbu }}; jj++) + buffer[jj] = (double)(*in_sign[ii*{{ dims.nbu }}+jj]); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ubu", (void *) buffer); + } + {%- endif -%} + + {%- if dims.ng > 0 and simulink_opts.inputs.lg -%} {#- lg #} + // lg + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + + for (int i = 0; i < {{ dims.ng }}; i++) + buffer[i] = (double)(*in_sign[i]); + + for (int ii = 0; ii < {{ dims.N }}; ii++) + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lg", buffer); + {%- endif -%} + {%- if dims.ng > 0 and simulink_opts.inputs.ug -%} {#- ug #} + // ug + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + + for (int i = 0; i < {{ dims.ng }}; i++) + buffer[i] = (double)(*in_sign[i]); + + for (int ii = 0; ii < {{ dims.N }}; ii++) + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ug", buffer); + {%- endif -%} + {%- if dims.nh > 0 and simulink_opts.inputs.lh -%} {#- lh #} + // lh + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + + for (int i = 0; i < {{ dims.nh }}; i++) + buffer[i] = (double)(*in_sign[i]); + + for (int ii = 0; ii < {{ dims.N }}; ii++) + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lh", buffer); + {%- endif -%} + {%- if dims.nh > 0 and simulink_opts.inputs.uh -%} {#- uh #} + // uh + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + + for (int i = 0; i < {{ dims.nh }}; i++) + buffer[i] = (double)(*in_sign[i]); + + for (int ii = 0; ii < {{ dims.N }}; ii++) + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "uh", buffer); + {%- endif -%} + + {%- if dims.ny_0 > 0 and simulink_opts.inputs.cost_W_0 %} {#- cost_W_0 #} + // cost_W_0 + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + for (int i = 0; i < {{ dims.ny_0 * dims.ny_0 }}; i++) + buffer[i] = (double)(*in_sign[i]); + + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "W", buffer); + {%- endif %} + + {%- if dims.ny > 0 and simulink_opts.inputs.cost_W %} {#- cost_W #} + // cost_W + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + for (int i = 0; i < {{ dims.ny * dims.ny }}; i++) + buffer[i] = (double)(*in_sign[i]); + + for (int ii = 1; ii < {{ dims.N }}; ii++) + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, ii, "W", buffer); + {%- endif %} + + {%- if dims.ny_e > 0 and simulink_opts.inputs.cost_W_e %} {#- cost_W_e #} + // cost_W_e + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + for (int i = 0; i < {{ dims.ny_e * dims.ny_e }}; i++) + buffer[i] = (double)(*in_sign[i]); + + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, {{ dims.N }}, "W", buffer); + {%- endif %} + + {%- if simulink_opts.inputs.reset_solver %} {#- reset_solver #} + // reset_solver + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + double reset = (double)(*in_sign[0]); + if (reset) + { + {{ model.name }}_acados_reset(capsule); + } + {%- endif %} + + {%- if simulink_opts.inputs.x_init %} {#- x_init #} + // x_init + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + for (int ii = 0; ii < {{ dims.N + 1 }}; ii++) + { + for (int jj = 0; jj < {{ dims.nx }}; jj++) + buffer[jj] = (double)(*in_sign[(ii)*{{ dims.nx }}+jj]); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, ii, "x", (void *) buffer); + } + {%- endif %} + + {%- if simulink_opts.inputs.u_init %} {#- u_init #} + // u_init + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + for (int ii = 0; ii < {{ dims.N }}; ii++) + { + for (int jj = 0; jj < {{ dims.nu }}; jj++) + buffer[jj] = (double)(*in_sign[(ii)*{{ dims.nu }}+jj]); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, ii, "u", (void *) buffer); + } + {%- endif %} + + /* call solver */ + int rti_phase = 0; + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "rti_phase", &rti_phase); + int acados_status = {{ model.name }}_acados_solve(capsule); + + + /* set outputs */ + // assign pointers to output signals + real_t *out_u0, *out_utraj, *out_xtraj, *out_status, *out_sqp_iter, *out_KKT_res, *out_x1, *out_cpu_time, *out_cpu_time_sim, *out_cpu_time_qp, *out_cpu_time_lin; + int tmp_int; + + {%- set i_output = -1 -%}{# note here i_output is 0-based #} + {%- if dims.nu > 0 and simulink_opts.outputs.u0 == 1 %} + {%- set i_output = i_output + 1 %} + out_u0 = ssGetOutputPortRealSignal(S, {{ i_output }}); + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "u", (void *) out_u0); + {%- endif %} + + {%- if simulink_opts.outputs.utraj == 1 %} + {%- set i_output = i_output + 1 %} + out_utraj = ssGetOutputPortRealSignal(S, {{ i_output }}); + for (int ii = 0; ii < {{ dims.N }}; ii++) + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, + "u", (void *) (out_utraj + ii * {{ dims.nu }})); + {%- endif %} + + {% if simulink_opts.outputs.xtraj == 1 %} + {%- set i_output = i_output + 1 %} + + out_xtraj = ssGetOutputPortRealSignal(S, {{ i_output }}); + for (int ii = 0; ii < {{ dims.N + 1 }}; ii++) + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, + "x", (void *) (out_xtraj + ii * {{ dims.nx }})); + {%- endif %} + + {%- if simulink_opts.outputs.solver_status == 1 %} + {%- set i_output = i_output + 1 %} + out_status = ssGetOutputPortRealSignal(S, {{ i_output }}); + *out_status = (real_t) acados_status; + {%- endif %} + + {%- if simulink_opts.outputs.KKT_residual == 1 %} + {%- set i_output = i_output + 1 %} + out_KKT_res = ssGetOutputPortRealSignal(S, {{ i_output }}); + *out_KKT_res = (real_t) nlp_out->inf_norm_res; + {%- endif %} + + {%- if dims.N > 0 and simulink_opts.outputs.x1 == 1 %} + {%- set i_output = i_output + 1 %} + out_x1 = ssGetOutputPortRealSignal(S, {{ i_output }}); + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 1, "x", (void *) out_x1); + {%- endif %} + + {%- if simulink_opts.outputs.CPU_time == 1 %} + {%- set i_output = i_output + 1 %} + out_cpu_time = ssGetOutputPortRealSignal(S, {{ i_output }}); + // get solution time + ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_tot", (void *) out_cpu_time); + {%- endif -%} + + {%- if simulink_opts.outputs.CPU_time_sim == 1 %} + {%- set i_output = i_output + 1 %} + out_cpu_time_sim = ssGetOutputPortRealSignal(S, {{ i_output }}); + ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_sim", (void *) out_cpu_time_sim); + {%- endif -%} + + {%- if simulink_opts.outputs.CPU_time_qp == 1 %} + {%- set i_output = i_output + 1 %} + out_cpu_time_qp = ssGetOutputPortRealSignal(S, {{ i_output }}); + ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_qp", (void *) out_cpu_time_qp); + {%- endif -%} + + {%- if simulink_opts.outputs.CPU_time_lin == 1 %} + {%- set i_output = i_output + 1 %} + out_cpu_time_lin = ssGetOutputPortRealSignal(S, {{ i_output }}); + ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_lin", (void *) out_cpu_time_lin); + {%- endif -%} + + {%- if simulink_opts.outputs.sqp_iter == 1 %} + {%- set i_output = i_output + 1 %} + out_sqp_iter = ssGetOutputPortRealSignal(S, {{ i_output }}); + // get sqp iter + ocp_nlp_get(nlp_config, capsule->nlp_solver, "sqp_iter", (void *) &tmp_int); + *out_sqp_iter = (real_t) tmp_int; + {%- endif %} + +} + +static void mdlTerminate(SimStruct *S) +{ + {{ model.name }}_solver_capsule *capsule = ssGetUserData(S); + + {{ model.name }}_acados_free(capsule); + {{ model.name }}_acados_free_capsule(capsule); +} + + +#ifdef MATLAB_MEX_FILE +#include "simulink.c" +#else +#include "cg_sfun.h" +#endif diff --git a/third_party/acados/acados_template/c_templates_tera/cost_y_0_fun.in.h b/third_party/acados/acados_template/c_templates_tera/cost_y_0_fun.in.h new file mode 100644 index 0000000000..347446e3f4 --- /dev/null +++ b/third_party/acados/acados_template/c_templates_tera/cost_y_0_fun.in.h @@ -0,0 +1,69 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef {{ model.name }}_Y_0_COST +#define {{ model.name }}_Y_0_COST + +#ifdef __cplusplus +extern "C" { +#endif + +{% if cost.cost_type_0 == "NONLINEAR_LS" %} +int {{ model.name }}_cost_y_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_y_0_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_y_0_fun_sparsity_in(int); +const int *{{ model.name }}_cost_y_0_fun_sparsity_out(int); +int {{ model.name }}_cost_y_0_fun_n_in(void); +int {{ model.name }}_cost_y_0_fun_n_out(void); + +int {{ model.name }}_cost_y_0_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_y_0_fun_jac_ut_xt_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_y_0_fun_jac_ut_xt_sparsity_in(int); +const int *{{ model.name }}_cost_y_0_fun_jac_ut_xt_sparsity_out(int); +int {{ model.name }}_cost_y_0_fun_jac_ut_xt_n_in(void); +int {{ model.name }}_cost_y_0_fun_jac_ut_xt_n_out(void); + +int {{ model.name }}_cost_y_0_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_y_0_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_y_0_hess_sparsity_in(int); +const int *{{ model.name }}_cost_y_0_hess_sparsity_out(int); +int {{ model.name }}_cost_y_0_hess_n_in(void); +int {{ model.name }}_cost_y_0_hess_n_out(void); +{% endif %} + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // {{ model.name }}_Y_0_COST diff --git a/third_party/acados/acados_template/c_templates_tera/cost_y_e_fun.in.h b/third_party/acados/acados_template/c_templates_tera/cost_y_e_fun.in.h new file mode 100644 index 0000000000..acc99009fe --- /dev/null +++ b/third_party/acados/acados_template/c_templates_tera/cost_y_e_fun.in.h @@ -0,0 +1,69 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef {{ model.name }}_Y_E_COST +#define {{ model.name }}_Y_E_COST + +#ifdef __cplusplus +extern "C" { +#endif + +{% if cost.cost_type_e == "NONLINEAR_LS" %} +int {{ model.name }}_cost_y_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_y_e_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_y_e_fun_sparsity_in(int); +const int *{{ model.name }}_cost_y_e_fun_sparsity_out(int); +int {{ model.name }}_cost_y_e_fun_n_in(void); +int {{ model.name }}_cost_y_e_fun_n_out(void); + +int {{ model.name }}_cost_y_e_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_y_e_fun_jac_ut_xt_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_y_e_fun_jac_ut_xt_sparsity_in(int); +const int *{{ model.name }}_cost_y_e_fun_jac_ut_xt_sparsity_out(int); +int {{ model.name }}_cost_y_e_fun_jac_ut_xt_n_in(void); +int {{ model.name }}_cost_y_e_fun_jac_ut_xt_n_out(void); + +int {{ model.name }}_cost_y_e_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_y_e_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_y_e_hess_sparsity_in(int); +const int *{{ model.name }}_cost_y_e_hess_sparsity_out(int); +int {{ model.name }}_cost_y_e_hess_n_in(void); +int {{ model.name }}_cost_y_e_hess_n_out(void); +{% endif %} + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // {{ model.name }}_Y_E_COST diff --git a/third_party/acados/acados_template/c_templates_tera/cost_y_fun.in.h b/third_party/acados/acados_template/c_templates_tera/cost_y_fun.in.h new file mode 100644 index 0000000000..1e03780cc1 --- /dev/null +++ b/third_party/acados/acados_template/c_templates_tera/cost_y_fun.in.h @@ -0,0 +1,69 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef {{ model.name }}_Y_COST +#define {{ model.name }}_Y_COST + +#ifdef __cplusplus +extern "C" { +#endif + +{% if cost.cost_type == "NONLINEAR_LS" %} +int {{ model.name }}_cost_y_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_y_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_y_fun_sparsity_in(int); +const int *{{ model.name }}_cost_y_fun_sparsity_out(int); +int {{ model.name }}_cost_y_fun_n_in(void); +int {{ model.name }}_cost_y_fun_n_out(void); + +int {{ model.name }}_cost_y_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_y_fun_jac_ut_xt_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_y_fun_jac_ut_xt_sparsity_in(int); +const int *{{ model.name }}_cost_y_fun_jac_ut_xt_sparsity_out(int); +int {{ model.name }}_cost_y_fun_jac_ut_xt_n_in(void); +int {{ model.name }}_cost_y_fun_jac_ut_xt_n_out(void); + +int {{ model.name }}_cost_y_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_y_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_y_hess_sparsity_in(int); +const int *{{ model.name }}_cost_y_hess_sparsity_out(int); +int {{ model.name }}_cost_y_hess_n_in(void); +int {{ model.name }}_cost_y_hess_n_out(void); +{% endif %} + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // {{ model.name }}_Y_COST diff --git a/third_party/acados/acados_template/c_templates_tera/external_cost.in.h b/third_party/acados/acados_template/c_templates_tera/external_cost.in.h new file mode 100644 index 0000000000..d200dba45e --- /dev/null +++ b/third_party/acados/acados_template/c_templates_tera/external_cost.in.h @@ -0,0 +1,74 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef {{ model.name }}_EXT_COST +#define {{ model.name }}_EXT_COST + +#ifdef __cplusplus +extern "C" { +#endif + +{% if cost.cost_ext_fun_type == "casadi" %} +{% if cost.cost_type == "EXTERNAL" %} +int {{ model.name }}_cost_ext_cost_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_ext_cost_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_ext_cost_fun_sparsity_in(int); +const int *{{ model.name }}_cost_ext_cost_fun_sparsity_out(int); +int {{ model.name }}_cost_ext_cost_fun_n_in(void); +int {{ model.name }}_cost_ext_cost_fun_n_out(void); + +int {{ model.name }}_cost_ext_cost_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_ext_cost_fun_jac_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_ext_cost_fun_jac_hess_sparsity_in(int); +const int *{{ model.name }}_cost_ext_cost_fun_jac_hess_sparsity_out(int); +int {{ model.name }}_cost_ext_cost_fun_jac_hess_n_in(void); +int {{ model.name }}_cost_ext_cost_fun_jac_hess_n_out(void); + +int {{ model.name }}_cost_ext_cost_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_ext_cost_fun_jac_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_ext_cost_fun_jac_sparsity_in(int); +const int *{{ model.name }}_cost_ext_cost_fun_jac_sparsity_out(int); +int {{ model.name }}_cost_ext_cost_fun_jac_n_in(void); +int {{ model.name }}_cost_ext_cost_fun_jac_n_out(void); +{% endif %} + +{% else %} +int {{ cost.cost_function_ext_cost }}(void **, void **, void *); +{% endif %} + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // {{ model.name }}_EXT_COST diff --git a/third_party/acados/acados_template/c_templates_tera/external_cost_0.in.h b/third_party/acados/acados_template/c_templates_tera/external_cost_0.in.h new file mode 100644 index 0000000000..8152447e5f --- /dev/null +++ b/third_party/acados/acados_template/c_templates_tera/external_cost_0.in.h @@ -0,0 +1,75 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef {{ model.name }}_EXT_COST_0 +#define {{ model.name }}_EXT_COST_0 + +#ifdef __cplusplus +extern "C" { +#endif + +{% if cost.cost_ext_fun_type_0 == "casadi" %} + +{% if cost.cost_type_0 == "EXTERNAL" %} +int {{ model.name }}_cost_ext_cost_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_ext_cost_0_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_ext_cost_0_fun_sparsity_in(int); +const int *{{ model.name }}_cost_ext_cost_0_fun_sparsity_out(int); +int {{ model.name }}_cost_ext_cost_0_fun_n_in(void); +int {{ model.name }}_cost_ext_cost_0_fun_n_out(void); + +int {{ model.name }}_cost_ext_cost_0_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_ext_cost_0_fun_jac_hess_sparsity_in(int); +const int *{{ model.name }}_cost_ext_cost_0_fun_jac_hess_sparsity_out(int); +int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_n_in(void); +int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_n_out(void); + +int {{ model.name }}_cost_ext_cost_0_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_ext_cost_0_fun_jac_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_ext_cost_0_fun_jac_sparsity_in(int); +const int *{{ model.name }}_cost_ext_cost_0_fun_jac_sparsity_out(int); +int {{ model.name }}_cost_ext_cost_0_fun_jac_n_in(void); +int {{ model.name }}_cost_ext_cost_0_fun_jac_n_out(void); +{% endif %} + +{% else %} +int {{ cost.cost_function_ext_cost_0 }}(void **, void **, void *); +{% endif %} + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // {{ model.name }}_EXT_COST_0 diff --git a/third_party/acados/acados_template/c_templates_tera/external_cost_e.in.h b/third_party/acados/acados_template/c_templates_tera/external_cost_e.in.h new file mode 100644 index 0000000000..56485db4c7 --- /dev/null +++ b/third_party/acados/acados_template/c_templates_tera/external_cost_e.in.h @@ -0,0 +1,74 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef {{ model.name }}_EXT_COST_E +#define {{ model.name }}_EXT_COST_E + +#ifdef __cplusplus +extern "C" { +#endif + +{% if cost.cost_ext_fun_type_e == "casadi" %} +{% if cost.cost_type_e == "EXTERNAL" %} +int {{ model.name }}_cost_ext_cost_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_ext_cost_e_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_ext_cost_e_fun_sparsity_in(int); +const int *{{ model.name }}_cost_ext_cost_e_fun_sparsity_out(int); +int {{ model.name }}_cost_ext_cost_e_fun_n_in(void); +int {{ model.name }}_cost_ext_cost_e_fun_n_out(void); + +int {{ model.name }}_cost_ext_cost_e_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_ext_cost_e_fun_jac_hess_sparsity_in(int); +const int *{{ model.name }}_cost_ext_cost_e_fun_jac_hess_sparsity_out(int); +int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_n_in(void); +int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_n_out(void); + +int {{ model.name }}_cost_ext_cost_e_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_ext_cost_e_fun_jac_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_ext_cost_e_fun_jac_sparsity_in(int); +const int *{{ model.name }}_cost_ext_cost_e_fun_jac_sparsity_out(int); +int {{ model.name }}_cost_ext_cost_e_fun_jac_n_in(void); +int {{ model.name }}_cost_ext_cost_e_fun_jac_n_out(void); +{% endif %} + +{% else %} +int {{ cost.cost_function_ext_cost_e }}(void **, void **, void *); +{% endif %} + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // {{ model.name }}_EXT_COST_E diff --git a/third_party/acados/acados_template/c_templates_tera/h_constraint.in.h b/third_party/acados/acados_template/c_templates_tera/h_constraint.in.h new file mode 100644 index 0000000000..e49176c6ef --- /dev/null +++ b/third_party/acados/acados_template/c_templates_tera/h_constraint.in.h @@ -0,0 +1,70 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + +#ifndef {{ model.name }}_H_CONSTRAINT +#define {{ model.name }}_H_CONSTRAINT + +#ifdef __cplusplus +extern "C" { +#endif + +{% if dims.nh > 0 %} +int {{ model.name }}_constr_h_fun_jac_uxt_zt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_constr_h_fun_jac_uxt_zt_work(int *, int *, int *, int *); +const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_sparsity_in(int); +const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_sparsity_out(int); +int {{ model.name }}_constr_h_fun_jac_uxt_zt_n_in(void); +int {{ model.name }}_constr_h_fun_jac_uxt_zt_n_out(void); + +int {{ model.name }}_constr_h_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_constr_h_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_constr_h_fun_sparsity_in(int); +const int *{{ model.name }}_constr_h_fun_sparsity_out(int); +int {{ model.name }}_constr_h_fun_n_in(void); +int {{ model.name }}_constr_h_fun_n_out(void); + +{% if solver_options.hessian_approx == "EXACT" -%} +int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_sparsity_in(int); +const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_sparsity_out(int); +int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess_n_in(void); +int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess_n_out(void); +{% endif %} +{% endif %} + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // {{ model.name }}_H_CONSTRAINT diff --git a/third_party/acados/acados_template/c_templates_tera/h_e_constraint.in.h b/third_party/acados/acados_template/c_templates_tera/h_e_constraint.in.h new file mode 100644 index 0000000000..a5dd711641 --- /dev/null +++ b/third_party/acados/acados_template/c_templates_tera/h_e_constraint.in.h @@ -0,0 +1,71 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef {{ model.name }}_H_E_CONSTRAINT +#define {{ model.name }}_H_E_CONSTRAINT + +#ifdef __cplusplus +extern "C" { +#endif + +{% if dims.nh_e > 0 %} +int {{ model.name }}_constr_h_e_fun_jac_uxt_zt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_work(int *, int *, int *, int *); +const int *{{ model.name }}_constr_h_e_fun_jac_uxt_zt_sparsity_in(int); +const int *{{ model.name }}_constr_h_e_fun_jac_uxt_zt_sparsity_out(int); +int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_n_in(void); +int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_n_out(void); + +int {{ model.name }}_constr_h_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_constr_h_e_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_constr_h_e_fun_sparsity_in(int); +const int *{{ model.name }}_constr_h_e_fun_sparsity_out(int); +int {{ model.name }}_constr_h_e_fun_n_in(void); +int {{ model.name }}_constr_h_e_fun_n_out(void); + +{% if solver_options.hessian_approx == "EXACT" -%} +int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_sparsity_in(int); +const int *{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_sparsity_out(int); +int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_n_in(void); +int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_n_out(void); +{% endif %} +{% endif %} + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // {{ model.name }}_H_E_CONSTRAINT diff --git a/third_party/acados/acados_template/c_templates_tera/main.in.c b/third_party/acados/acados_template/c_templates_tera/main.in.c new file mode 100644 index 0000000000..99c4f13be1 --- /dev/null +++ b/third_party/acados/acados_template/c_templates_tera/main.in.c @@ -0,0 +1,216 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +// standard +#include +#include +// acados +#include "acados/utils/print.h" +#include "acados/utils/math.h" +#include "acados_c/ocp_nlp_interface.h" +#include "acados_c/external_function_interface.h" +#include "acados_solver_{{ model.name }}.h" + +#define NX {{ model.name | upper }}_NX +#define NZ {{ model.name | upper }}_NZ +#define NU {{ model.name | upper }}_NU +#define NP {{ model.name | upper }}_NP +#define NBX {{ model.name | upper }}_NBX +#define NBX0 {{ model.name | upper }}_NBX0 +#define NBU {{ model.name | upper }}_NBU +#define NSBX {{ model.name | upper }}_NSBX +#define NSBU {{ model.name | upper }}_NSBU +#define NSH {{ model.name | upper }}_NSH +#define NSG {{ model.name | upper }}_NSG +#define NSPHI {{ model.name | upper }}_NSPHI +#define NSHN {{ model.name | upper }}_NSHN +#define NSGN {{ model.name | upper }}_NSGN +#define NSPHIN {{ model.name | upper }}_NSPHIN +#define NSBXN {{ model.name | upper }}_NSBXN +#define NS {{ model.name | upper }}_NS +#define NSN {{ model.name | upper }}_NSN +#define NG {{ model.name | upper }}_NG +#define NBXN {{ model.name | upper }}_NBXN +#define NGN {{ model.name | upper }}_NGN +#define NY0 {{ model.name | upper }}_NY0 +#define NY {{ model.name | upper }}_NY +#define NYN {{ model.name | upper }}_NYN +#define NH {{ model.name | upper }}_NH +#define NPHI {{ model.name | upper }}_NPHI +#define NHN {{ model.name | upper }}_NHN +#define NPHIN {{ model.name | upper }}_NPHIN +#define NR {{ model.name | upper }}_NR + + +int main() +{ + + {{ model.name }}_solver_capsule *acados_ocp_capsule = {{ model.name }}_acados_create_capsule(); + // there is an opportunity to change the number of shooting intervals in C without new code generation + int N = {{ model.name | upper }}_N; + // allocate the array and fill it accordingly + double* new_time_steps = NULL; + int status = {{ model.name }}_acados_create_with_discretization(acados_ocp_capsule, N, new_time_steps); + + if (status) + { + printf("{{ model.name }}_acados_create() returned status %d. Exiting.\n", status); + exit(1); + } + + ocp_nlp_config *nlp_config = {{ model.name }}_acados_get_nlp_config(acados_ocp_capsule); + ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims(acados_ocp_capsule); + ocp_nlp_in *nlp_in = {{ model.name }}_acados_get_nlp_in(acados_ocp_capsule); + ocp_nlp_out *nlp_out = {{ model.name }}_acados_get_nlp_out(acados_ocp_capsule); + ocp_nlp_solver *nlp_solver = {{ model.name }}_acados_get_nlp_solver(acados_ocp_capsule); + void *nlp_opts = {{ model.name }}_acados_get_nlp_opts(acados_ocp_capsule); + + // initial condition + int idxbx0[NBX0]; + {%- for i in range(end=dims.nbx_0) %} + idxbx0[{{ i }}] = {{ constraints.idxbx_0[i] }}; + {%- endfor %} + + double lbx0[NBX0]; + double ubx0[NBX0]; + {%- for i in range(end=dims.nbx_0) %} + lbx0[{{ i }}] = {{ constraints.lbx_0[i] }}; + ubx0[{{ i }}] = {{ constraints.ubx_0[i] }}; + {%- endfor %} + + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxbx", idxbx0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", lbx0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", ubx0); + + // initialization for state values + double x_init[NX]; + {%- for i in range(end=dims.nx) %} + x_init[{{ i }}] = 0.0; + {%- endfor %} + + // initial value for control input + double u0[NU]; + {%- for i in range(end=dims.nu) %} + u0[{{ i }}] = 0.0; + {%- endfor %} + + + {%- if dims.np > 0 %} + // set parameters + double p[NP]; + {%- for item in parameter_values %} + p[{{ loop.index0 }}] = {{ item }}; + {%- endfor %} + + for (int ii = 0; ii <= N; ii++) + { + {{ model.name }}_acados_update_params(acados_ocp_capsule, ii, p, NP); + } + {% endif %}{# if np > 0 #} + + // prepare evaluation + int NTIMINGS = 1; + double min_time = 1e12; + double kkt_norm_inf; + double elapsed_time; + int sqp_iter; + + double xtraj[NX * (N+1)]; + double utraj[NU * N]; + + + // solve ocp in loop + int rti_phase = 0; + + for (int ii = 0; ii < NTIMINGS; ii++) + { + // initialize solution + for (int i = 0; i < N; i++) + { + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "x", x_init); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "u", u0); + } + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, N, "x", x_init); + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "rti_phase", &rti_phase); + status = {{ model.name }}_acados_solve(acados_ocp_capsule); + ocp_nlp_get(nlp_config, nlp_solver, "time_tot", &elapsed_time); + min_time = MIN(elapsed_time, min_time); + } + + /* print solution and statistics */ + for (int ii = 0; ii <= nlp_dims->N; ii++) + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "x", &xtraj[ii*NX]); + for (int ii = 0; ii < nlp_dims->N; ii++) + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "u", &utraj[ii*NU]); + + printf("\n--- xtraj ---\n"); + d_print_exp_tran_mat( NX, N+1, xtraj, NX); + printf("\n--- utraj ---\n"); + d_print_exp_tran_mat( NU, N, utraj, NU ); + // ocp_nlp_out_print(nlp_solver->dims, nlp_out); + + printf("\nsolved ocp %d times, solution printed above\n\n", NTIMINGS); + + if (status == ACADOS_SUCCESS) + { + printf("{{ model.name }}_acados_solve(): SUCCESS!\n"); + } + else + { + printf("{{ model.name }}_acados_solve() failed with status %d.\n", status); + } + + // get solution + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "kkt_norm_inf", &kkt_norm_inf); + ocp_nlp_get(nlp_config, nlp_solver, "sqp_iter", &sqp_iter); + + {{ model.name }}_acados_print_stats(acados_ocp_capsule); + + printf("\nSolver info:\n"); + printf(" SQP iterations %2d\n minimum time for %d solve %f [ms]\n KKT %e\n", + sqp_iter, NTIMINGS, min_time*1000, kkt_norm_inf); + + // free solver + status = {{ model.name }}_acados_free(acados_ocp_capsule); + if (status) { + printf("{{ model.name }}_acados_free() returned status %d. \n", status); + } + // free solver capsule + status = {{ model.name }}_acados_free_capsule(acados_ocp_capsule); + if (status) { + printf("{{ model.name }}_acados_free_capsule() returned status %d. \n", status); + } + + return status; +} diff --git a/third_party/acados/acados_template/c_templates_tera/main_mex.in.c b/third_party/acados/acados_template/c_templates_tera/main_mex.in.c new file mode 100644 index 0000000000..8da5db29a0 --- /dev/null +++ b/third_party/acados/acados_template/c_templates_tera/main_mex.in.c @@ -0,0 +1,184 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +// standard +#include +#include +// acados +#include "acados/utils/print.h" +#include "acados/utils/math.h" +#include "acados_c/ocp_nlp_interface.h" +#include "acados_solver_{{ model.name }}.h" +// mex +#include "mex.h" + +/* auxilary mex */ +// prints a matrix in column-major format (exponential notation) +void MEX_print_exp_mat(int m, int n, double *A, int lda) +{ + for (int i=0; iN; i++) + { + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "x", x_init); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "u", u0); + } + status = {{ model.name }}_acados_solve(); + ocp_nlp_get(nlp_config, nlp_solver, "time_tot", &elapsed_time); + min_time = MIN(elapsed_time, min_time); + } + + /* print solution and statistics */ + for (int ii = 0; ii <= nlp_dims->N; ii++) + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "x", &xtraj[ii*{{ dims.nx }}]); + for (int ii = 0; ii < nlp_dims->N; ii++) + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "u", &utraj[ii*{{ dims.nu }}]); + + mexPrintf("\n--- xtraj ---\n"); + MEX_print_exp_tran_mat( {{ dims.nx }}, {{ dims.N }}+1, xtraj, {{ dims.nx }} ); + mexPrintf("\n--- utraj ---\n"); + MEX_print_exp_tran_mat( {{ dims.nu }}, {{ dims.N }}, utraj, {{ dims.nu }} ); + + mexPrintf("\nsolved ocp %d times, solution printed above\n\n", NTIMINGS); + + if (status == ACADOS_SUCCESS) + mexPrintf("{{ model.name }}_acados_solve(): SUCCESS!\n"); + else + mexPrintf("{{ model.name }}_acados_solve() failed with status %d.\n", status); + + // get solution + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "kkt_norm_inf", &kkt_norm_inf); + ocp_nlp_get(nlp_config, nlp_solver, "sqp_iter", &sqp_iter); + + mexPrintf("\nSolver info:\n"); + mexPrintf(" SQP iterations %2d\n minimum time for 1 solve %f [ms]\n KKT %e\n", + sqp_iter, min_time*1000, kkt_norm_inf); + + // free solver + status = {{ model.name }}_acados_free(); + if (status) + { + mexPrintf("{{ model.name }}_acados_free() returned status %d.\n", status); + } + + return; +} diff --git a/third_party/acados/acados_template/c_templates_tera/main_sim.in.c b/third_party/acados/acados_template/c_templates_tera/main_sim.in.c new file mode 100644 index 0000000000..743e81d593 --- /dev/null +++ b/third_party/acados/acados_template/c_templates_tera/main_sim.in.c @@ -0,0 +1,135 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +// standard +#include +#include +// acados +#include "acados/utils/print.h" +#include "acados/utils/math.h" +#include "acados_c/sim_interface.h" +#include "acados_sim_solver_{{ model.name }}.h" + +#define NX {{ model.name | upper }}_NX +#define NZ {{ model.name | upper }}_NZ +#define NU {{ model.name | upper }}_NU +#define NP {{ model.name | upper }}_NP + + +int main() +{ + int status = 0; + sim_solver_capsule *capsule = {{ model.name }}_acados_sim_solver_create_capsule(); + status = {{ model.name }}_acados_sim_create(capsule); + + if (status) + { + printf("acados_create() returned status %d. Exiting.\n", status); + exit(1); + } + + sim_config *acados_sim_config = {{ model.name }}_acados_get_sim_config(capsule); + sim_in *acados_sim_in = {{ model.name }}_acados_get_sim_in(capsule); + sim_out *acados_sim_out = {{ model.name }}_acados_get_sim_out(capsule); + void *acados_sim_dims = {{ model.name }}_acados_get_sim_dims(capsule); + + // initial condition + double x_current[NX]; + {%- for i in range(end=dims.nx) %} + x_current[{{ i }}] = 0.0; + {%- endfor %} + + {% if constraints.lbx_0 %} + {%- for i in range(end=dims.nbx_0) %} + x_current[{{ constraints.idxbx_0[i] }}] = {{ constraints.lbx_0[i] }}; + {%- endfor %} + {% if dims.nbx_0 != dims.nx %} + printf("main_sim: NOTE: initial state not fully defined via lbx_0, using 0.0 for indices that are not in idxbx_0."); + {%- endif %} + {% else %} + printf("main_sim: initial state not defined, should be in lbx_0, using zero vector."); + {%- endif %} + + + // initial value for control input + double u0[NU]; + {%- for i in range(end=dims.nu) %} + u0[{{ i }}] = 0.0; + {%- endfor %} + + {%- if dims.np > 0 %} + // set parameters + double p[NP]; + {%- for item in parameter_values %} + p[{{ loop.index0 }}] = {{ item }}; + {%- endfor %} + + {{ model.name }}_acados_sim_update_params(capsule, p, NP); + {% endif %}{# if np > 0 #} + + int n_sim_steps = 3; + // solve ocp in loop + for (int ii = 0; ii < n_sim_steps; ii++) + { + sim_in_set(acados_sim_config, acados_sim_dims, + acados_sim_in, "x", x_current); + status = {{ model.name }}_acados_sim_solve(capsule); + + if (status != ACADOS_SUCCESS) + { + printf("acados_solve() failed with status %d.\n", status); + } + + sim_out_get(acados_sim_config, acados_sim_dims, + acados_sim_out, "x", x_current); + + printf("\nx_current, %d\n", ii); + for (int jj = 0; jj < NX; jj++) + { + printf("%e\n", x_current[jj]); + } + } + + printf("\nPerformed %d simulation steps with acados integrator successfully.\n\n", n_sim_steps); + + // free solver + status = {{ model.name }}_acados_sim_free(capsule); + if (status) { + printf("{{ model.name }}_acados_sim_free() returned status %d. \n", status); + } + + {{ model.name }}_acados_sim_solver_free_capsule(capsule); + + return status; +} diff --git a/third_party/acados/acados_template/c_templates_tera/make_main_mex.in.m b/third_party/acados/acados_template/c_templates_tera/make_main_mex.in.m new file mode 100644 index 0000000000..9188686a0d --- /dev/null +++ b/third_party/acados/acados_template/c_templates_tera/make_main_mex.in.m @@ -0,0 +1,105 @@ +% +% Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +% Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +% Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +% Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +% +% This file is part of acados. +% +% The 2-Clause BSD License +% +% Redistribution and use in source and binary forms, with or without +% modification, are permitted provided that the following conditions are met: +% +% 1. Redistributions of source code must retain the above copyright notice, +% this list of conditions and the following disclaimer. +% +% 2. Redistributions in binary form must reproduce the above copyright notice, +% this list of conditions and the following disclaimer in the documentation +% and/or other materials provided with the distribution. +% +% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +% POSSIBILITY OF SUCH DAMAGE.; +% + +function make_main_mex_{{ model.name }}() + + opts.output_dir = pwd; + + % get acados folder + acados_folder = getenv('ACADOS_INSTALL_DIR'); + + % set paths + acados_include = ['-I' fullfile(acados_folder, 'include')]; + template_lib_include = ['-l' 'acados_solver_{{ model.name }}']; + template_lib_path = ['-L' fullfile(pwd)]; + + acados_lib_path = ['-L' fullfile(acados_folder, 'lib')]; + external_include = ['-I', fullfile(acados_folder, 'external')]; + blasfeo_include = ['-I', fullfile(acados_folder, 'external', 'blasfeo', 'include')]; + hpipm_include = ['-I', fullfile(acados_folder, 'external', 'hpipm', 'include')]; + + mex_names = { ... + 'main_mex_{{ model.name }}' ... + }; + + mex_files = cell(length(mex_names), 1); + for k=1:length(mex_names) + mex_files{k} = fullfile([mex_names{k}, '.c']); + end + + %% octave C flags + if is_octave() + if ~exist(fullfile(opts.output_dir, 'cflags_octave.txt'), 'file') + diary(fullfile(opts.output_dir, 'cflags_octave.txt')); + diary on + mkoctfile -p CFLAGS + diary off + input_file = fopen(fullfile(opts.output_dir, 'cflags_octave.txt'), 'r'); + cflags_tmp = fscanf(input_file, '%[^\n]s'); + fclose(input_file); + if ~ismac() + cflags_tmp = [cflags_tmp, ' -std=c99 -fopenmp']; + else + cflags_tmp = [cflags_tmp, ' -std=c99']; + end + input_file = fopen(fullfile(opts.output_dir, 'cflags_octave.txt'), 'w'); + fprintf(input_file, '%s', cflags_tmp); + fclose(input_file); + end + % read cflags from file + input_file = fopen(fullfile(opts.output_dir, 'cflags_octave.txt'), 'r'); + cflags_tmp = fscanf(input_file, '%[^\n]s'); + fclose(input_file); + setenv('CFLAGS', cflags_tmp); + end + + %% compile mex + for ii=1:length(mex_files) + disp(['compiling ', mex_files{ii}]) + if is_octave() + % mkoctfile -p CFLAGS + mex(acados_include, template_lib_include, external_include, blasfeo_include, hpipm_include,... + acados_lib_path, template_lib_path, '-lacados', '-lhpipm', '-lblasfeo', mex_files{ii}) + else + if ismac() + FLAGS = 'CFLAGS=$CFLAGS -std=c99'; + else + FLAGS = 'CFLAGS=$CFLAGS -std=c99 -fopenmp'; + end + mex(FLAGS, acados_include, template_lib_include, external_include, blasfeo_include, hpipm_include,... + acados_lib_path, template_lib_path, '-lacados', '-lhpipm', '-lblasfeo', mex_files{ii}) + end + end + + +end \ No newline at end of file diff --git a/third_party/acados/acados_template/c_templates_tera/make_mex.in.m b/third_party/acados/acados_template/c_templates_tera/make_mex.in.m new file mode 100644 index 0000000000..cde30f6f41 --- /dev/null +++ b/third_party/acados/acados_template/c_templates_tera/make_mex.in.m @@ -0,0 +1,110 @@ +% +% Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +% Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +% Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +% Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +% +% This file is part of acados. +% +% The 2-Clause BSD License +% +% Redistribution and use in source and binary forms, with or without +% modification, are permitted provided that the following conditions are met: +% +% 1. Redistributions of source code must retain the above copyright notice, +% this list of conditions and the following disclaimer. +% +% 2. Redistributions in binary form must reproduce the above copyright notice, +% this list of conditions and the following disclaimer in the documentation +% and/or other materials provided with the distribution. +% +% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +% POSSIBILITY OF SUCH DAMAGE.; +% + +function make_mex_{{ model.name }}() + + opts.output_dir = pwd; + + % get acados folder + acados_folder = getenv('ACADOS_INSTALL_DIR'); + + % set paths + acados_include = ['-I' fullfile(acados_folder, 'include')]; + template_lib_include = ['-l' 'acados_ocp_solver_{{ model.name }}']; + template_lib_path = ['-L' fullfile(pwd)]; + + acados_lib_path = ['-L' fullfile(acados_folder, 'lib')]; + external_include = ['-I', fullfile(acados_folder, 'external')]; + blasfeo_include = ['-I', fullfile(acados_folder, 'external', 'blasfeo', 'include')]; + hpipm_include = ['-I', fullfile(acados_folder, 'external', 'hpipm', 'include')]; + + mex_include = ['-I', fullfile(acados_folder, 'interfaces', 'acados_matlab_octave')]; + + mex_names = { ... + 'acados_mex_create_{{ model.name }}' ... + 'acados_mex_free_{{ model.name }}' ... + 'acados_mex_solve_{{ model.name }}' ... + 'acados_mex_set_{{ model.name }}' ... + }; + + mex_files = cell(length(mex_names), 1); + for k=1:length(mex_names) + mex_files{k} = fullfile([mex_names{k}, '.c']); + end + + %% octave C flags + if is_octave() + if ~exist(fullfile(opts.output_dir, 'cflags_octave.txt'), 'file') + diary(fullfile(opts.output_dir, 'cflags_octave.txt')); + diary on + mkoctfile -p CFLAGS + diary off + input_file = fopen(fullfile(opts.output_dir, 'cflags_octave.txt'), 'r'); + cflags_tmp = fscanf(input_file, '%[^\n]s'); + fclose(input_file); + if ~ismac() + cflags_tmp = [cflags_tmp, ' -std=c99 -fopenmp']; + else + cflags_tmp = [cflags_tmp, ' -std=c99']; + end + input_file = fopen(fullfile(opts.output_dir, 'cflags_octave.txt'), 'w'); + fprintf(input_file, '%s', cflags_tmp); + fclose(input_file); + end + % read cflags from file + input_file = fopen(fullfile(opts.output_dir, 'cflags_octave.txt'), 'r'); + cflags_tmp = fscanf(input_file, '%[^\n]s'); + fclose(input_file); + setenv('CFLAGS', cflags_tmp); + end + + %% compile mex + for ii=1:length(mex_files) + disp(['compiling ', mex_files{ii}]) + if is_octave() + % mkoctfile -p CFLAGS + mex(acados_include, template_lib_include, external_include, blasfeo_include, hpipm_include,... + acados_lib_path, template_lib_path, mex_include, '-lacados', '-lhpipm', '-lblasfeo', mex_files{ii}) + else + if ismac() + FLAGS = 'CFLAGS=$CFLAGS -std=c99'; + else + FLAGS = 'CFLAGS=$CFLAGS -std=c99 -fopenmp'; + end + mex(FLAGS, acados_include, template_lib_include, external_include, blasfeo_include, hpipm_include,... + acados_lib_path, template_lib_path, mex_include, '-lacados', '-lhpipm', '-lblasfeo', mex_files{ii}) + end + end + + +end \ No newline at end of file diff --git a/third_party/acados/acados_template/c_templates_tera/make_sfun.in.m b/third_party/acados/acados_template/c_templates_tera/make_sfun.in.m new file mode 100644 index 0000000000..77603a78fa --- /dev/null +++ b/third_party/acados/acados_template/c_templates_tera/make_sfun.in.m @@ -0,0 +1,344 @@ +% +% Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +% Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +% Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +% Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +% +% This file is part of acados. +% +% The 2-Clause BSD License +% +% Redistribution and use in source and binary forms, with or without +% modification, are permitted provided that the following conditions are met: +% +% 1. Redistributions of source code must retain the above copyright notice, +% this list of conditions and the following disclaimer. +% +% 2. Redistributions in binary form must reproduce the above copyright notice, +% this list of conditions and the following disclaimer in the documentation +% and/or other materials provided with the distribution. +% +% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +% POSSIBILITY OF SUCH DAMAGE.; +% + +SOURCES = { ... + {%- if solver_options.integrator_type == 'ERK' %} + '{{ model.name }}_model/{{ model.name }}_expl_ode_fun.c', ... + '{{ model.name }}_model/{{ model.name }}_expl_vde_forw.c',... + {%- if solver_options.hessian_approx == 'EXACT' %} + '{{ model.name }}_model/{{ model.name }}_expl_ode_hess.c',... + {%- endif %} + {%- elif solver_options.integrator_type == "IRK" %} + '{{ model.name }}_model/{{ model.name }}_impl_dae_fun.c', ... + '{{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_z.c', ... + '{{ model.name }}_model/{{ model.name }}_impl_dae_jac_x_xdot_u_z.c', ... + {%- if solver_options.hessian_approx == 'EXACT' %} + '{{ model.name }}_model/{{ model.name }}_impl_dae_hess.c',... + {%- endif %} + {%- elif solver_options.integrator_type == "GNSF" %} + {% if model.gnsf.purely_linear != 1 %} + '{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.c',... + '{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun_jac_y.c',... + '{{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.c',... + {% if model.gnsf.nontrivial_f_LO == 1 %} + '{{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c',... + {%- endif %} + {%- endif %} + '{{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.c',... + {%- elif solver_options.integrator_type == "DISCRETE" %} + '{{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun.c',... + '{{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac.c',... + {%- if solver_options.hessian_approx == "EXACT" %} + '{{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac_hess.c',... + {%- endif %} + {%- endif %} + {%- if cost.cost_type_0 == "NONLINEAR_LS" %} + '{{ model.name }}_cost/{{ model.name }}_cost_y_0_fun.c',... + '{{ model.name }}_cost/{{ model.name }}_cost_y_0_fun_jac_ut_xt.c',... + '{{ model.name }}_cost/{{ model.name }}_cost_y_0_hess.c',... + {%- elif cost.cost_type_0 == "EXTERNAL" %} + '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun.c',... + '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac.c',... + '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac_hess.c',... + {%- endif %} + + {%- if cost.cost_type == "NONLINEAR_LS" %} + '{{ model.name }}_cost/{{ model.name }}_cost_y_fun.c',... + '{{ model.name }}_cost/{{ model.name }}_cost_y_fun_jac_ut_xt.c',... + '{{ model.name }}_cost/{{ model.name }}_cost_y_hess.c',... + {%- elif cost.cost_type == "EXTERNAL" %} + '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun.c',... + '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac.c',... + '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac_hess.c',... + {%- endif %} + {%- if cost.cost_type_e == "NONLINEAR_LS" %} + '{{ model.name }}_cost/{{ model.name }}_cost_y_e_fun.c',... + '{{ model.name }}_cost/{{ model.name }}_cost_y_e_fun_jac_ut_xt.c',... + '{{ model.name }}_cost/{{ model.name }}_cost_y_e_hess.c',... + {%- elif cost.cost_type_e == "EXTERNAL" %} + '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun.c',... + '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac.c',... + '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac_hess.c',... + {%- endif %} + {%- if constraints.constr_type == "BGH" and dims.nh > 0 %} + '{{ model.name }}_constraints/{{ model.name }}_constr_h_fun.c', ... + '{{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt_hess.c', ... + '{{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt.c', ... + {%- elif constraints.constr_type == "BGP" and dims.nphi > 0 %} + '{{ model.name }}_constraints/{{ model.name }}_phi_constraint.c', ... + {%- endif %} + {%- if constraints.constr_type_e == "BGH" and dims.nh_e > 0 %} + '{{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun.c', ... + '{{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess.c', ... + '{{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt.c', ... + {%- elif constraints.constr_type_e == "BGP" and dims.nphi_e > 0 %} + '{{ model.name }}_constraints/{{ model.name }}_phi_e_constraint.c', ... + {%- endif %} + 'acados_solver_sfunction_{{ model.name }}.c', ... + 'acados_solver_{{ model.name }}.c' + }; + +INC_PATH = '{{ acados_include_path }}'; + +INCS = {['-I', fullfile(INC_PATH, 'blasfeo', 'include')], ... + ['-I', fullfile(INC_PATH, 'hpipm', 'include')], ... + ['-I', fullfile(INC_PATH, 'acados')], ... + ['-I', fullfile(INC_PATH)]}; + +{% if solver_options.qp_solver is containing("QPOASES") %} +INCS{end+1} = ['-I', fullfile(INC_PATH, 'qpOASES_e')]; +{% endif %} + +CFLAGS = 'CFLAGS=$CFLAGS'; +LDFLAGS = 'LDFLAGS=$LDFLAGS'; +COMPFLAGS = 'COMPFLAGS=$COMPFLAGS'; +COMPDEFINES = 'COMPDEFINES=$COMPDEFINES'; + +{% if solver_options.qp_solver is containing("QPOASES") %} +CFLAGS = [ CFLAGS, ' -DACADOS_WITH_QPOASES ' ]; +COMPDEFINES = [ COMPDEFINES, ' -DACADOS_WITH_QPOASES ' ]; +{%- elif solver_options.qp_solver is containing("OSQP") %} +CFLAGS = [ CFLAGS, ' -DACADOS_WITH_OSQP ' ]; +COMPDEFINES = [ COMPDEFINES, ' -DACADOS_WITH_OSQP ' ]; +{%- elif solver_options.qp_solver is containing("QPDUNES") %} +CFLAGS = [ CFLAGS, ' -DACADOS_WITH_QPDUNES ' ]; +COMPDEFINES = [ COMPDEFINES, ' -DACADOS_WITH_QPDUNES ' ]; +{%- elif solver_options.qp_solver is containing("HPMPC") %} +CFLAGS = [ CFLAGS, ' -DACADOS_WITH_HPMPC ' ]; +COMPDEFINES = [ COMPDEFINES, ' -DACADOS_WITH_HPMPC ' ]; +{% endif %} + +LIB_PATH = ['-L', fullfile('{{ acados_lib_path }}')]; + +LIBS = {'-lacados', '-lhpipm', '-lblasfeo'}; + +% acados linking libraries and flags +{%- if acados_link_libs and os and os == "pc" %} +LDFLAGS = [LDFLAGS ' {{ acados_link_libs.openmp }}']; +COMPFLAGS = [COMPFLAGS ' {{ acados_link_libs.openmp }}']; +LIBS{end+1} = '{{ acados_link_libs.qpoases }}'; +LIBS{end+1} = '{{ acados_link_libs.hpmpc }}'; +LIBS{end+1} = '{{ acados_link_libs.osqp }}'; +{%- else %} + {% if solver_options.qp_solver is containing("QPOASES") %} +LIBS{end+1} = '-lqpOASES_e'; + {% endif %} +{%- endif %} + +mex('-v', '-O', CFLAGS, LDFLAGS, COMPFLAGS, COMPDEFINES, INCS{:}, ... + LIB_PATH, LIBS{:}, SOURCES{:}, ... + '-output', 'acados_solver_sfunction_{{ model.name }}' ); + +fprintf( [ '\n\nSuccessfully created sfunction:\nacados_solver_sfunction_{{ model.name }}', '.', ... + eval('mexext')] ); + + +%% print note on usage of s-function +fprintf('\n\nNote: Usage of Sfunction is as follows:\n') +input_note = 'Inputs are:\n'; +i_in = 1; + + +{%- if dims.nbx_0 > 0 and simulink_opts.inputs.lbx_0 -%} {#- lbx_0 #} +input_note = strcat(input_note, num2str(i_in), ') lbx_0 - lower bound on x for stage 0,',... + ' size [{{ dims.nbx_0 }}]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if dims.nbx_0 > 0 and simulink_opts.inputs.ubx_0 -%} {#- ubx_0 #} +input_note = strcat(input_note, num2str(i_in), ') ubx_0 - upper bound on x for stage 0,',... + ' size [{{ dims.nbx_0 }}]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if dims.np > 0 and simulink_opts.inputs.parameter_traj -%} {#- parameter_traj #} +input_note = strcat(input_note, num2str(i_in), ') parameters - concatenated for all shooting nodes 0 to N+1,',... + ' size [{{ (dims.N+1)*dims.np }}]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if dims.ny_0 > 0 and simulink_opts.inputs.y_ref_0 %} +input_note = strcat(input_note, num2str(i_in), ') y_ref_0, size [{{ dims.ny_0 }}]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if dims.ny > 0 and dims.N > 1 and simulink_opts.inputs.y_ref %} +input_note = strcat(input_note, num2str(i_in), ') y_ref - concatenated for shooting nodes 1 to N-1,',... + ' size [{{ (dims.N-1) * dims.ny }}]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if dims.ny_e > 0 and dims.N > 0 and simulink_opts.inputs.y_ref_e %} +input_note = strcat(input_note, num2str(i_in), ') y_ref_e, size [{{ dims.ny_e }}]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.lbx -%} {#- lbx #} +input_note = strcat(input_note, num2str(i_in), ') lbx for shooting nodes 1 to N-1, size [{{ (dims.N-1) * dims.nbx }}]\n '); +i_in = i_in + 1; +{%- endif %} +{%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.ubx -%} {#- ubx #} +input_note = strcat(input_note, num2str(i_in), ') ubx for shooting nodes 1 to N-1, size [{{ (dims.N-1) * dims.nbx }}]\n '); +i_in = i_in + 1; +{%- endif %} + + +{%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.lbx_e -%} {#- lbx_e #} +input_note = strcat(input_note, num2str(i_in), ') lbx_e (lbx at shooting node N), size [{{ dims.nbx_e }}]\n '); +i_in = i_in + 1; +{%- endif %} +{%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.ubx_e -%} {#- ubx_e #} +input_note = strcat(input_note, num2str(i_in), ') ubx_e (ubx at shooting node N), size [{{ dims.nbx_e }}]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.lbu -%} {#- lbu #} +input_note = strcat(input_note, num2str(i_in), ') lbu for shooting nodes 0 to N-1, size [{{ dims.N*dims.nbu }}]\n '); +i_in = i_in + 1; +{%- endif -%} +{%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.ubu -%} {#- ubu #} +input_note = strcat(input_note, num2str(i_in), ') ubu for shooting nodes 0 to N-1, size [{{ dims.N*dims.nbu }}]\n '); +i_in = i_in + 1; +{%- endif -%} + +{%- if dims.ng > 0 and simulink_opts.inputs.lg -%} {#- lg #} +input_note = strcat(input_note, num2str(i_in), ') lg, size [{{ dims.ng }}]\n '); +i_in = i_in + 1; +{%- endif %} +{%- if dims.ng > 0 and simulink_opts.inputs.ug -%} {#- ug #} +input_note = strcat(input_note, num2str(i_in), ') ug, size [{{ dims.ng }}]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if dims.nh > 0 and simulink_opts.inputs.lh -%} {#- lh #} +input_note = strcat(input_note, num2str(i_in), ') lh, size [{{ dims.nh }}]\n '); +i_in = i_in + 1; +{%- endif %} +{%- if dims.nh > 0 and simulink_opts.inputs.uh -%} {#- uh #} +input_note = strcat(input_note, num2str(i_in), ') uh, size [{{ dims.nh }}]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if dims.ny_0 > 0 and simulink_opts.inputs.cost_W_0 %} {#- cost_W_0 #} +input_note = strcat(input_note, num2str(i_in), ') cost_W_0 in column-major format, size [{{ dims.ny_0 * dims.ny_0 }}]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if dims.ny > 0 and simulink_opts.inputs.cost_W %} {#- cost_W #} +input_note = strcat(input_note, num2str(i_in), ') cost_W in column-major format, that is set for all intermediate shooting nodes: 1 to N-1, size [{{ dims.ny * dims.ny }}]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if dims.ny_e > 0 and simulink_opts.inputs.cost_W_e %} {#- cost_W_e #} +input_note = strcat(input_note, num2str(i_in), ') cost_W_e in column-major format, size [{{ dims.ny_e * dims.ny_e }}]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if simulink_opts.inputs.reset_solver %} {#- reset_solver #} +input_note = strcat(input_note, num2str(i_in), ') reset_solver determines if iterate is set to all zeros before other initializations (x_init, u_init) are set and before solver is called, size [1]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if simulink_opts.inputs.x_init %} {#- x_init #} +input_note = strcat(input_note, num2str(i_in), ') initialization of x for all shooting nodes, size [{{ dims.nx * (dims.N+1) }}]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if simulink_opts.inputs.u_init %} {#- u_init #} +input_note = strcat(input_note, num2str(i_in), ') initialization of u for shooting nodes 0 to N-1, size [{{ dims.nu * (dims.N) }}]\n '); +i_in = i_in + 1; +{%- endif %} + +fprintf(input_note) + +disp(' ') + +output_note = 'Outputs are:\n'; +i_out = 0; + +{%- if dims.nu > 0 and simulink_opts.outputs.u0 == 1 %} +i_out = i_out + 1; +output_note = strcat(output_note, num2str(i_out), ') u0, control input at node 0, size [{{ dims.nu }}]\n '); +{%- endif %} + +{%- if simulink_opts.outputs.utraj == 1 %} +i_out = i_out + 1; +output_note = strcat(output_note, num2str(i_out), ') utraj, control input concatenated for nodes 0 to N-1, size [{{ dims.nu * dims.N }}]\n '); +{%- endif %} + +{%- if simulink_opts.outputs.xtraj == 1 %} +i_out = i_out + 1; +output_note = strcat(output_note, num2str(i_out), ') xtraj, state concatenated for nodes 0 to N, size [{{ dims.nx * (dims.N + 1) }}]\n '); +{%- endif %} + +{%- if simulink_opts.outputs.solver_status == 1 %} +i_out = i_out + 1; +output_note = strcat(output_note, num2str(i_out), ') acados solver status (0 = SUCCESS)\n '); +{%- endif %} + +{%- if simulink_opts.outputs.KKT_residual == 1 %} +i_out = i_out + 1; +output_note = strcat(output_note, num2str(i_out), ') KKT residual\n '); +{%- endif %} + +{%- if dims.N > 0 and simulink_opts.outputs.x1 == 1 %} +i_out = i_out + 1; +output_note = strcat(output_note, num2str(i_out), ') x1, state at node 1\n '); +{%- endif %} + +{%- if simulink_opts.outputs.CPU_time == 1 %} +i_out = i_out + 1; +output_note = strcat(output_note, num2str(i_out), ') CPU time\n '); +{%- endif %} + +{%- if simulink_opts.outputs.CPU_time_sim == 1 %} +i_out = i_out + 1; +output_note = strcat(output_note, num2str(i_out), ') CPU time integrator\n '); +{%- endif %} + +{%- if simulink_opts.outputs.CPU_time_qp == 1 %} +i_out = i_out + 1; +output_note = strcat(output_note, num2str(i_out), ') CPU time QP solution\n '); +{%- endif %} + +{%- if simulink_opts.outputs.CPU_time_lin == 1 %} +i_out = i_out + 1; +output_note = strcat(output_note, num2str(i_out), ') CPU time linearization (including integrator)\n '); +{%- endif %} + +{%- if simulink_opts.outputs.sqp_iter == 1 %} +i_out = i_out + 1; +output_note = strcat(output_note, num2str(i_out), ') SQP iterations\n '); +{%- endif %} + +fprintf(output_note) diff --git a/third_party/acados/acados_template/c_templates_tera/make_sfun_sim.in.m b/third_party/acados/acados_template/c_templates_tera/make_sfun_sim.in.m new file mode 100644 index 0000000000..a0c503e41a --- /dev/null +++ b/third_party/acados/acados_template/c_templates_tera/make_sfun_sim.in.m @@ -0,0 +1,103 @@ +% +% Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +% Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +% Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +% Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +% +% This file is part of acados. +% +% The 2-Clause BSD License +% +% Redistribution and use in source and binary forms, with or without +% modification, are permitted provided that the following conditions are met: +% +% 1. Redistributions of source code must retain the above copyright notice, +% this list of conditions and the following disclaimer. +% +% 2. Redistributions in binary form must reproduce the above copyright notice, +% this list of conditions and the following disclaimer in the documentation +% and/or other materials provided with the distribution. +% +% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +% POSSIBILITY OF SUCH DAMAGE.; +% + +SOURCES = [ 'acados_sim_solver_sfunction_{{ model.name }}.c ', ... + 'acados_sim_solver_{{ model.name }}.c ', ... + {%- if solver_options.integrator_type == 'ERK' %} + '{{ model.name }}_model/{{ model.name }}_expl_ode_fun.c ', ... + '{{ model.name }}_model/{{ model.name }}_expl_vde_forw.c ',... + {%- if solver_options.hessian_approx == 'EXACT' %} + '{{ model.name }}_model/{{ model.name }}_expl_ode_hess.c ',... + {%- endif %} + {%- elif solver_options.integrator_type == "IRK" %} + '{{ model.name }}_model/{{ model.name }}_impl_dae_fun.c ', ... + '{{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_z.c ', ... + '{{ model.name }}_model/{{ model.name }}_impl_dae_jac_x_xdot_u_z.c ', ... + {%- if solver_options.hessian_approx == 'EXACT' %} + '{{ model.name }}_model/{{ model.name }}_impl_dae_hess.c ',... + {%- endif %} + {%- elif solver_options.integrator_type == "GNSF" %} + {% if model.gnsf.purely_linear != 1 %} + '{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.c ' + '{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun_jac_y.c ' + '{{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.c ' + {% if model.gnsf.nontrivial_f_LO == 1 %} + '{{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c ' + {%- endif %} + {%- endif %} + '{{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.c ' + {%- endif %} + ]; + +INC_PATH = '{{ acados_include_path }}'; + +INCS = [ ' -I', fullfile(INC_PATH, 'blasfeo', 'include'), ... + ' -I', fullfile(INC_PATH, 'hpipm', 'include'), ... + ' -I', INC_PATH, ' -I', fullfile(INC_PATH, 'acados'), ' ']; + +CFLAGS = ' -O'; + +LIB_PATH = '{{ acados_lib_path }}'; + +LIBS = '-lacados -lblasfeo -lhpipm'; + +eval( [ 'mex -v -output acados_sim_solver_sfunction_{{ model.name }} ', ... + CFLAGS, INCS, ' ', SOURCES, ' -L', LIB_PATH, ' ', LIBS ]); + +fprintf( [ '\n\nSuccessfully created sfunction:\nacados_sim_solver_sfunction_{{ model.name }}', '.', ... + eval('mexext')] ); + + +%% print note on usage of s-function +fprintf('\n\nNote: Usage of Sfunction is as follows:\n') +input_note = 'Inputs are:\n1) x0, initial state, size [{{ dims.nx }}]\n '; +i_in = 2; +{%- if dims.nu > 0 %} +input_note = strcat(input_note, num2str(i_in), ') u, size [{{ dims.nu }}]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if dims.np > 0 %} +input_note = strcat(input_note, num2str(i_in), ') parameters, size [{{ dims.np }}]\n '); +i_in = i_in + 1; +{%- endif %} + + +fprintf(input_note) + +disp(' ') + +output_note = strcat('Outputs are:\n', ... + '1) x1 - simulated state, size [{{ dims.nx }}]\n'); + +fprintf(output_note) diff --git a/third_party/acados/acados_template/c_templates_tera/mex_solver.in.m b/third_party/acados/acados_template/c_templates_tera/mex_solver.in.m new file mode 100644 index 0000000000..278e0a605c --- /dev/null +++ b/third_party/acados/acados_template/c_templates_tera/mex_solver.in.m @@ -0,0 +1,166 @@ +% +% Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +% Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +% Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +% Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +% +% This file is part of acados. +% +% The 2-Clause BSD License +% +% Redistribution and use in source and binary forms, with or without +% modification, are permitted provided that the following conditions are met: +% +% 1. Redistributions of source code must retain the above copyright notice, +% this list of conditions and the following disclaimer. +% +% 2. Redistributions in binary form must reproduce the above copyright notice, +% this list of conditions and the following disclaimer in the documentation +% and/or other materials provided with the distribution. +% +% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +% POSSIBILITY OF SUCH DAMAGE.; +% + +classdef {{ model.name }}_mex_solver < handle + + properties + C_ocp + C_ocp_ext_fun + cost_ext_fun_type + cost_ext_fun_type_e + end % properties + + + + methods + + % constructor + function obj = {{ model.name }}_mex_solver() + make_mex_{{ model.name }}(); + [obj.C_ocp, obj.C_ocp_ext_fun] = acados_mex_create_{{ model.name }}(); + % to have path to destructor when changing directory + addpath('.') + obj.cost_ext_fun_type = '{{ cost.cost_ext_fun_type }}'; + obj.cost_ext_fun_type_e = '{{ cost.cost_ext_fun_type_e }}'; + end + + % destructor + function delete(obj) + if ~isempty(obj.C_ocp) + acados_mex_free_{{ model.name }}(obj.C_ocp); + end + end + + % solve + function solve(obj) + acados_mex_solve_{{ model.name }}(obj.C_ocp); + end + + % set -- borrowed from MEX interface + function set(varargin) + obj = varargin{1}; + field = varargin{2}; + value = varargin{3}; + if ~isa(field, 'char') + error('field must be a char vector, use '' '''); + end + if nargin==3 + acados_mex_set_{{ model.name }}(obj.cost_ext_fun_type, obj.cost_ext_fun_type_e, obj.C_ocp, obj.C_ocp_ext_fun, field, value); + elseif nargin==4 + stage = varargin{4}; + acados_mex_set_{{ model.name }}(obj.cost_ext_fun_type, obj.cost_ext_fun_type_e, obj.C_ocp, obj.C_ocp_ext_fun, field, value, stage); + else + disp('acados_ocp.set: wrong number of input arguments (2 or 3 allowed)'); + end + end + + function value = get_cost(obj) + value = ocp_get_cost(obj.C_ocp); + end + + % get -- borrowed from MEX interface + function value = get(varargin) + % usage: + % obj.get(field, value, [stage]) + obj = varargin{1}; + field = varargin{2}; + if any(strfind('sens', field)) + error('field sens* (sensitivities of optimal solution) not yet supported for templated MEX.') + end + if ~isa(field, 'char') + error('field must be a char vector, use '' '''); + end + + if nargin==2 + value = ocp_get(obj.C_ocp, field); + elseif nargin==3 + stage = varargin{3}; + value = ocp_get(obj.C_ocp, field, stage); + else + disp('acados_ocp.get: wrong number of input arguments (1 or 2 allowed)'); + end + end + + + % print + function print(varargin) + if nargin < 2 + field = 'stat'; + else + field = varargin{2}; + end + + obj = varargin{1}; + + if strcmp(field, 'stat') + stat = obj.get('stat'); + {%- if solver_options.nlp_solver_type == "SQP" %} + fprintf('\niter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter\talpha'); + if size(stat,2)>8 + fprintf('\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp'); + end + fprintf('\n'); + for jj=1:size(stat,1) + fprintf('%d\t%e\t%e\t%e\t%e\t%d\t%d\t%e', stat(jj,1), stat(jj,2), stat(jj,3), stat(jj,4), stat(jj,5), stat(jj,6), stat(jj,7), stat(jj, 8)); + if size(stat,2)>8 + fprintf('\t%e\t%e\t%e\t%e', stat(jj,9), stat(jj,10), stat(jj,11), stat(jj,12)); + end + fprintf('\n'); + end + fprintf('\n'); + {%- else %} + fprintf('\niter\tqp_status\tqp_iter'); + if size(stat,2)>3 + fprintf('\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp'); + end + fprintf('\n'); + for jj=1:size(stat,1) + fprintf('%d\t%d\t\t%d', stat(jj,1), stat(jj,2), stat(jj,3)); + if size(stat,2)>3 + fprintf('\t%e\t%e\t%e\t%e', stat(jj,4), stat(jj,5), stat(jj,6), stat(jj,7)); + end + fprintf('\n'); + end + {% endif %} + + else + fprintf('unsupported field in function print of acados_ocp.print, got %s', field); + keyboard + end + + end + + end % methods + +end % class + diff --git a/third_party/acados/acados_template/c_templates_tera/model.in.h b/third_party/acados/acados_template/c_templates_tera/model.in.h new file mode 100644 index 0000000000..918e2bc29e --- /dev/null +++ b/third_party/acados/acados_template/c_templates_tera/model.in.h @@ -0,0 +1,211 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + +#ifndef {{ model.name }}_MODEL +#define {{ model.name }}_MODEL + +#ifdef __cplusplus +extern "C" { +#endif + +{%- if solver_options.hessian_approx %} + {%- set hessian_approx = solver_options.hessian_approx %} +{%- elif solver_options.sens_hess %} + {%- set hessian_approx = "EXACT" %} +{%- else %} + {%- set hessian_approx = "GAUSS_NEWTON" %} +{%- endif %} + +{% if solver_options.integrator_type == "IRK" or solver_options.integrator_type == "LIFTED_IRK" %} +// implicit ODE +int {{ model.name }}_impl_dae_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_impl_dae_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_impl_dae_fun_sparsity_in(int); +const int *{{ model.name }}_impl_dae_fun_sparsity_out(int); +int {{ model.name }}_impl_dae_fun_n_in(void); +int {{ model.name }}_impl_dae_fun_n_out(void); + +// implicit ODE +int {{ model.name }}_impl_dae_fun_jac_x_xdot_z(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_impl_dae_fun_jac_x_xdot_z_work(int *, int *, int *, int *); +const int *{{ model.name }}_impl_dae_fun_jac_x_xdot_z_sparsity_in(int); +const int *{{ model.name }}_impl_dae_fun_jac_x_xdot_z_sparsity_out(int); +int {{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_in(void); +int {{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_out(void); + +// implicit ODE +int {{ model.name }}_impl_dae_jac_x_xdot_u_z(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_impl_dae_jac_x_xdot_u_z_work(int *, int *, int *, int *); +const int *{{ model.name }}_impl_dae_jac_x_xdot_u_z_sparsity_in(int); +const int *{{ model.name }}_impl_dae_jac_x_xdot_u_z_sparsity_out(int); +int {{ model.name }}_impl_dae_jac_x_xdot_u_z_n_in(void); +int {{ model.name }}_impl_dae_jac_x_xdot_u_z_n_out(void); + +// implicit ODE - for lifted_irk +int {{ model.name }}_impl_dae_fun_jac_x_xdot_u(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_impl_dae_fun_jac_x_xdot_u_work(int *, int *, int *, int *); +const int *{{ model.name }}_impl_dae_fun_jac_x_xdot_u_sparsity_in(int); +const int *{{ model.name }}_impl_dae_fun_jac_x_xdot_u_sparsity_out(int); +int {{ model.name }}_impl_dae_fun_jac_x_xdot_u_n_in(void); +int {{ model.name }}_impl_dae_fun_jac_x_xdot_u_n_out(void); + +{%- if hessian_approx == "EXACT" %} +int {{ model.name }}_impl_dae_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_impl_dae_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_impl_dae_hess_sparsity_in(int); +const int *{{ model.name }}_impl_dae_hess_sparsity_out(int); +int {{ model.name }}_impl_dae_hess_n_in(void); +int {{ model.name }}_impl_dae_hess_n_out(void); +{%- endif %} + +{% elif solver_options.integrator_type == "GNSF" %} +/* GNSF Functions */ + {% if model.gnsf.purely_linear != 1 %} +// phi_fun +int {{ model.name }}_gnsf_phi_fun(const double** arg, double** res, int* iw, double* w, void *mem); +int {{ model.name }}_gnsf_phi_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_gnsf_phi_fun_sparsity_in(int); +const int *{{ model.name }}_gnsf_phi_fun_sparsity_out(int); +int {{ model.name }}_gnsf_phi_fun_n_in(void); +int {{ model.name }}_gnsf_phi_fun_n_out(void); + +// phi_fun_jac_y +int {{ model.name }}_gnsf_phi_fun_jac_y(const double** arg, double** res, int* iw, double* w, void *mem); +int {{ model.name }}_gnsf_phi_fun_jac_y_work(int *, int *, int *, int *); +const int *{{ model.name }}_gnsf_phi_fun_jac_y_sparsity_in(int); +const int *{{ model.name }}_gnsf_phi_fun_jac_y_sparsity_out(int); +int {{ model.name }}_gnsf_phi_fun_jac_y_n_in(void); +int {{ model.name }}_gnsf_phi_fun_jac_y_n_out(void); + +// phi_jac_y_uhat +int {{ model.name }}_gnsf_phi_jac_y_uhat(const double** arg, double** res, int* iw, double* w, void *mem); +int {{ model.name }}_gnsf_phi_jac_y_uhat_work(int *, int *, int *, int *); +const int *{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_in(int); +const int *{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_out(int); +int {{ model.name }}_gnsf_phi_jac_y_uhat_n_in(void); +int {{ model.name }}_gnsf_phi_jac_y_uhat_n_out(void); + {% if model.gnsf.nontrivial_f_LO == 1 %} +// f_lo_fun_jac_x1k1uz +int {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz(const double** arg, double** res, int* iw, double* w, void *mem); +int {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_work(int *, int *, int *, int *); +const int *{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_in(int); +const int *{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_out(int); +int {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_in(void); +int {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_out(void); + {%- endif %} + {%- endif %} +// used to import model matrices +int {{ model.name }}_gnsf_get_matrices_fun(const double** arg, double** res, int* iw, double* w, void *mem); +int {{ model.name }}_gnsf_get_matrices_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_gnsf_get_matrices_fun_sparsity_in(int); +const int *{{ model.name }}_gnsf_get_matrices_fun_sparsity_out(int); +int {{ model.name }}_gnsf_get_matrices_fun_n_in(void); +int {{ model.name }}_gnsf_get_matrices_fun_n_out(void); + +{% elif solver_options.integrator_type == "ERK" %} +/* explicit ODE */ + +// explicit ODE +int {{ model.name }}_expl_ode_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_expl_ode_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_expl_ode_fun_sparsity_in(int); +const int *{{ model.name }}_expl_ode_fun_sparsity_out(int); +int {{ model.name }}_expl_ode_fun_n_in(void); +int {{ model.name }}_expl_ode_fun_n_out(void); + +// explicit forward VDE +int {{ model.name }}_expl_vde_forw(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_expl_vde_forw_work(int *, int *, int *, int *); +const int *{{ model.name }}_expl_vde_forw_sparsity_in(int); +const int *{{ model.name }}_expl_vde_forw_sparsity_out(int); +int {{ model.name }}_expl_vde_forw_n_in(void); +int {{ model.name }}_expl_vde_forw_n_out(void); + +// explicit adjoint VDE +int {{ model.name }}_expl_vde_adj(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_expl_vde_adj_work(int *, int *, int *, int *); +const int *{{ model.name }}_expl_vde_adj_sparsity_in(int); +const int *{{ model.name }}_expl_vde_adj_sparsity_out(int); +int {{ model.name }}_expl_vde_adj_n_in(void); +int {{ model.name }}_expl_vde_adj_n_out(void); + +{%- if hessian_approx == "EXACT" %} +int {{ model.name }}_expl_ode_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_expl_ode_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_expl_ode_hess_sparsity_in(int); +const int *{{ model.name }}_expl_ode_hess_sparsity_out(int); +int {{ model.name }}_expl_ode_hess_n_in(void); +int {{ model.name }}_expl_ode_hess_n_out(void); +{%- endif %} + +{% elif solver_options.integrator_type == "DISCRETE" %} + +{% if model.dyn_ext_fun_type == "casadi" %} +int {{ model.name }}_dyn_disc_phi_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_dyn_disc_phi_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_dyn_disc_phi_fun_sparsity_in(int); +const int *{{ model.name }}_dyn_disc_phi_fun_sparsity_out(int); +int {{ model.name }}_dyn_disc_phi_fun_n_in(void); +int {{ model.name }}_dyn_disc_phi_fun_n_out(void); + +int {{ model.name }}_dyn_disc_phi_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_dyn_disc_phi_fun_jac_work(int *, int *, int *, int *); +const int *{{ model.name }}_dyn_disc_phi_fun_jac_sparsity_in(int); +const int *{{ model.name }}_dyn_disc_phi_fun_jac_sparsity_out(int); +int {{ model.name }}_dyn_disc_phi_fun_jac_n_in(void); +int {{ model.name }}_dyn_disc_phi_fun_jac_n_out(void); + +{%- if hessian_approx == "EXACT" %} +int {{ model.name }}_dyn_disc_phi_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_dyn_disc_phi_fun_jac_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_dyn_disc_phi_fun_jac_hess_sparsity_in(int); +const int *{{ model.name }}_dyn_disc_phi_fun_jac_hess_sparsity_out(int); +int {{ model.name }}_dyn_disc_phi_fun_jac_hess_n_in(void); +int {{ model.name }}_dyn_disc_phi_fun_jac_hess_n_out(void); +{%- endif %} +{% else %} + {%- if hessian_approx == "EXACT" %} +int {{ model.dyn_disc_fun_jac_hess }}(void **, void **, void *); + {% endif %} +int {{ model.dyn_disc_fun_jac }}(void **, void **, void *); +int {{ model.dyn_disc_fun }}(void **, void **, void *); +{% endif %} + + +{% endif %} + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // {{ model.name }}_MODEL diff --git a/third_party/acados/acados_template/c_templates_tera/phi_constraint.in.h b/third_party/acados/acados_template/c_templates_tera/phi_constraint.in.h new file mode 100644 index 0000000000..283ed7f889 --- /dev/null +++ b/third_party/acados/acados_template/c_templates_tera/phi_constraint.in.h @@ -0,0 +1,55 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + +#ifndef {{ model.name }}_PHI_CONSTRAINT +#define {{ model.name }}_PHI_CONSTRAINT + +#ifdef __cplusplus +extern "C" { +#endif + +{% if dims.nphi > 0 %} +// implicit ODE +int {{ model.name }}_phi_constraint(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_phi_constraint_work(int *, int *, int *, int *); +const int *{{ model.name }}_phi_constraint_sparsity_in(int); +const int *{{ model.name }}_phi_constraint_sparsity_out(int); +int {{ model.name }}_phi_constraint_n_in(void); +int {{ model.name }}_phi_constraint_n_out(void); +{% endif %} + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // {{ model.name }}_PHI_CONSTRAINT diff --git a/third_party/acados/acados_template/c_templates_tera/phi_e_constraint.in.h b/third_party/acados/acados_template/c_templates_tera/phi_e_constraint.in.h new file mode 100644 index 0000000000..dc8e293ad7 --- /dev/null +++ b/third_party/acados/acados_template/c_templates_tera/phi_e_constraint.in.h @@ -0,0 +1,21 @@ +#ifndef {{ model.name }}_PHI_E_CONSTRAINT +#define {{ model.name }}_PHI_E_CONSTRAINT + +#ifdef __cplusplus +extern "C" { +#endif + +{% if dims.nphi_e > 0 %} +int {{ model.name }}_phi_e_constraint(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_phi_e_constraint_work(int *, int *, int *, int *); +const int *{{ model.name }}_phi_e_constraint_sparsity_in(int); +const int *{{ model.name }}_phi_e_constraint_sparsity_out(int); +int {{ model.name }}_phi_e_constraint_n_in(void); +int {{ model.name }}_phi_e_constraint_n_out(void); +{% endif %} + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // {{ model.name }}_PHI_E_CONSTRAINT diff --git a/third_party/acados/acados_template/generate_c_code_constraint.py b/third_party/acados/acados_template/generate_c_code_constraint.py new file mode 100644 index 0000000000..c79ddc129a --- /dev/null +++ b/third_party/acados/acados_template/generate_c_code_constraint.py @@ -0,0 +1,180 @@ +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + +import os +from casadi import * +from .utils import ALLOWED_CASADI_VERSIONS, is_empty, casadi_length, casadi_version_warning + +def generate_c_code_constraint( model, con_name, is_terminal, opts ): + + casadi_version = CasadiMeta.version() + casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double') + + if casadi_version not in (ALLOWED_CASADI_VERSIONS): + casadi_version_warning(casadi_version) + + # load constraint variables and expression + x = model.x + p = model.p + + if isinstance(x, casadi.MX): + symbol = MX.sym + else: + symbol = SX.sym + + if is_terminal: + con_h_expr = model.con_h_expr_e + con_phi_expr = model.con_phi_expr_e + # create dummy u, z + u = symbol('u', 0, 0) + z = symbol('z', 0, 0) + else: + con_h_expr = model.con_h_expr + con_phi_expr = model.con_phi_expr + u = model.u + z = model.z + + if (not is_empty(con_h_expr)) and (not is_empty(con_phi_expr)): + raise Exception("acados: you can either have constraint_h, or constraint_phi, not both.") + + if not (is_empty(con_h_expr) and is_empty(con_phi_expr)): + if is_empty(con_h_expr): + constr_type = 'BGP' + else: + constr_type = 'BGH' + + if is_empty(p): + p = symbol('p', 0, 0) + + if is_empty(z): + z = symbol('z', 0, 0) + + if not (is_empty(con_h_expr)) and opts['generate_hess']: + # multipliers for hessian + nh = casadi_length(con_h_expr) + lam_h = symbol('lam_h', nh, 1) + + # set up & change directory + code_export_dir = opts["code_export_directory"] + if not os.path.exists(code_export_dir): + os.makedirs(code_export_dir) + + cwd = os.getcwd() + os.chdir(code_export_dir) + gen_dir = con_name + '_constraints' + if not os.path.exists(gen_dir): + os.mkdir(gen_dir) + gen_dir_location = os.path.join('.', gen_dir) + os.chdir(gen_dir_location) + + # export casadi functions + if constr_type == 'BGH': + if is_terminal: + fun_name = con_name + '_constr_h_e_fun_jac_uxt_zt' + else: + fun_name = con_name + '_constr_h_fun_jac_uxt_zt' + + jac_ux_t = transpose(jacobian(con_h_expr, vertcat(u,x))) + jac_z_t = jacobian(con_h_expr, z) + constraint_fun_jac_tran = Function(fun_name, [x, u, z, p], \ + [con_h_expr, jac_ux_t, jac_z_t]) + + constraint_fun_jac_tran.generate(fun_name, casadi_opts) + if opts['generate_hess']: + + if is_terminal: + fun_name = con_name + '_constr_h_e_fun_jac_uxt_zt_hess' + else: + fun_name = con_name + '_constr_h_fun_jac_uxt_zt_hess' + + # adjoint + adj_ux = jtimes(con_h_expr, vertcat(u, x), lam_h, True) + # hessian + hess_ux = jacobian(adj_ux, vertcat(u, x)) + + adj_z = jtimes(con_h_expr, z, lam_h, True) + hess_z = jacobian(adj_z, z) + + # set up functions + constraint_fun_jac_tran_hess = \ + Function(fun_name, [x, u, lam_h, z, p], \ + [con_h_expr, jac_ux_t, hess_ux, jac_z_t, hess_z]) + + # generate C code + constraint_fun_jac_tran_hess.generate(fun_name, casadi_opts) + + if is_terminal: + fun_name = con_name + '_constr_h_e_fun' + else: + fun_name = con_name + '_constr_h_fun' + h_fun = Function(fun_name, [x, u, z, p], [con_h_expr]) + h_fun.generate(fun_name, casadi_opts) + + else: # BGP constraint + if is_terminal: + fun_name = con_name + '_phi_e_constraint' + r = model.con_r_in_phi_e + con_r_expr = model.con_r_expr_e + else: + fun_name = con_name + '_phi_constraint' + r = model.con_r_in_phi + con_r_expr = model.con_r_expr + + nphi = casadi_length(con_phi_expr) + con_phi_expr_x_u_z = substitute(con_phi_expr, r, con_r_expr) + phi_jac_u = jacobian(con_phi_expr_x_u_z, u) + phi_jac_x = jacobian(con_phi_expr_x_u_z, x) + phi_jac_z = jacobian(con_phi_expr_x_u_z, z) + + hess = hessian(con_phi_expr[0], r)[0] + for i in range(1, nphi): + hess = vertcat(hess, hessian(con_phi_expr[i], r)[0]) + + r_jac_u = jacobian(con_r_expr, u) + r_jac_x = jacobian(con_r_expr, x) + + constraint_phi = \ + Function(fun_name, [x, u, z, p], \ + [con_phi_expr_x_u_z, \ + vertcat(transpose(phi_jac_u), \ + transpose(phi_jac_x)), \ + transpose(phi_jac_z), \ + hess, vertcat(transpose(r_jac_u), \ + transpose(r_jac_x))]) + + constraint_phi.generate(fun_name, casadi_opts) + + # change directory back + os.chdir(cwd) + + return diff --git a/third_party/acados/acados_template/generate_c_code_discrete_dynamics.py b/third_party/acados/acados_template/generate_c_code_discrete_dynamics.py new file mode 100644 index 0000000000..c6a245ff81 --- /dev/null +++ b/third_party/acados/acados_template/generate_c_code_discrete_dynamics.py @@ -0,0 +1,98 @@ +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES LOSS OF USE, DATA, OR PROFITS OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# + +import os +import casadi as ca +from .utils import ALLOWED_CASADI_VERSIONS, casadi_length, casadi_version_warning + +def generate_c_code_discrete_dynamics( model, opts ): + + casadi_version = ca.CasadiMeta.version() + casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double') + + if casadi_version not in (ALLOWED_CASADI_VERSIONS): + casadi_version_warning(casadi_version) + + # load model + x = model.x + u = model.u + p = model.p + phi = model.disc_dyn_expr + model_name = model.name + nx = casadi_length(x) + + if isinstance(phi, ca.MX): + symbol = ca.MX.sym + elif isinstance(phi, ca.SX): + symbol = ca.SX.sym + else: + Exception("generate_c_code_disc_dyn: disc_dyn_expr must be a CasADi expression, you have type: {}".format(type(phi))) + + # assume nx1 = nx !!! + lam = symbol('lam', nx, 1) + + # generate jacobians + ux = ca.vertcat(u,x) + jac_ux = ca.jacobian(phi, ux) + # generate adjoint + adj_ux = ca.jtimes(phi, ux, lam, True) + # generate hessian + hess_ux = ca.jacobian(adj_ux, ux) + + ## change directory + code_export_dir = opts["code_export_directory"] + if not os.path.exists(code_export_dir): + os.makedirs(code_export_dir) + + cwd = os.getcwd() + os.chdir(code_export_dir) + model_dir = model_name + '_model' + if not os.path.exists(model_dir): + os.mkdir(model_dir) + model_dir_location = os.path.join('.', model_dir) + os.chdir(model_dir_location) + + # set up & generate Functions + fun_name = model_name + '_dyn_disc_phi_fun' + phi_fun = ca.Function(fun_name, [x, u, p], [phi]) + phi_fun.generate(fun_name, casadi_opts) + + fun_name = model_name + '_dyn_disc_phi_fun_jac' + phi_fun_jac_ut_xt = ca.Function(fun_name, [x, u, p], [phi, jac_ux.T]) + phi_fun_jac_ut_xt.generate(fun_name, casadi_opts) + + fun_name = model_name + '_dyn_disc_phi_fun_jac_hess' + phi_fun_jac_ut_xt_hess = ca.Function(fun_name, [x, u, lam, p], [phi, jac_ux.T, hess_ux]) + phi_fun_jac_ut_xt_hess.generate(fun_name, casadi_opts) + + os.chdir(cwd) diff --git a/third_party/acados/acados_template/generate_c_code_explicit_ode.py b/third_party/acados/acados_template/generate_c_code_explicit_ode.py new file mode 100644 index 0000000000..76e6400535 --- /dev/null +++ b/third_party/acados/acados_template/generate_c_code_explicit_ode.py @@ -0,0 +1,124 @@ +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + +import os +from casadi import * +from .utils import ALLOWED_CASADI_VERSIONS, is_empty, casadi_version_warning + +def generate_c_code_explicit_ode( model, opts ): + + casadi_version = CasadiMeta.version() + casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double') + if casadi_version not in (ALLOWED_CASADI_VERSIONS): + casadi_version_warning(casadi_version) + + + generate_hess = opts["generate_hess"] + code_export_dir = opts["code_export_directory"] + + # load model + x = model.x + u = model.u + p = model.p + f_expl = model.f_expl_expr + model_name = model.name + + ## get model dimensions + nx = x.size()[0] + nu = u.size()[0] + + if isinstance(f_expl, casadi.MX): + symbol = MX.sym + elif isinstance(f_expl, casadi.SX): + symbol = SX.sym + else: + raise Exception("Invalid type for f_expl! Possible types are 'SX' and 'MX'. Exiting.") + ## set up functions to be exported + Sx = symbol('Sx', nx, nx) + Sp = symbol('Sp', nx, nu) + lambdaX = symbol('lambdaX', nx, 1) + + fun_name = model_name + '_expl_ode_fun' + + ## Set up functions + expl_ode_fun = Function(fun_name, [x, u, p], [f_expl]) + + vdeX = jtimes(f_expl,x,Sx) + vdeP = jacobian(f_expl,u) + jtimes(f_expl,x,Sp) + + fun_name = model_name + '_expl_vde_forw' + + expl_vde_forw = Function(fun_name, [x, Sx, Sp, u, p], [f_expl, vdeX, vdeP]) + + adj = jtimes(f_expl, vertcat(x, u), lambdaX, True) + + fun_name = model_name + '_expl_vde_adj' + expl_vde_adj = Function(fun_name, [x, lambdaX, u, p], [adj]) + + if generate_hess: + S_forw = vertcat(horzcat(Sx, Sp), horzcat(DM.zeros(nu,nx), DM.eye(nu))) + hess = mtimes(transpose(S_forw),jtimes(adj, vertcat(x,u), S_forw)) + hess2 = [] + for j in range(nx+nu): + for i in range(j,nx+nu): + hess2 = vertcat(hess2, hess[i,j]) + + fun_name = model_name + '_expl_ode_hess' + expl_ode_hess = Function(fun_name, [x, Sx, Sp, lambdaX, u, p], [adj, hess2]) + + ## generate C code + if not os.path.exists(code_export_dir): + os.makedirs(code_export_dir) + + cwd = os.getcwd() + os.chdir(code_export_dir) + model_dir = model_name + '_model' + if not os.path.exists(model_dir): + os.mkdir(model_dir) + model_dir_location = os.path.join('.', model_dir) + os.chdir(model_dir_location) + fun_name = model_name + '_expl_ode_fun' + expl_ode_fun.generate(fun_name, casadi_opts) + + fun_name = model_name + '_expl_vde_forw' + expl_vde_forw.generate(fun_name, casadi_opts) + + fun_name = model_name + '_expl_vde_adj' + expl_vde_adj.generate(fun_name, casadi_opts) + + if generate_hess: + fun_name = model_name + '_expl_ode_hess' + expl_ode_hess.generate(fun_name, casadi_opts) + os.chdir(cwd) + + return diff --git a/third_party/acados/acados_template/generate_c_code_external_cost.py b/third_party/acados/acados_template/generate_c_code_external_cost.py new file mode 100644 index 0000000000..8396522619 --- /dev/null +++ b/third_party/acados/acados_template/generate_c_code_external_cost.py @@ -0,0 +1,116 @@ +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + +import os +from casadi import SX, MX, Function, transpose, vertcat, horzcat, hessian, CasadiMeta +from .utils import ALLOWED_CASADI_VERSIONS, casadi_version_warning + + +def generate_c_code_external_cost(model, stage_type, opts): + + casadi_version = CasadiMeta.version() + casadi_opts = dict(mex=False, casadi_int="int", casadi_real="double") + + if casadi_version not in (ALLOWED_CASADI_VERSIONS): + casadi_version_warning(casadi_version) + + x = model.x + p = model.p + + if isinstance(x, MX): + symbol = MX.sym + else: + symbol = SX.sym + + if stage_type == 'terminal': + suffix_name = "_cost_ext_cost_e_fun" + suffix_name_hess = "_cost_ext_cost_e_fun_jac_hess" + suffix_name_jac = "_cost_ext_cost_e_fun_jac" + u = symbol("u", 0, 0) + ext_cost = model.cost_expr_ext_cost_e + custom_hess = model.cost_expr_ext_cost_custom_hess_e + + elif stage_type == 'path': + suffix_name = "_cost_ext_cost_fun" + suffix_name_hess = "_cost_ext_cost_fun_jac_hess" + suffix_name_jac = "_cost_ext_cost_fun_jac" + u = model.u + ext_cost = model.cost_expr_ext_cost + custom_hess = model.cost_expr_ext_cost_custom_hess + + elif stage_type == 'initial': + suffix_name = "_cost_ext_cost_0_fun" + suffix_name_hess = "_cost_ext_cost_0_fun_jac_hess" + suffix_name_jac = "_cost_ext_cost_0_fun_jac" + u = model.u + ext_cost = model.cost_expr_ext_cost_0 + custom_hess = model.cost_expr_ext_cost_custom_hess_0 + + # set up functions to be exported + fun_name = model.name + suffix_name + fun_name_hess = model.name + suffix_name_hess + fun_name_jac = model.name + suffix_name_jac + + # generate expression for full gradient and Hessian + full_hess, grad = hessian(ext_cost, vertcat(u, x)) + + if custom_hess is not None: + full_hess = custom_hess + + ext_cost_fun = Function(fun_name, [x, u, p], [ext_cost]) + ext_cost_fun_jac_hess = Function( + fun_name_hess, [x, u, p], [ext_cost, grad, full_hess] + ) + ext_cost_fun_jac = Function( + fun_name_jac, [x, u, p], [ext_cost, grad] + ) + + # generate C code + code_export_dir = opts["code_export_directory"] + if not os.path.exists(code_export_dir): + os.makedirs(code_export_dir) + + cwd = os.getcwd() + os.chdir(code_export_dir) + gen_dir = model.name + '_cost' + if not os.path.exists(gen_dir): + os.mkdir(gen_dir) + gen_dir_location = "./" + gen_dir + os.chdir(gen_dir_location) + + ext_cost_fun.generate(fun_name, casadi_opts) + ext_cost_fun_jac_hess.generate(fun_name_hess, casadi_opts) + ext_cost_fun_jac.generate(fun_name_jac, casadi_opts) + + os.chdir(cwd) + return diff --git a/third_party/acados/acados_template/generate_c_code_gnsf.py b/third_party/acados/acados_template/generate_c_code_gnsf.py new file mode 100644 index 0000000000..97acb8e330 --- /dev/null +++ b/third_party/acados/acados_template/generate_c_code_gnsf.py @@ -0,0 +1,131 @@ +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + +import os +from casadi import * +from .utils import ALLOWED_CASADI_VERSIONS, is_empty, casadi_version_warning + +def generate_c_code_gnsf( model, opts ): + + casadi_version = CasadiMeta.version() + casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double') + if casadi_version not in (ALLOWED_CASADI_VERSIONS): + casadi_version_warning(casadi_version) + + model_name = model.name + code_export_dir = opts["code_export_directory"] + + # set up directory + if not os.path.exists(code_export_dir): + os.makedirs(code_export_dir) + + cwd = os.getcwd() + os.chdir(code_export_dir) + model_dir = model_name + '_model' + if not os.path.exists(model_dir): + os.mkdir(model_dir) + model_dir_location = os.path.join('.', model_dir) + os.chdir(model_dir_location) + + # obtain gnsf dimensions + get_matrices_fun = model.get_matrices_fun + phi_fun = model.phi_fun + + size_gnsf_A = get_matrices_fun.size_out(0) + gnsf_nx1 = size_gnsf_A[1] + gnsf_nz1 = size_gnsf_A[0] - size_gnsf_A[1] + gnsf_nuhat = max(phi_fun.size_in(1)) + gnsf_ny = max(phi_fun.size_in(0)) + gnsf_nout = max(phi_fun.size_out(0)) + + # set up expressions + # if the model uses MX because of cost/constraints + # the DAE can be exported as SX -> detect GNSF in Matlab + # -> evaluated SX GNSF functions with MX. + u = model.u + + if isinstance(u, casadi.MX): + symbol = MX.sym + else: + symbol = SX.sym + + y = symbol("y", gnsf_ny, 1) + uhat = symbol("uhat", gnsf_nuhat, 1) + p = model.p + x1 = symbol("gnsf_x1", gnsf_nx1, 1) + x1dot = symbol("gnsf_x1dot", gnsf_nx1, 1) + z1 = symbol("gnsf_z1", gnsf_nz1, 1) + dummy = symbol("gnsf_dummy", 1, 1) + empty_var = symbol("gnsf_empty_var", 0, 0) + + ## generate C code + fun_name = model_name + '_gnsf_phi_fun' + phi_fun_ = Function(fun_name, [y, uhat, p], [phi_fun(y, uhat, p)]) + phi_fun_.generate(fun_name, casadi_opts) + + fun_name = model_name + '_gnsf_phi_fun_jac_y' + phi_fun_jac_y = model.phi_fun_jac_y + phi_fun_jac_y_ = Function(fun_name, [y, uhat, p], phi_fun_jac_y(y, uhat, p)) + phi_fun_jac_y_.generate(fun_name, casadi_opts) + + fun_name = model_name + '_gnsf_phi_jac_y_uhat' + phi_jac_y_uhat = model.phi_jac_y_uhat + phi_jac_y_uhat_ = Function(fun_name, [y, uhat, p], phi_jac_y_uhat(y, uhat, p)) + phi_jac_y_uhat_.generate(fun_name, casadi_opts) + + fun_name = model_name + '_gnsf_f_lo_fun_jac_x1k1uz' + f_lo_fun_jac_x1k1uz = model.f_lo_fun_jac_x1k1uz + f_lo_fun_jac_x1k1uz_eval = f_lo_fun_jac_x1k1uz(x1, x1dot, z1, u, p) + + # avoid codegeneration issue + if not isinstance(f_lo_fun_jac_x1k1uz_eval, tuple) and is_empty(f_lo_fun_jac_x1k1uz_eval): + f_lo_fun_jac_x1k1uz_eval = [empty_var] + + f_lo_fun_jac_x1k1uz_ = Function(fun_name, [x1, x1dot, z1, u, p], + f_lo_fun_jac_x1k1uz_eval) + f_lo_fun_jac_x1k1uz_.generate(fun_name, casadi_opts) + + fun_name = model_name + '_gnsf_get_matrices_fun' + get_matrices_fun_ = Function(fun_name, [dummy], get_matrices_fun(1)) + get_matrices_fun_.generate(fun_name, casadi_opts) + + # remove fields for json dump + del model.phi_fun + del model.phi_fun_jac_y + del model.phi_jac_y_uhat + del model.f_lo_fun_jac_x1k1uz + del model.get_matrices_fun + + os.chdir(cwd) + + return diff --git a/third_party/acados/acados_template/generate_c_code_implicit_ode.py b/third_party/acados/acados_template/generate_c_code_implicit_ode.py new file mode 100644 index 0000000000..f2d50b43ab --- /dev/null +++ b/third_party/acados/acados_template/generate_c_code_implicit_ode.py @@ -0,0 +1,139 @@ +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + +import os +from casadi import * +from .utils import ALLOWED_CASADI_VERSIONS, is_empty, casadi_length, casadi_version_warning + +def generate_c_code_implicit_ode( model, opts ): + + casadi_version = CasadiMeta.version() + casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double') + if casadi_version not in (ALLOWED_CASADI_VERSIONS): + casadi_version_warning(casadi_version) + + generate_hess = opts["generate_hess"] + code_export_dir = opts["code_export_directory"] + + ## load model + x = model.x + xdot = model.xdot + u = model.u + z = model.z + p = model.p + f_impl = model.f_impl_expr + model_name = model.name + + ## get model dimensions + nx = casadi_length(x) + nu = casadi_length(u) + nz = casadi_length(z) + + ## generate jacobians + jac_x = jacobian(f_impl, x) + jac_xdot = jacobian(f_impl, xdot) + jac_u = jacobian(f_impl, u) + jac_z = jacobian(f_impl, z) + + ## generate hessian + x_xdot_z_u = vertcat(x, xdot, z, u) + + if isinstance(x, casadi.MX): + symbol = MX.sym + else: + symbol = SX.sym + + multiplier = symbol('multiplier', nx + nz) + + ADJ = jtimes(f_impl, x_xdot_z_u, multiplier, True) + HESS = jacobian(ADJ, x_xdot_z_u) + + ## Set up functions + p = model.p + fun_name = model_name + '_impl_dae_fun' + impl_dae_fun = Function(fun_name, [x, xdot, u, z, p], [f_impl]) + + fun_name = model_name + '_impl_dae_fun_jac_x_xdot_z' + impl_dae_fun_jac_x_xdot_z = Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_z]) + + # fun_name = model_name + '_impl_dae_fun_jac_x_xdot_z' + # impl_dae_fun_jac_x_xdot = Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_z]) + + # fun_name = model_name + '_impl_dae_jac_x_xdot_u' + # impl_dae_jac_x_xdot_u = Function(fun_name, [x, xdot, u, z, p], [jac_x, jac_xdot, jac_u, jac_z]) + + fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u_z' + impl_dae_fun_jac_x_xdot_u_z = Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_u, jac_z]) + + fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u' + impl_dae_fun_jac_x_xdot_u = Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_u]) + + fun_name = model_name + '_impl_dae_jac_x_xdot_u_z' + impl_dae_jac_x_xdot_u_z = Function(fun_name, [x, xdot, u, z, p], [jac_x, jac_xdot, jac_u, jac_z]) + + + fun_name = model_name + '_impl_dae_hess' + impl_dae_hess = Function(fun_name, [x, xdot, u, z, multiplier, p], [HESS]) + + # generate C code + if not os.path.exists(code_export_dir): + os.makedirs(code_export_dir) + + cwd = os.getcwd() + os.chdir(code_export_dir) + model_dir = model_name + '_model' + if not os.path.exists(model_dir): + os.mkdir(model_dir) + model_dir_location = os.path.join('.', model_dir) + os.chdir(model_dir_location) + + fun_name = model_name + '_impl_dae_fun' + impl_dae_fun.generate(fun_name, casadi_opts) + + fun_name = model_name + '_impl_dae_fun_jac_x_xdot_z' + impl_dae_fun_jac_x_xdot_z.generate(fun_name, casadi_opts) + + fun_name = model_name + '_impl_dae_jac_x_xdot_u_z' + impl_dae_jac_x_xdot_u_z.generate(fun_name, casadi_opts) + + fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u_z' + impl_dae_fun_jac_x_xdot_u_z.generate(fun_name, casadi_opts) + + fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u' + impl_dae_fun_jac_x_xdot_u.generate(fun_name, casadi_opts) + + if generate_hess: + fun_name = model_name + '_impl_dae_hess' + impl_dae_hess.generate(fun_name, casadi_opts) + + os.chdir(cwd) diff --git a/third_party/acados/acados_template/generate_c_code_nls_cost.py b/third_party/acados/acados_template/generate_c_code_nls_cost.py new file mode 100644 index 0000000000..ffcd78ca7b --- /dev/null +++ b/third_party/acados/acados_template/generate_c_code_nls_cost.py @@ -0,0 +1,113 @@ +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + +import os +from casadi import * +from .utils import ALLOWED_CASADI_VERSIONS, casadi_length, casadi_version_warning + +def generate_c_code_nls_cost( model, cost_name, stage_type, opts ): + + casadi_version = CasadiMeta.version() + casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double') + + if casadi_version not in (ALLOWED_CASADI_VERSIONS): + casadi_version_warning(casadi_version) + + x = model.x + p = model.p + + if isinstance(x, casadi.MX): + symbol = MX.sym + else: + symbol = SX.sym + + if stage_type == 'terminal': + middle_name = '_cost_y_e' + u = symbol('u', 0, 0) + cost_expr = model.cost_y_expr_e + + elif stage_type == 'initial': + middle_name = '_cost_y_0' + u = model.u + cost_expr = model.cost_y_expr_0 + + elif stage_type == 'path': + middle_name = '_cost_y' + u = model.u + cost_expr = model.cost_y_expr + + # set up directory + code_export_dir = opts["code_export_directory"] + if not os.path.exists(code_export_dir): + os.makedirs(code_export_dir) + + cwd = os.getcwd() + os.chdir(code_export_dir) + gen_dir = cost_name + '_cost' + if not os.path.exists(gen_dir): + os.mkdir(gen_dir) + gen_dir_location = os.path.join('.', gen_dir) + os.chdir(gen_dir_location) + + # set up expressions + cost_jac_expr = transpose(jacobian(cost_expr, vertcat(u, x))) + + ny = casadi_length(cost_expr) + + y = symbol('y', ny, 1) + + y_adj = jtimes(cost_expr, vertcat(u, x), y, True) + y_hess = jacobian(y_adj, vertcat(u, x)) + + ## generate C code + suffix_name = '_fun' + fun_name = cost_name + middle_name + suffix_name + y_fun = Function( fun_name, [x, u, p], \ + [ cost_expr ]) + y_fun.generate( fun_name, casadi_opts ) + + suffix_name = '_fun_jac_ut_xt' + fun_name = cost_name + middle_name + suffix_name + y_fun_jac_ut_xt = Function(fun_name, [x, u, p], \ + [ cost_expr, cost_jac_expr ]) + y_fun_jac_ut_xt.generate( fun_name, casadi_opts ) + + suffix_name = '_hess' + fun_name = cost_name + middle_name + suffix_name + y_hess = Function(fun_name, [x, u, y, p], [ y_hess ]) + y_hess.generate( fun_name, casadi_opts ) + + os.chdir(cwd) + + return + diff --git a/third_party/acados/acados_template/simulink_default_opts.json b/third_party/acados/acados_template/simulink_default_opts.json new file mode 100644 index 0000000000..70c939cb88 --- /dev/null +++ b/third_party/acados/acados_template/simulink_default_opts.json @@ -0,0 +1,40 @@ +{ + "outputs": { + "u0": 1, + "utraj": 0, + "xtraj": 0, + "solver_status": 1, + "KKT_residual": 1, + "x1": 1, + "CPU_time": 1, + "CPU_time_sim": 0, + "CPU_time_qp": 0, + "CPU_time_lin": 0, + "sqp_iter": 1 + }, + "inputs": { + "lbx_0": 1, + "ubx_0": 1, + "parameter_traj": 1, + "y_ref_0": 1, + "y_ref": 1, + "y_ref_e": 1, + "lbx": 1, + "ubx": 1, + "lbx_e": 1, + "ubx_e": 1, + "lbu": 1, + "ubu": 1, + "lg": 1, + "ug": 1, + "lh": 1, + "uh": 1, + "cost_W_0": 0, + "cost_W": 0, + "cost_W_e": 0, + "reset_solver": 0, + "x_init": 0, + "u_init": 0 + }, + "samplingtime": "t0" +} diff --git a/third_party/acados/acados_template/utils.py b/third_party/acados/acados_template/utils.py new file mode 100644 index 0000000000..8f44f61e7e --- /dev/null +++ b/third_party/acados/acados_template/utils.py @@ -0,0 +1,440 @@ +# -*- coding: future_fstrings -*- +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + +import os, sys, json +import urllib.request +import shutil +import numpy as np +from casadi import SX, MX, DM, Function, CasadiMeta + +ALLOWED_CASADI_VERSIONS = ('3.5.5', '3.5.4', '3.5.3', '3.5.2', '3.5.1', '3.4.5', '3.4.0') + +TERA_VERSION = "0.0.34" + +def get_acados_path(): + ACADOS_PATH = os.environ.get('ACADOS_SOURCE_DIR') + if not ACADOS_PATH: + acados_template_path = os.path.dirname(os.path.abspath(__file__)) + acados_path = os.path.join(acados_template_path, '..','..','..') + ACADOS_PATH = os.path.realpath(acados_path) + msg = 'Warning: Did not find environment variable ACADOS_SOURCE_DIR, ' + msg += 'guessed ACADOS_PATH to be {}.\n'.format(ACADOS_PATH) + msg += 'Please export ACADOS_SOURCE_DIR to avoid this warning.' + print(msg) + return ACADOS_PATH + + +def get_python_interface_path(): + ACADOS_PYTHON_INTERFACE_PATH = os.environ.get('ACADOS_PYTHON_INTERFACE_PATH') + if not ACADOS_PYTHON_INTERFACE_PATH: + acados_path = get_acados_path() + ACADOS_PYTHON_INTERFACE_PATH = os.path.join(acados_path, 'interfaces', 'acados_template', 'acados_template') + return ACADOS_PYTHON_INTERFACE_PATH + + +def get_tera_exec_path(): + TERA_PATH = os.environ.get('TERA_PATH') + if not TERA_PATH: + TERA_PATH = os.path.join(get_acados_path(), 'bin', 't_renderer') + if os.name == 'nt': + TERA_PATH += '.exe' + return TERA_PATH + + +platform2tera = { + "linux": "linux", + "darwin": "osx", + "win32": "windows" +} + + +def casadi_version_warning(casadi_version): + msg = 'Warning: Please note that the following versions of CasADi are ' + msg += 'officially supported: {}.\n '.format(" or ".join(ALLOWED_CASADI_VERSIONS)) + msg += 'If there is an incompatibility with the CasADi generated code, ' + msg += 'please consider changing your CasADi version.\n' + msg += 'Version {} currently in use.'.format(casadi_version) + print(msg) + + +def is_column(x): + if isinstance(x, np.ndarray): + if x.ndim == 1: + return True + elif x.ndim == 2 and x.shape[1] == 1: + return True + else: + return False + elif isinstance(x, (MX, SX, DM)): + if x.shape[1] == 1: + return True + elif x.shape[0] == 0 and x.shape[1] == 0: + return True + else: + return False + elif x == None or x == []: + return False + else: + raise Exception("is_column expects one of the following types: np.ndarray, casadi.MX, casadi.SX." + + " Got: " + str(type(x))) + + +def is_empty(x): + if isinstance(x, (MX, SX, DM)): + return x.is_empty() + elif isinstance(x, np.ndarray): + if np.prod(x.shape) == 0: + return True + else: + return False + elif x == None or x == []: + return True + else: + raise Exception("is_empty expects one of the following types: casadi.MX, casadi.SX, " + + "None, numpy array empty list. Got: " + str(type(x))) + + +def casadi_length(x): + if isinstance(x, (MX, SX, DM)): + return int(np.prod(x.shape)) + else: + raise Exception("casadi_length expects one of the following types: casadi.MX, casadi.SX." + + " Got: " + str(type(x))) + + +def make_model_consistent(model): + x = model.x + xdot = model.xdot + u = model.u + z = model.z + p = model.p + + if isinstance(x, MX): + symbol = MX.sym + elif isinstance(x, SX): + symbol = SX.sym + else: + raise Exception("model.x must be casadi.SX or casadi.MX, got {}".format(type(x))) + + if is_empty(p): + model.p = symbol('p', 0, 0) + + if is_empty(z): + model.z = symbol('z', 0, 0) + + return model + + +def get_tera(): + tera_path = get_tera_exec_path() + acados_path = get_acados_path() + + if os.path.exists(tera_path) and os.access(tera_path, os.X_OK): + return tera_path + + repo_url = "https://github.com/acados/tera_renderer/releases" + url = "{}/download/v{}/t_renderer-v{}-{}".format( + repo_url, TERA_VERSION, TERA_VERSION, platform2tera[sys.platform]) + + manual_install = 'For manual installation follow these instructions:\n' + manual_install += '1 Download binaries from {}\n'.format(url) + manual_install += '2 Copy them in {}/bin\n'.format(acados_path) + manual_install += '3 Strip the version and platform from the binaries: ' + manual_install += 'as t_renderer-v0.0.34-X -> t_renderer)\n' + manual_install += '4 Enable execution privilege on the file "t_renderer" with:\n' + manual_install += '"chmod +x {}"\n\n'.format(tera_path) + + msg = "\n" + msg += 'Tera template render executable not found, ' + msg += 'while looking in path:\n{}\n'.format(tera_path) + msg += 'In order to be able to render the templates, ' + msg += 'you need to download the tera renderer binaries from:\n' + msg += '{}\n\n'.format(repo_url) + msg += 'Do you wish to set up Tera renderer automatically?\n' + msg += 'y/N? (press y to download tera or any key for manual installation)\n' + + if input(msg) == 'y': + print("Dowloading {}".format(url)) + with urllib.request.urlopen(url) as response, open(tera_path, 'wb') as out_file: + shutil.copyfileobj(response, out_file) + print("Successfully downloaded t_renderer.") + os.chmod(tera_path, 0o755) + return tera_path + + msg_cancel = "\nYou cancelled automatic download.\n\n" + msg_cancel += manual_install + msg_cancel += "Once installed re-run your script.\n\n" + print(msg_cancel) + + sys.exit(1) + + +def render_template(in_file, out_file, template_dir, json_path): + cwd = os.getcwd() + if not os.path.exists(template_dir): + os.mkdir(template_dir) + os.chdir(template_dir) + + tera_path = get_tera() + + # setting up loader and environment + acados_path = os.path.dirname(os.path.abspath(__file__)) + template_glob = os.path.join(acados_path, 'c_templates_tera', '*') + + # call tera as system cmd + os_cmd = f"{tera_path} '{template_glob}' '{in_file}' '{json_path}' '{out_file}'" + # Windows cmd.exe can not cope with '...', so use "..." instead: + if os.name == 'nt': + os_cmd = os_cmd.replace('\'', '\"') + + status = os.system(os_cmd) + if (status != 0): + raise Exception(f'Rendering of {in_file} failed!\n\nAttempted to execute OS command:\n{os_cmd}\n\nExiting.\n') + + os.chdir(cwd) + + +## Conversion functions +def np_array_to_list(np_array): + if isinstance(np_array, (np.ndarray)): + return np_array.tolist() + elif isinstance(np_array, (SX)): + return DM(np_array).full() + elif isinstance(np_array, (DM)): + return np_array.full() + else: + raise(Exception(f"Cannot convert to list type {type(np_array)}")) + + +def format_class_dict(d): + """ + removes the __ artifact from class to dict conversion + """ + out = {} + for k, v in d.items(): + if isinstance(v, dict): + v = format_class_dict(v) + + out_key = k.split('__', 1)[-1] + out[k.replace(k, out_key)] = v + return out + + +def get_ocp_nlp_layout(): + python_interface_path = get_python_interface_path() + abs_path = os.path.join(python_interface_path, 'acados_layout.json') + with open(abs_path, 'r') as f: + ocp_nlp_layout = json.load(f) + return ocp_nlp_layout + + +def ocp_check_against_layout(ocp_nlp, ocp_dims): + """ + Check dimensions against layout + Parameters + --------- + ocp_nlp : dict + dictionary loaded from JSON to be post-processed. + + ocp_dims : instance of AcadosOcpDims + """ + + ocp_nlp_layout = get_ocp_nlp_layout() + + ocp_check_against_layout_recursion(ocp_nlp, ocp_dims, ocp_nlp_layout) + return + + +def ocp_check_against_layout_recursion(ocp_nlp, ocp_dims, layout): + + for key, item in ocp_nlp.items(): + + try: + layout_of_key = layout[key] + except KeyError: + raise Exception("ocp_check_against_layout_recursion: field" \ + " '{0}' is not in layout but in OCP description.".format(key)) + + if isinstance(item, dict): + ocp_check_against_layout_recursion(item, ocp_dims, layout_of_key) + + if 'ndarray' in layout_of_key: + if isinstance(item, int) or isinstance(item, float): + item = np.array([item]) + if isinstance(item, (list, np.ndarray)) and (layout_of_key[0] != 'str'): + dim_layout = [] + dim_names = layout_of_key[1] + + for dim_name in dim_names: + dim_layout.append(ocp_dims[dim_name]) + + dims = tuple(dim_layout) + + item = np.array(item) + item_dims = item.shape + if len(item_dims) != len(dims): + raise Exception('Mismatching dimensions for field {0}. ' \ + 'Expected {1} dimensional array, got {2} dimensional array.' \ + .format(key, len(dims), len(item_dims))) + + if np.prod(item_dims) != 0 or np.prod(dims) != 0: + if dims != item_dims: + raise Exception('acados -- mismatching dimensions for field {0}. ' \ + 'Provided data has dimensions {1}, ' \ + 'while associated dimensions {2} are {3}' \ + .format(key, item_dims, dim_names, dims)) + return + + +def J_to_idx(J): + nrows = J.shape[0] + idx = np.zeros((nrows, )) + for i in range(nrows): + this_idx = np.nonzero(J[i,:])[0] + if len(this_idx) != 1: + raise Exception('Invalid J matrix structure detected, ' \ + 'must contain one nonzero element per row. Exiting.') + if this_idx.size > 0 and J[i,this_idx[0]] != 1: + raise Exception('J matrices can only contain 1s. Exiting.') + idx[i] = this_idx[0] + return idx + + +def J_to_idx_slack(J): + nrows = J.shape[0] + ncol = J.shape[1] + idx = np.zeros((ncol, )) + i_idx = 0 + for i in range(nrows): + this_idx = np.nonzero(J[i,:])[0] + if len(this_idx) == 1: + idx[i_idx] = i + i_idx = i_idx + 1 + elif len(this_idx) > 1: + raise Exception('J_to_idx_slack: Invalid J matrix. Exiting. ' \ + 'Found more than one nonzero in row ' + str(i)) + if this_idx.size > 0 and J[i,this_idx[0]] != 1: + raise Exception('J_to_idx_slack: J matrices can only contain 1s, ' \ + 'got J(' + str(i) + ', ' + str(this_idx[0]) + ') = ' + str(J[i,this_idx[0]]) ) + if not i_idx == ncol: + raise Exception('J_to_idx_slack: J must contain a 1 in every column!') + return idx + + +def acados_dae_model_json_dump(model): + + # load model + x = model.x + xdot = model.xdot + u = model.u + z = model.z + p = model.p + + f_impl = model.f_impl_expr + model_name = model.name + + # create struct with impl_dae_fun, casadi_version + fun_name = model_name + '_impl_dae_fun' + impl_dae_fun = Function(fun_name, [x, xdot, u, z, p], [f_impl]) + + casadi_version = CasadiMeta.version() + str_impl_dae_fun = impl_dae_fun.serialize() + + dae_dict = {"str_impl_dae_fun": str_impl_dae_fun, "casadi_version": casadi_version} + + # dump + json_file = model_name + '_acados_dae.json' + with open(json_file, 'w') as f: + json.dump(dae_dict, f, default=np_array_to_list, indent=4, sort_keys=True) + print("dumped ", model_name, " dae to file:", json_file, "\n") + + +def set_up_imported_gnsf_model(acados_formulation): + + gnsf = acados_formulation.gnsf_model + + # check CasADi version + # dump_casadi_version = gnsf['casadi_version'] + # casadi_version = CasadiMeta.version() + + # if not casadi_version == dump_casadi_version: + # print("WARNING: GNSF model was dumped with another CasADi version.\n" + # + "This might yield errors. Please use the same version for compatibility, serialize version: " + # + dump_casadi_version + " current Python CasADi verison: " + casadi_version) + # input("Press any key to attempt to continue...") + + # load model + phi_fun = Function.deserialize(gnsf['phi_fun']) + phi_fun_jac_y = Function.deserialize(gnsf['phi_fun_jac_y']) + phi_jac_y_uhat = Function.deserialize(gnsf['phi_jac_y_uhat']) + get_matrices_fun = Function.deserialize(gnsf['get_matrices_fun']) + + # obtain gnsf dimensions + size_gnsf_A = get_matrices_fun.size_out(0) + acados_formulation.dims.gnsf_nx1 = size_gnsf_A[1] + acados_formulation.dims.gnsf_nz1 = size_gnsf_A[0] - size_gnsf_A[1] + acados_formulation.dims.gnsf_nuhat = max(phi_fun.size_in(1)) + acados_formulation.dims.gnsf_ny = max(phi_fun.size_in(0)) + acados_formulation.dims.gnsf_nout = max(phi_fun.size_out(0)) + + # save gnsf functions in model + acados_formulation.model.phi_fun = phi_fun + acados_formulation.model.phi_fun_jac_y = phi_fun_jac_y + acados_formulation.model.phi_jac_y_uhat = phi_jac_y_uhat + acados_formulation.model.get_matrices_fun = get_matrices_fun + + # get_matrices_fun = Function([model_name,'_gnsf_get_matrices_fun'], {dummy},... + # {A, B, C, E, L_x, L_xdot, L_z, L_u, A_LO, c, E_LO, B_LO,... + # nontrivial_f_LO, purely_linear, ipiv_x, ipiv_z, c_LO}); + get_matrices_out = get_matrices_fun(0) + acados_formulation.model.gnsf['nontrivial_f_LO'] = int(get_matrices_out[12]) + acados_formulation.model.gnsf['purely_linear'] = int(get_matrices_out[13]) + + if "f_lo_fun_jac_x1k1uz" in gnsf: + f_lo_fun_jac_x1k1uz = Function.deserialize(gnsf['f_lo_fun_jac_x1k1uz']) + acados_formulation.model.f_lo_fun_jac_x1k1uz = f_lo_fun_jac_x1k1uz + else: + dummy_var_x1 = SX.sym('dummy_var_x1', acados_formulation.dims.gnsf_nx1) + dummy_var_x1dot = SX.sym('dummy_var_x1dot', acados_formulation.dims.gnsf_nx1) + dummy_var_z1 = SX.sym('dummy_var_z1', acados_formulation.dims.gnsf_nz1) + dummy_var_u = SX.sym('dummy_var_z1', acados_formulation.dims.nu) + dummy_var_p = SX.sym('dummy_var_z1', acados_formulation.dims.np) + empty_var = SX.sym('empty_var', 0, 0) + + empty_fun = Function('empty_fun', \ + [dummy_var_x1, dummy_var_x1dot, dummy_var_z1, dummy_var_u, dummy_var_p], + [empty_var]) + acados_formulation.model.f_lo_fun_jac_x1k1uz = empty_fun + + del acados_formulation.gnsf_model diff --git a/third_party/acados/build.sh b/third_party/acados/build.sh index 0481e8159b..9b18b2b67d 100755 --- a/third_party/acados/build.sh +++ b/third_party/acados/build.sh @@ -43,11 +43,10 @@ mkdir -p $INSTALL_DIR rm $DIR/acados_repo/lib/*.json -rm -rf $DIR/include +rm -rf $DIR/include $DIR/acados_template 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 +cp -r $DIR/acados_repo/interfaces/acados_template/acados_template $DIR/ #pip3 install -e $DIR/acados/interfaces/acados_template # build tera diff --git a/selfdrive/controls/lib/cluster/.gitignore b/third_party/cluster/.gitignore similarity index 100% rename from selfdrive/controls/lib/cluster/.gitignore rename to third_party/cluster/.gitignore diff --git a/selfdrive/controls/lib/cluster/LICENSE b/third_party/cluster/LICENSE similarity index 100% rename from selfdrive/controls/lib/cluster/LICENSE rename to third_party/cluster/LICENSE diff --git a/selfdrive/controls/lib/cluster/README b/third_party/cluster/README similarity index 100% rename from selfdrive/controls/lib/cluster/README rename to third_party/cluster/README diff --git a/selfdrive/controls/lib/cluster/SConscript b/third_party/cluster/SConscript similarity index 100% rename from selfdrive/controls/lib/cluster/SConscript rename to third_party/cluster/SConscript diff --git a/selfdrive/controls/lib/cluster/__init__.py b/third_party/cluster/__init__.py similarity index 100% rename from selfdrive/controls/lib/cluster/__init__.py rename to third_party/cluster/__init__.py diff --git a/selfdrive/controls/lib/cluster/fastcluster.cpp b/third_party/cluster/fastcluster.cpp similarity index 100% rename from selfdrive/controls/lib/cluster/fastcluster.cpp rename to third_party/cluster/fastcluster.cpp diff --git a/selfdrive/controls/lib/cluster/fastcluster.h b/third_party/cluster/fastcluster.h similarity index 100% rename from selfdrive/controls/lib/cluster/fastcluster.h rename to third_party/cluster/fastcluster.h diff --git a/selfdrive/controls/lib/cluster/fastcluster_R_dm.cpp b/third_party/cluster/fastcluster_R_dm.cpp similarity index 100% rename from selfdrive/controls/lib/cluster/fastcluster_R_dm.cpp rename to third_party/cluster/fastcluster_R_dm.cpp diff --git a/selfdrive/controls/lib/cluster/fastcluster_dm.cpp b/third_party/cluster/fastcluster_dm.cpp similarity index 100% rename from selfdrive/controls/lib/cluster/fastcluster_dm.cpp rename to third_party/cluster/fastcluster_dm.cpp diff --git a/selfdrive/controls/lib/cluster/fastcluster_py.py b/third_party/cluster/fastcluster_py.py similarity index 100% rename from selfdrive/controls/lib/cluster/fastcluster_py.py rename to third_party/cluster/fastcluster_py.py diff --git a/selfdrive/controls/lib/cluster/test.cpp b/third_party/cluster/test.cpp similarity index 100% rename from selfdrive/controls/lib/cluster/test.cpp rename to third_party/cluster/test.cpp diff --git a/system/camerad/include/media/cam_cpas.h b/third_party/linux/include/media/cam_cpas.h similarity index 100% rename from system/camerad/include/media/cam_cpas.h rename to third_party/linux/include/media/cam_cpas.h diff --git a/system/camerad/include/media/cam_defs.h b/third_party/linux/include/media/cam_defs.h similarity index 100% rename from system/camerad/include/media/cam_defs.h rename to third_party/linux/include/media/cam_defs.h diff --git a/system/camerad/include/media/cam_fd.h b/third_party/linux/include/media/cam_fd.h similarity index 100% rename from system/camerad/include/media/cam_fd.h rename to third_party/linux/include/media/cam_fd.h diff --git a/system/camerad/include/media/cam_icp.h b/third_party/linux/include/media/cam_icp.h similarity index 100% rename from system/camerad/include/media/cam_icp.h rename to third_party/linux/include/media/cam_icp.h diff --git a/system/camerad/include/media/cam_isp.h b/third_party/linux/include/media/cam_isp.h similarity index 100% rename from system/camerad/include/media/cam_isp.h rename to third_party/linux/include/media/cam_isp.h diff --git a/system/camerad/include/media/cam_isp_ife.h b/third_party/linux/include/media/cam_isp_ife.h similarity index 100% rename from system/camerad/include/media/cam_isp_ife.h rename to third_party/linux/include/media/cam_isp_ife.h diff --git a/system/camerad/include/media/cam_isp_vfe.h b/third_party/linux/include/media/cam_isp_vfe.h similarity index 100% rename from system/camerad/include/media/cam_isp_vfe.h rename to third_party/linux/include/media/cam_isp_vfe.h diff --git a/system/camerad/include/media/cam_jpeg.h b/third_party/linux/include/media/cam_jpeg.h similarity index 100% rename from system/camerad/include/media/cam_jpeg.h rename to third_party/linux/include/media/cam_jpeg.h diff --git a/system/camerad/include/media/cam_lrme.h b/third_party/linux/include/media/cam_lrme.h similarity index 100% rename from system/camerad/include/media/cam_lrme.h rename to third_party/linux/include/media/cam_lrme.h diff --git a/system/camerad/include/media/cam_req_mgr.h b/third_party/linux/include/media/cam_req_mgr.h similarity index 100% rename from system/camerad/include/media/cam_req_mgr.h rename to third_party/linux/include/media/cam_req_mgr.h diff --git a/system/camerad/include/media/cam_sensor.h b/third_party/linux/include/media/cam_sensor.h similarity index 100% rename from system/camerad/include/media/cam_sensor.h rename to third_party/linux/include/media/cam_sensor.h diff --git a/system/camerad/include/media/cam_sensor_cmn_header.h b/third_party/linux/include/media/cam_sensor_cmn_header.h similarity index 100% rename from system/camerad/include/media/cam_sensor_cmn_header.h rename to third_party/linux/include/media/cam_sensor_cmn_header.h diff --git a/system/camerad/include/media/cam_sync.h b/third_party/linux/include/media/cam_sync.h similarity index 100% rename from system/camerad/include/media/cam_sync.h rename to third_party/linux/include/media/cam_sync.h diff --git a/system/camerad/include/msm_cam_sensor.h b/third_party/linux/include/msm_cam_sensor.h similarity index 100% rename from system/camerad/include/msm_cam_sensor.h rename to third_party/linux/include/msm_cam_sensor.h diff --git a/system/camerad/include/msm_camsensor_sdk.h b/third_party/linux/include/msm_camsensor_sdk.h similarity index 100% rename from system/camerad/include/msm_camsensor_sdk.h rename to third_party/linux/include/msm_camsensor_sdk.h diff --git a/third_party/linux/include/msm_kgsl.h b/third_party/linux/include/msm_kgsl.h new file mode 100644 index 0000000000..93582eb066 --- /dev/null +++ b/third_party/linux/include/msm_kgsl.h @@ -0,0 +1,1449 @@ +#ifndef _UAPI_MSM_KGSL_H +#define _UAPI_MSM_KGSL_H + +#include +#include + +/* + * The KGSL version has proven not to be very useful in userspace if features + * are cherry picked into other trees out of order so it is frozen as of 3.14. + * It is left here for backwards compatabilty and as a reminder that + * software releases are never linear. Also, I like pie. + */ + +#define KGSL_VERSION_MAJOR 3 +#define KGSL_VERSION_MINOR 14 + +/* + * We have traditionally mixed context and issueibcmds / command batch flags + * together into a big flag stew. This worked fine until we started adding a + * lot more command batch flags and we started running out of bits. Turns out + * we have a bit of room in the context type / priority mask that we could use + * for command batches, but that means we need to split out the flags into two + * coherent sets. + * + * If any future definitions are for both context and cmdbatch add both defines + * and link the cmdbatch to the context define as we do below. Otherwise feel + * free to add exclusive bits to either set. + */ + +/* --- context flags --- */ +#define KGSL_CONTEXT_SAVE_GMEM 0x00000001 +#define KGSL_CONTEXT_NO_GMEM_ALLOC 0x00000002 +/* This is a cmdbatch exclusive flag - use the CMDBATCH equivalent instead */ +#define KGSL_CONTEXT_SUBMIT_IB_LIST 0x00000004 +#define KGSL_CONTEXT_CTX_SWITCH 0x00000008 +#define KGSL_CONTEXT_PREAMBLE 0x00000010 +#define KGSL_CONTEXT_TRASH_STATE 0x00000020 +#define KGSL_CONTEXT_PER_CONTEXT_TS 0x00000040 +#define KGSL_CONTEXT_USER_GENERATED_TS 0x00000080 +/* This is a cmdbatch exclusive flag - use the CMDBATCH equivalent instead */ +#define KGSL_CONTEXT_END_OF_FRAME 0x00000100 +#define KGSL_CONTEXT_NO_FAULT_TOLERANCE 0x00000200 +/* This is a cmdbatch exclusive flag - use the CMDBATCH equivalent instead */ +#define KGSL_CONTEXT_SYNC 0x00000400 +#define KGSL_CONTEXT_PWR_CONSTRAINT 0x00000800 + +#define KGSL_CONTEXT_PRIORITY_MASK 0x0000F000 +#define KGSL_CONTEXT_PRIORITY_SHIFT 12 +#define KGSL_CONTEXT_PRIORITY_UNDEF 0 + +#define KGSL_CONTEXT_IFH_NOP 0x00010000 +#define KGSL_CONTEXT_SECURE 0x00020000 + +#define KGSL_CONTEXT_PREEMPT_STYLE_MASK 0x0E000000 +#define KGSL_CONTEXT_PREEMPT_STYLE_SHIFT 25 +#define KGSL_CONTEXT_PREEMPT_STYLE_DEFAULT 0x0 +#define KGSL_CONTEXT_PREEMPT_STYLE_RINGBUFFER 0x1 +#define KGSL_CONTEXT_PREEMPT_STYLE_FINEGRAIN 0x2 + +#define KGSL_CONTEXT_TYPE_MASK 0x01F00000 +#define KGSL_CONTEXT_TYPE_SHIFT 20 +#define KGSL_CONTEXT_TYPE_ANY 0 +#define KGSL_CONTEXT_TYPE_GL 1 +#define KGSL_CONTEXT_TYPE_CL 2 +#define KGSL_CONTEXT_TYPE_C2D 3 +#define KGSL_CONTEXT_TYPE_RS 4 +#define KGSL_CONTEXT_TYPE_UNKNOWN 0x1E + +#define KGSL_CONTEXT_INVALID 0xffffffff + +/* + * --- command batch flags --- + * The bits that are linked to a KGSL_CONTEXT equivalent are either legacy + * definitions or bits that are valid for both contexts and cmdbatches. To be + * safe the other 8 bits that are still available in the context field should be + * omitted here in case we need to share - the other bits are available for + * cmdbatch only flags as needed + */ +#define KGSL_CMDBATCH_MEMLIST 0x00000001 +#define KGSL_CMDBATCH_MARKER 0x00000002 +#define KGSL_CMDBATCH_SUBMIT_IB_LIST KGSL_CONTEXT_SUBMIT_IB_LIST /* 0x004 */ +#define KGSL_CMDBATCH_CTX_SWITCH KGSL_CONTEXT_CTX_SWITCH /* 0x008 */ +#define KGSL_CMDBATCH_PROFILING 0x00000010 +#define KGSL_CMDBATCH_PROFILING_KTIME 0x00000020 +#define KGSL_CMDBATCH_END_OF_FRAME KGSL_CONTEXT_END_OF_FRAME /* 0x100 */ +#define KGSL_CMDBATCH_SYNC KGSL_CONTEXT_SYNC /* 0x400 */ +#define KGSL_CMDBATCH_PWR_CONSTRAINT KGSL_CONTEXT_PWR_CONSTRAINT /* 0x800 */ + +/* + * Reserve bits [16:19] and bits [28:31] for possible bits shared between + * contexts and command batches. Update this comment as new flags are added. + */ + +/* + * gpu_command_object flags - these flags communicate the type of command or + * memory object being submitted for a GPU command + */ + +/* Flags for GPU command objects */ +#define KGSL_CMDLIST_IB 0x00000001U +#define KGSL_CMDLIST_CTXTSWITCH_PREAMBLE 0x00000002U +#define KGSL_CMDLIST_IB_PREAMBLE 0x00000004U + +/* Flags for GPU command memory objects */ +#define KGSL_OBJLIST_MEMOBJ 0x00000008U +#define KGSL_OBJLIST_PROFILE 0x00000010U + +/* Flags for GPU command sync points */ +#define KGSL_CMD_SYNCPOINT_TYPE_TIMESTAMP 0 +#define KGSL_CMD_SYNCPOINT_TYPE_FENCE 1 + +/* --- Memory allocation flags --- */ + +/* General allocation hints */ +#define KGSL_MEMFLAGS_SECURE 0x00000008ULL +#define KGSL_MEMFLAGS_GPUREADONLY 0x01000000U +#define KGSL_MEMFLAGS_GPUWRITEONLY 0x02000000U +#define KGSL_MEMFLAGS_FORCE_32BIT 0x100000000ULL + +/* Memory caching hints */ +#define KGSL_CACHEMODE_MASK 0x0C000000U +#define KGSL_CACHEMODE_SHIFT 26 + +#define KGSL_CACHEMODE_WRITECOMBINE 0 +#define KGSL_CACHEMODE_UNCACHED 1 +#define KGSL_CACHEMODE_WRITETHROUGH 2 +#define KGSL_CACHEMODE_WRITEBACK 3 + +#define KGSL_MEMFLAGS_USE_CPU_MAP 0x10000000ULL + +/* Memory types for which allocations are made */ +#define KGSL_MEMTYPE_MASK 0x0000FF00 +#define KGSL_MEMTYPE_SHIFT 8 + +#define KGSL_MEMTYPE_OBJECTANY 0 +#define KGSL_MEMTYPE_FRAMEBUFFER 1 +#define KGSL_MEMTYPE_RENDERBUFFER 2 +#define KGSL_MEMTYPE_ARRAYBUFFER 3 +#define KGSL_MEMTYPE_ELEMENTARRAYBUFFER 4 +#define KGSL_MEMTYPE_VERTEXARRAYBUFFER 5 +#define KGSL_MEMTYPE_TEXTURE 6 +#define KGSL_MEMTYPE_SURFACE 7 +#define KGSL_MEMTYPE_EGL_SURFACE 8 +#define KGSL_MEMTYPE_GL 9 +#define KGSL_MEMTYPE_CL 10 +#define KGSL_MEMTYPE_CL_BUFFER_MAP 11 +#define KGSL_MEMTYPE_CL_BUFFER_NOMAP 12 +#define KGSL_MEMTYPE_CL_IMAGE_MAP 13 +#define KGSL_MEMTYPE_CL_IMAGE_NOMAP 14 +#define KGSL_MEMTYPE_CL_KERNEL_STACK 15 +#define KGSL_MEMTYPE_COMMAND 16 +#define KGSL_MEMTYPE_2D 17 +#define KGSL_MEMTYPE_EGL_IMAGE 18 +#define KGSL_MEMTYPE_EGL_SHADOW 19 +#define KGSL_MEMTYPE_MULTISAMPLE 20 +#define KGSL_MEMTYPE_KERNEL 255 + +/* + * Alignment hint, passed as the power of 2 exponent. + * i.e 4k (2^12) would be 12, 64k (2^16)would be 16. + */ +#define KGSL_MEMALIGN_MASK 0x00FF0000 +#define KGSL_MEMALIGN_SHIFT 16 + +enum kgsl_user_mem_type { + KGSL_USER_MEM_TYPE_PMEM = 0x00000000, + KGSL_USER_MEM_TYPE_ASHMEM = 0x00000001, + KGSL_USER_MEM_TYPE_ADDR = 0x00000002, + KGSL_USER_MEM_TYPE_ION = 0x00000003, + /* + * ION type is retained for backwards compatibilty but Ion buffers are + * dma-bufs so try to use that naming if we can + */ + KGSL_USER_MEM_TYPE_DMABUF = 0x00000003, + KGSL_USER_MEM_TYPE_MAX = 0x00000007, +}; +#define KGSL_MEMFLAGS_USERMEM_MASK 0x000000e0 +#define KGSL_MEMFLAGS_USERMEM_SHIFT 5 + +/* + * Unfortunately, enum kgsl_user_mem_type starts at 0 which does not + * leave a good value for allocated memory. In the flags we use + * 0 to indicate allocated memory and thus need to add 1 to the enum + * values. + */ +#define KGSL_USERMEM_FLAG(x) (((x) + 1) << KGSL_MEMFLAGS_USERMEM_SHIFT) + +#define KGSL_MEMFLAGS_NOT_USERMEM 0 +#define KGSL_MEMFLAGS_USERMEM_PMEM KGSL_USERMEM_FLAG(KGSL_USER_MEM_TYPE_PMEM) +#define KGSL_MEMFLAGS_USERMEM_ASHMEM \ + KGSL_USERMEM_FLAG(KGSL_USER_MEM_TYPE_ASHMEM) +#define KGSL_MEMFLAGS_USERMEM_ADDR KGSL_USERMEM_FLAG(KGSL_USER_MEM_TYPE_ADDR) +#define KGSL_MEMFLAGS_USERMEM_ION KGSL_USERMEM_FLAG(KGSL_USER_MEM_TYPE_ION) + +/* --- generic KGSL flag values --- */ + +#define KGSL_FLAGS_NORMALMODE 0x00000000 +#define KGSL_FLAGS_SAFEMODE 0x00000001 +#define KGSL_FLAGS_INITIALIZED0 0x00000002 +#define KGSL_FLAGS_INITIALIZED 0x00000004 +#define KGSL_FLAGS_STARTED 0x00000008 +#define KGSL_FLAGS_ACTIVE 0x00000010 +#define KGSL_FLAGS_RESERVED0 0x00000020 +#define KGSL_FLAGS_RESERVED1 0x00000040 +#define KGSL_FLAGS_RESERVED2 0x00000080 +#define KGSL_FLAGS_SOFT_RESET 0x00000100 +#define KGSL_FLAGS_PER_CONTEXT_TIMESTAMPS 0x00000200 + +/* Server Side Sync Timeout in milliseconds */ +#define KGSL_SYNCOBJ_SERVER_TIMEOUT 2000 + +/* + * Reset status values for context + */ +enum kgsl_ctx_reset_stat { + KGSL_CTX_STAT_NO_ERROR = 0x00000000, + KGSL_CTX_STAT_GUILTY_CONTEXT_RESET_EXT = 0x00000001, + KGSL_CTX_STAT_INNOCENT_CONTEXT_RESET_EXT = 0x00000002, + KGSL_CTX_STAT_UNKNOWN_CONTEXT_RESET_EXT = 0x00000003 +}; + +#define KGSL_CONVERT_TO_MBPS(val) \ + (val*1000*1000U) + +/* device id */ +enum kgsl_deviceid { + KGSL_DEVICE_3D0 = 0x00000000, + KGSL_DEVICE_MAX +}; + +struct kgsl_devinfo { + + unsigned int device_id; + /* chip revision id + * coreid:8 majorrev:8 minorrev:8 patch:8 + */ + unsigned int chip_id; + unsigned int mmu_enabled; + unsigned long gmem_gpubaseaddr; + /* + * This field contains the adreno revision + * number 200, 205, 220, etc... + */ + unsigned int gpu_id; + size_t gmem_sizebytes; +}; + +/* + * struct kgsl_devmemstore - this structure defines the region of memory + * that can be mmap()ed from this driver. The timestamp fields are volatile + * because they are written by the GPU + * @soptimestamp: Start of pipeline timestamp written by GPU before the + * commands in concern are processed + * @sbz: Unused, kept for 8 byte alignment + * @eoptimestamp: End of pipeline timestamp written by GPU after the + * commands in concern are processed + * @sbz2: Unused, kept for 8 byte alignment + * @preempted: Indicates if the context was preempted + * @sbz3: Unused, kept for 8 byte alignment + * @ref_wait_ts: Timestamp on which to generate interrupt, unused now. + * @sbz4: Unused, kept for 8 byte alignment + * @current_context: The current context the GPU is working on + * @sbz5: Unused, kept for 8 byte alignment + */ +struct kgsl_devmemstore { + volatile unsigned int soptimestamp; + unsigned int sbz; + volatile unsigned int eoptimestamp; + unsigned int sbz2; + volatile unsigned int preempted; + unsigned int sbz3; + volatile unsigned int ref_wait_ts; + unsigned int sbz4; + unsigned int current_context; + unsigned int sbz5; +}; + +#define KGSL_MEMSTORE_OFFSET(ctxt_id, field) \ + ((ctxt_id)*sizeof(struct kgsl_devmemstore) + \ + offsetof(struct kgsl_devmemstore, field)) + +/* timestamp id*/ +enum kgsl_timestamp_type { + KGSL_TIMESTAMP_CONSUMED = 0x00000001, /* start-of-pipeline timestamp */ + KGSL_TIMESTAMP_RETIRED = 0x00000002, /* end-of-pipeline timestamp*/ + KGSL_TIMESTAMP_QUEUED = 0x00000003, +}; + +/* property types - used with kgsl_device_getproperty */ +#define KGSL_PROP_DEVICE_INFO 0x1 +#define KGSL_PROP_DEVICE_SHADOW 0x2 +#define KGSL_PROP_DEVICE_POWER 0x3 +#define KGSL_PROP_SHMEM 0x4 +#define KGSL_PROP_SHMEM_APERTURES 0x5 +#define KGSL_PROP_MMU_ENABLE 0x6 +#define KGSL_PROP_INTERRUPT_WAITS 0x7 +#define KGSL_PROP_VERSION 0x8 +#define KGSL_PROP_GPU_RESET_STAT 0x9 +#define KGSL_PROP_PWRCTRL 0xE +#define KGSL_PROP_PWR_CONSTRAINT 0x12 +#define KGSL_PROP_UCHE_GMEM_VADDR 0x13 +#define KGSL_PROP_SP_GENERIC_MEM 0x14 +#define KGSL_PROP_UCODE_VERSION 0x15 +#define KGSL_PROP_GPMU_VERSION 0x16 +#define KGSL_PROP_DEVICE_BITNESS 0x18 + +struct kgsl_shadowprop { + unsigned long gpuaddr; + size_t size; + unsigned int flags; /* contains KGSL_FLAGS_ values */ +}; + +struct kgsl_version { + unsigned int drv_major; + unsigned int drv_minor; + unsigned int dev_major; + unsigned int dev_minor; +}; + +struct kgsl_sp_generic_mem { + uint64_t local; + uint64_t pvt; +}; + +struct kgsl_ucode_version { + unsigned int pfp; + unsigned int pm4; +}; + +struct kgsl_gpmu_version { + unsigned int major; + unsigned int minor; + unsigned int features; +}; + +/* Performance counter groups */ + +#define KGSL_PERFCOUNTER_GROUP_CP 0x0 +#define KGSL_PERFCOUNTER_GROUP_RBBM 0x1 +#define KGSL_PERFCOUNTER_GROUP_PC 0x2 +#define KGSL_PERFCOUNTER_GROUP_VFD 0x3 +#define KGSL_PERFCOUNTER_GROUP_HLSQ 0x4 +#define KGSL_PERFCOUNTER_GROUP_VPC 0x5 +#define KGSL_PERFCOUNTER_GROUP_TSE 0x6 +#define KGSL_PERFCOUNTER_GROUP_RAS 0x7 +#define KGSL_PERFCOUNTER_GROUP_UCHE 0x8 +#define KGSL_PERFCOUNTER_GROUP_TP 0x9 +#define KGSL_PERFCOUNTER_GROUP_SP 0xA +#define KGSL_PERFCOUNTER_GROUP_RB 0xB +#define KGSL_PERFCOUNTER_GROUP_PWR 0xC +#define KGSL_PERFCOUNTER_GROUP_VBIF 0xD +#define KGSL_PERFCOUNTER_GROUP_VBIF_PWR 0xE +#define KGSL_PERFCOUNTER_GROUP_MH 0xF +#define KGSL_PERFCOUNTER_GROUP_PA_SU 0x10 +#define KGSL_PERFCOUNTER_GROUP_SQ 0x11 +#define KGSL_PERFCOUNTER_GROUP_SX 0x12 +#define KGSL_PERFCOUNTER_GROUP_TCF 0x13 +#define KGSL_PERFCOUNTER_GROUP_TCM 0x14 +#define KGSL_PERFCOUNTER_GROUP_TCR 0x15 +#define KGSL_PERFCOUNTER_GROUP_L2 0x16 +#define KGSL_PERFCOUNTER_GROUP_VSC 0x17 +#define KGSL_PERFCOUNTER_GROUP_CCU 0x18 +#define KGSL_PERFCOUNTER_GROUP_LRZ 0x19 +#define KGSL_PERFCOUNTER_GROUP_CMP 0x1A +#define KGSL_PERFCOUNTER_GROUP_ALWAYSON 0x1B +#define KGSL_PERFCOUNTER_GROUP_SP_PWR 0x1C +#define KGSL_PERFCOUNTER_GROUP_TP_PWR 0x1D +#define KGSL_PERFCOUNTER_GROUP_RB_PWR 0x1E +#define KGSL_PERFCOUNTER_GROUP_CCU_PWR 0x1F +#define KGSL_PERFCOUNTER_GROUP_UCHE_PWR 0x20 +#define KGSL_PERFCOUNTER_GROUP_CP_PWR 0x21 +#define KGSL_PERFCOUNTER_GROUP_GPMU_PWR 0x22 +#define KGSL_PERFCOUNTER_GROUP_ALWAYSON_PWR 0x23 +#define KGSL_PERFCOUNTER_GROUP_MAX 0x24 + +#define KGSL_PERFCOUNTER_NOT_USED 0xFFFFFFFF +#define KGSL_PERFCOUNTER_BROKEN 0xFFFFFFFE + +/* structure holds list of ibs */ +struct kgsl_ibdesc { + unsigned long gpuaddr; + unsigned long __pad; + size_t sizedwords; + unsigned int ctrl; +}; + +/** + * struct kgsl_cmdbatch_profiling_buffer + * @wall_clock_s: Ringbuffer submission time (seconds). + * If KGSL_CMDBATCH_PROFILING_KTIME is set, time is provided + * in kernel clocks, otherwise wall clock time is used. + * @wall_clock_ns: Ringbuffer submission time (nanoseconds). + * If KGSL_CMDBATCH_PROFILING_KTIME is set time is provided + * in kernel clocks, otherwise wall clock time is used. + * @gpu_ticks_queued: GPU ticks at ringbuffer submission + * @gpu_ticks_submitted: GPU ticks when starting cmdbatch execution + * @gpu_ticks_retired: GPU ticks when finishing cmdbatch execution + * + * This structure defines the profiling buffer used to measure cmdbatch + * execution time + */ +struct kgsl_cmdbatch_profiling_buffer { + uint64_t wall_clock_s; + uint64_t wall_clock_ns; + uint64_t gpu_ticks_queued; + uint64_t gpu_ticks_submitted; + uint64_t gpu_ticks_retired; +}; + +/* ioctls */ +#define KGSL_IOC_TYPE 0x09 + +/* get misc info about the GPU + type should be a value from enum kgsl_property_type + value points to a structure that varies based on type + sizebytes is sizeof() that structure + for KGSL_PROP_DEVICE_INFO, use struct kgsl_devinfo + this structure contaings hardware versioning info. + for KGSL_PROP_DEVICE_SHADOW, use struct kgsl_shadowprop + this is used to find mmap() offset and sizes for mapping + struct kgsl_memstore into userspace. +*/ +struct kgsl_device_getproperty { + unsigned int type; + void __user *value; + size_t sizebytes; +}; + +#define IOCTL_KGSL_DEVICE_GETPROPERTY \ + _IOWR(KGSL_IOC_TYPE, 0x2, struct kgsl_device_getproperty) + +/* IOCTL_KGSL_DEVICE_READ (0x3) - removed 03/2012 + */ + +/* block until the GPU has executed past a given timestamp + * timeout is in milliseconds. + */ +struct kgsl_device_waittimestamp { + unsigned int timestamp; + unsigned int timeout; +}; + +#define IOCTL_KGSL_DEVICE_WAITTIMESTAMP \ + _IOW(KGSL_IOC_TYPE, 0x6, struct kgsl_device_waittimestamp) + +struct kgsl_device_waittimestamp_ctxtid { + unsigned int context_id; + unsigned int timestamp; + unsigned int timeout; +}; + +#define IOCTL_KGSL_DEVICE_WAITTIMESTAMP_CTXTID \ + _IOW(KGSL_IOC_TYPE, 0x7, struct kgsl_device_waittimestamp_ctxtid) + +/* DEPRECATED: issue indirect commands to the GPU. + * drawctxt_id must have been created with IOCTL_KGSL_DRAWCTXT_CREATE + * ibaddr and sizedwords must specify a subset of a buffer created + * with IOCTL_KGSL_SHAREDMEM_FROM_PMEM + * flags may be a mask of KGSL_CONTEXT_ values + * timestamp is a returned counter value which can be passed to + * other ioctls to determine when the commands have been executed by + * the GPU. + * + * This fucntion is deprecated - consider using IOCTL_KGSL_SUBMIT_COMMANDS + * instead + */ +struct kgsl_ringbuffer_issueibcmds { + unsigned int drawctxt_id; + unsigned long ibdesc_addr; + unsigned int numibs; + unsigned int timestamp; /*output param */ + unsigned int flags; +}; + +#define IOCTL_KGSL_RINGBUFFER_ISSUEIBCMDS \ + _IOWR(KGSL_IOC_TYPE, 0x10, struct kgsl_ringbuffer_issueibcmds) + +/* read the most recently executed timestamp value + * type should be a value from enum kgsl_timestamp_type + */ +struct kgsl_cmdstream_readtimestamp { + unsigned int type; + unsigned int timestamp; /*output param */ +}; + +#define IOCTL_KGSL_CMDSTREAM_READTIMESTAMP_OLD \ + _IOR(KGSL_IOC_TYPE, 0x11, struct kgsl_cmdstream_readtimestamp) + +#define IOCTL_KGSL_CMDSTREAM_READTIMESTAMP \ + _IOWR(KGSL_IOC_TYPE, 0x11, struct kgsl_cmdstream_readtimestamp) + +/* free memory when the GPU reaches a given timestamp. + * gpuaddr specify a memory region created by a + * IOCTL_KGSL_SHAREDMEM_FROM_PMEM call + * type should be a value from enum kgsl_timestamp_type + */ +struct kgsl_cmdstream_freememontimestamp { + unsigned long gpuaddr; + unsigned int type; + unsigned int timestamp; +}; + +#define IOCTL_KGSL_CMDSTREAM_FREEMEMONTIMESTAMP \ + _IOW(KGSL_IOC_TYPE, 0x12, struct kgsl_cmdstream_freememontimestamp) + +/* Previous versions of this header had incorrectly defined + IOCTL_KGSL_CMDSTREAM_FREEMEMONTIMESTAMP as a read-only ioctl instead + of a write only ioctl. To ensure binary compatability, the following + #define will be used to intercept the incorrect ioctl +*/ + +#define IOCTL_KGSL_CMDSTREAM_FREEMEMONTIMESTAMP_OLD \ + _IOR(KGSL_IOC_TYPE, 0x12, struct kgsl_cmdstream_freememontimestamp) + +/* create a draw context, which is used to preserve GPU state. + * The flags field may contain a mask KGSL_CONTEXT_* values + */ +struct kgsl_drawctxt_create { + unsigned int flags; + unsigned int drawctxt_id; /*output param */ +}; + +#define IOCTL_KGSL_DRAWCTXT_CREATE \ + _IOWR(KGSL_IOC_TYPE, 0x13, struct kgsl_drawctxt_create) + +/* destroy a draw context */ +struct kgsl_drawctxt_destroy { + unsigned int drawctxt_id; +}; + +#define IOCTL_KGSL_DRAWCTXT_DESTROY \ + _IOW(KGSL_IOC_TYPE, 0x14, struct kgsl_drawctxt_destroy) + +/* add a block of pmem, fb, ashmem or user allocated address + * into the GPU address space */ +struct kgsl_map_user_mem { + int fd; + unsigned long gpuaddr; /*output param */ + size_t len; + size_t offset; + unsigned long hostptr; /*input param */ + enum kgsl_user_mem_type memtype; + unsigned int flags; +}; + +#define IOCTL_KGSL_MAP_USER_MEM \ + _IOWR(KGSL_IOC_TYPE, 0x15, struct kgsl_map_user_mem) + +struct kgsl_cmdstream_readtimestamp_ctxtid { + unsigned int context_id; + unsigned int type; + unsigned int timestamp; /*output param */ +}; + +#define IOCTL_KGSL_CMDSTREAM_READTIMESTAMP_CTXTID \ + _IOWR(KGSL_IOC_TYPE, 0x16, struct kgsl_cmdstream_readtimestamp_ctxtid) + +struct kgsl_cmdstream_freememontimestamp_ctxtid { + unsigned int context_id; + unsigned long gpuaddr; + unsigned int type; + unsigned int timestamp; +}; + +#define IOCTL_KGSL_CMDSTREAM_FREEMEMONTIMESTAMP_CTXTID \ + _IOW(KGSL_IOC_TYPE, 0x17, \ + struct kgsl_cmdstream_freememontimestamp_ctxtid) + +/* add a block of pmem or fb into the GPU address space */ +struct kgsl_sharedmem_from_pmem { + int pmem_fd; + unsigned long gpuaddr; /*output param */ + unsigned int len; + unsigned int offset; +}; + +#define IOCTL_KGSL_SHAREDMEM_FROM_PMEM \ + _IOWR(KGSL_IOC_TYPE, 0x20, struct kgsl_sharedmem_from_pmem) + +/* remove memory from the GPU's address space */ +struct kgsl_sharedmem_free { + unsigned long gpuaddr; +}; + +#define IOCTL_KGSL_SHAREDMEM_FREE \ + _IOW(KGSL_IOC_TYPE, 0x21, struct kgsl_sharedmem_free) + +struct kgsl_cff_user_event { + unsigned char cff_opcode; + unsigned int op1; + unsigned int op2; + unsigned int op3; + unsigned int op4; + unsigned int op5; + unsigned int __pad[2]; +}; + +#define IOCTL_KGSL_CFF_USER_EVENT \ + _IOW(KGSL_IOC_TYPE, 0x31, struct kgsl_cff_user_event) + +struct kgsl_gmem_desc { + unsigned int x; + unsigned int y; + unsigned int width; + unsigned int height; + unsigned int pitch; +}; + +struct kgsl_buffer_desc { + void *hostptr; + unsigned long gpuaddr; + int size; + unsigned int format; + unsigned int pitch; + unsigned int enabled; +}; + +struct kgsl_bind_gmem_shadow { + unsigned int drawctxt_id; + struct kgsl_gmem_desc gmem_desc; + unsigned int shadow_x; + unsigned int shadow_y; + struct kgsl_buffer_desc shadow_buffer; + unsigned int buffer_id; +}; + +#define IOCTL_KGSL_DRAWCTXT_BIND_GMEM_SHADOW \ + _IOW(KGSL_IOC_TYPE, 0x22, struct kgsl_bind_gmem_shadow) + +/* add a block of memory into the GPU address space */ + +/* + * IOCTL_KGSL_SHAREDMEM_FROM_VMALLOC deprecated 09/2012 + * use IOCTL_KGSL_GPUMEM_ALLOC instead + */ + +struct kgsl_sharedmem_from_vmalloc { + unsigned long gpuaddr; /*output param */ + unsigned int hostptr; + unsigned int flags; +}; + +#define IOCTL_KGSL_SHAREDMEM_FROM_VMALLOC \ + _IOWR(KGSL_IOC_TYPE, 0x23, struct kgsl_sharedmem_from_vmalloc) + +/* + * This is being deprecated in favor of IOCTL_KGSL_GPUMEM_CACHE_SYNC which + * supports both directions (flush and invalidate). This code will still + * work, but by definition it will do a flush of the cache which might not be + * what you want to have happen on a buffer following a GPU operation. It is + * safer to go with IOCTL_KGSL_GPUMEM_CACHE_SYNC + */ + +#define IOCTL_KGSL_SHAREDMEM_FLUSH_CACHE \ + _IOW(KGSL_IOC_TYPE, 0x24, struct kgsl_sharedmem_free) + +struct kgsl_drawctxt_set_bin_base_offset { + unsigned int drawctxt_id; + unsigned int offset; +}; + +#define IOCTL_KGSL_DRAWCTXT_SET_BIN_BASE_OFFSET \ + _IOW(KGSL_IOC_TYPE, 0x25, struct kgsl_drawctxt_set_bin_base_offset) + +enum kgsl_cmdwindow_type { + KGSL_CMDWINDOW_MIN = 0x00000000, + KGSL_CMDWINDOW_2D = 0x00000000, + KGSL_CMDWINDOW_3D = 0x00000001, /* legacy */ + KGSL_CMDWINDOW_MMU = 0x00000002, + KGSL_CMDWINDOW_ARBITER = 0x000000FF, + KGSL_CMDWINDOW_MAX = 0x000000FF, +}; + +/* write to the command window */ +struct kgsl_cmdwindow_write { + enum kgsl_cmdwindow_type target; + unsigned int addr; + unsigned int data; +}; + +#define IOCTL_KGSL_CMDWINDOW_WRITE \ + _IOW(KGSL_IOC_TYPE, 0x2e, struct kgsl_cmdwindow_write) + +struct kgsl_gpumem_alloc { + unsigned long gpuaddr; /* output param */ + size_t size; + unsigned int flags; +}; + +#define IOCTL_KGSL_GPUMEM_ALLOC \ + _IOWR(KGSL_IOC_TYPE, 0x2f, struct kgsl_gpumem_alloc) + +struct kgsl_cff_syncmem { + unsigned long gpuaddr; + size_t len; + unsigned int __pad[2]; /* For future binary compatibility */ +}; + +#define IOCTL_KGSL_CFF_SYNCMEM \ + _IOW(KGSL_IOC_TYPE, 0x30, struct kgsl_cff_syncmem) + +/* + * A timestamp event allows the user space to register an action following an + * expired timestamp. Note IOCTL_KGSL_TIMESTAMP_EVENT has been redefined to + * _IOWR to support fences which need to return a fd for the priv parameter. + */ + +struct kgsl_timestamp_event { + int type; /* Type of event (see list below) */ + unsigned int timestamp; /* Timestamp to trigger event on */ + unsigned int context_id; /* Context for the timestamp */ + void __user *priv; /* Pointer to the event specific blob */ + size_t len; /* Size of the event specific blob */ +}; + +#define IOCTL_KGSL_TIMESTAMP_EVENT_OLD \ + _IOW(KGSL_IOC_TYPE, 0x31, struct kgsl_timestamp_event) + +/* A genlock timestamp event releases an existing lock on timestamp expire */ + +#define KGSL_TIMESTAMP_EVENT_GENLOCK 1 + +struct kgsl_timestamp_event_genlock { + int handle; /* Handle of the genlock lock to release */ +}; + +/* A fence timestamp event releases an existing lock on timestamp expire */ + +#define KGSL_TIMESTAMP_EVENT_FENCE 2 + +struct kgsl_timestamp_event_fence { + int fence_fd; /* Fence to signal */ +}; + +/* + * Set a property within the kernel. Uses the same structure as + * IOCTL_KGSL_GETPROPERTY + */ + +#define IOCTL_KGSL_SETPROPERTY \ + _IOW(KGSL_IOC_TYPE, 0x32, struct kgsl_device_getproperty) + +#define IOCTL_KGSL_TIMESTAMP_EVENT \ + _IOWR(KGSL_IOC_TYPE, 0x33, struct kgsl_timestamp_event) + +/** + * struct kgsl_gpumem_alloc_id - argument to IOCTL_KGSL_GPUMEM_ALLOC_ID + * @id: returned id value for this allocation. + * @flags: mask of KGSL_MEM* values requested and actual flags on return. + * @size: requested size of the allocation and actual size on return. + * @mmapsize: returned size to pass to mmap() which may be larger than 'size' + * @gpuaddr: returned GPU address for the allocation + * + * Allocate memory for access by the GPU. The flags and size fields are echoed + * back by the kernel, so that the caller can know if the request was + * adjusted. + * + * Supported flags: + * KGSL_MEMFLAGS_GPUREADONLY: the GPU will be unable to write to the buffer + * KGSL_MEMTYPE*: usage hint for debugging aid + * KGSL_MEMALIGN*: alignment hint, may be ignored or adjusted by the kernel. + * KGSL_MEMFLAGS_USE_CPU_MAP: If set on call and return, the returned GPU + * address will be 0. Calling mmap() will set the GPU address. + */ +struct kgsl_gpumem_alloc_id { + unsigned int id; + unsigned int flags; + size_t size; + size_t mmapsize; + unsigned long gpuaddr; +/* private: reserved for future use*/ + unsigned long __pad[2]; +}; + +#define IOCTL_KGSL_GPUMEM_ALLOC_ID \ + _IOWR(KGSL_IOC_TYPE, 0x34, struct kgsl_gpumem_alloc_id) + +/** + * struct kgsl_gpumem_free_id - argument to IOCTL_KGSL_GPUMEM_FREE_ID + * @id: GPU allocation id to free + * + * Free an allocation by id, in case a GPU address has not been assigned or + * is unknown. Freeing an allocation by id with this ioctl or by GPU address + * with IOCTL_KGSL_SHAREDMEM_FREE are equivalent. + */ +struct kgsl_gpumem_free_id { + unsigned int id; +/* private: reserved for future use*/ + unsigned int __pad; +}; + +#define IOCTL_KGSL_GPUMEM_FREE_ID \ + _IOWR(KGSL_IOC_TYPE, 0x35, struct kgsl_gpumem_free_id) + +/** + * struct kgsl_gpumem_get_info - argument to IOCTL_KGSL_GPUMEM_GET_INFO + * @gpuaddr: GPU address to query. Also set on return. + * @id: GPU allocation id to query. Also set on return. + * @flags: returned mask of KGSL_MEM* values. + * @size: returned size of the allocation. + * @mmapsize: returned size to pass mmap(), which may be larger than 'size' + * @useraddr: returned address of the userspace mapping for this buffer + * + * This ioctl allows querying of all user visible attributes of an existing + * allocation, by either the GPU address or the id returned by a previous + * call to IOCTL_KGSL_GPUMEM_ALLOC_ID. Legacy allocation ioctls may not + * return all attributes so this ioctl can be used to look them up if needed. + * + */ +struct kgsl_gpumem_get_info { + unsigned long gpuaddr; + unsigned int id; + unsigned int flags; + size_t size; + size_t mmapsize; + unsigned long useraddr; +/* private: reserved for future use*/ + unsigned long __pad[4]; +}; + +#define IOCTL_KGSL_GPUMEM_GET_INFO\ + _IOWR(KGSL_IOC_TYPE, 0x36, struct kgsl_gpumem_get_info) + +/** + * struct kgsl_gpumem_sync_cache - argument to IOCTL_KGSL_GPUMEM_SYNC_CACHE + * @gpuaddr: GPU address of the buffer to sync. + * @id: id of the buffer to sync. Either gpuaddr or id is sufficient. + * @op: a mask of KGSL_GPUMEM_CACHE_* values + * @offset: offset into the buffer + * @length: number of bytes starting from offset to perform + * the cache operation on + * + * Sync the L2 cache for memory headed to and from the GPU - this replaces + * KGSL_SHAREDMEM_FLUSH_CACHE since it can handle cache management for both + * directions + * + */ +struct kgsl_gpumem_sync_cache { + unsigned long gpuaddr; + unsigned int id; + unsigned int op; + size_t offset; + size_t length; +}; + +#define KGSL_GPUMEM_CACHE_CLEAN (1 << 0) +#define KGSL_GPUMEM_CACHE_TO_GPU KGSL_GPUMEM_CACHE_CLEAN + +#define KGSL_GPUMEM_CACHE_INV (1 << 1) +#define KGSL_GPUMEM_CACHE_FROM_GPU KGSL_GPUMEM_CACHE_INV + +#define KGSL_GPUMEM_CACHE_FLUSH \ + (KGSL_GPUMEM_CACHE_CLEAN | KGSL_GPUMEM_CACHE_INV) + +/* Flag to ensure backwards compatibility of kgsl_gpumem_sync_cache struct */ +#define KGSL_GPUMEM_CACHE_RANGE (1 << 31U) + +#define IOCTL_KGSL_GPUMEM_SYNC_CACHE \ + _IOW(KGSL_IOC_TYPE, 0x37, struct kgsl_gpumem_sync_cache) + +/** + * struct kgsl_perfcounter_get - argument to IOCTL_KGSL_PERFCOUNTER_GET + * @groupid: Performance counter group ID + * @countable: Countable to select within the group + * @offset: Return offset of the reserved LO counter + * @offset_hi: Return offset of the reserved HI counter + * + * Get an available performance counter from a specified groupid. The offset + * of the performance counter will be returned after successfully assigning + * the countable to the counter for the specified group. An error will be + * returned and an offset of 0 if the groupid is invalid or there are no + * more counters left. After successfully getting a perfcounter, the user + * must call kgsl_perfcounter_put(groupid, contable) when finished with + * the perfcounter to clear up perfcounter resources. + * + */ +struct kgsl_perfcounter_get { + unsigned int groupid; + unsigned int countable; + unsigned int offset; + unsigned int offset_hi; +/* private: reserved for future use */ + unsigned int __pad; /* For future binary compatibility */ +}; + +#define IOCTL_KGSL_PERFCOUNTER_GET \ + _IOWR(KGSL_IOC_TYPE, 0x38, struct kgsl_perfcounter_get) + +/** + * struct kgsl_perfcounter_put - argument to IOCTL_KGSL_PERFCOUNTER_PUT + * @groupid: Performance counter group ID + * @countable: Countable to release within the group + * + * Put an allocated performance counter to allow others to have access to the + * resource that was previously taken. This is only to be called after + * successfully getting a performance counter from kgsl_perfcounter_get(). + * + */ +struct kgsl_perfcounter_put { + unsigned int groupid; + unsigned int countable; +/* private: reserved for future use */ + unsigned int __pad[2]; /* For future binary compatibility */ +}; + +#define IOCTL_KGSL_PERFCOUNTER_PUT \ + _IOW(KGSL_IOC_TYPE, 0x39, struct kgsl_perfcounter_put) + +/** + * struct kgsl_perfcounter_query - argument to IOCTL_KGSL_PERFCOUNTER_QUERY + * @groupid: Performance counter group ID + * @countable: Return active countables array + * @size: Size of active countables array + * @max_counters: Return total number counters for the group ID + * + * Query the available performance counters given a groupid. The array + * *countables is used to return the current active countables in counters. + * The size of the array is passed in so the kernel will only write at most + * size or counter->size for the group id. The total number of available + * counters for the group ID is returned in max_counters. + * If the array or size passed in are invalid, then only the maximum number + * of counters will be returned, no data will be written to *countables. + * If the groupid is invalid an error code will be returned. + * + */ +struct kgsl_perfcounter_query { + unsigned int groupid; + /* Array to return the current countable for up to size counters */ + unsigned int __user *countables; + unsigned int count; + unsigned int max_counters; +/* private: reserved for future use */ + unsigned int __pad[2]; /* For future binary compatibility */ +}; + +#define IOCTL_KGSL_PERFCOUNTER_QUERY \ + _IOWR(KGSL_IOC_TYPE, 0x3A, struct kgsl_perfcounter_query) + +/** + * struct kgsl_perfcounter_query - argument to IOCTL_KGSL_PERFCOUNTER_QUERY + * @groupid: Performance counter group IDs + * @countable: Performance counter countable IDs + * @value: Return performance counter reads + * @size: Size of all arrays (groupid/countable pair and return value) + * + * Read in the current value of a performance counter given by the groupid + * and countable. + * + */ + +struct kgsl_perfcounter_read_group { + unsigned int groupid; + unsigned int countable; + unsigned long long value; +}; + +struct kgsl_perfcounter_read { + struct kgsl_perfcounter_read_group __user *reads; + unsigned int count; +/* private: reserved for future use */ + unsigned int __pad[2]; /* For future binary compatibility */ +}; + +#define IOCTL_KGSL_PERFCOUNTER_READ \ + _IOWR(KGSL_IOC_TYPE, 0x3B, struct kgsl_perfcounter_read) +/* + * struct kgsl_gpumem_sync_cache_bulk - argument to + * IOCTL_KGSL_GPUMEM_SYNC_CACHE_BULK + * @id_list: list of GPU buffer ids of the buffers to sync + * @count: number of GPU buffer ids in id_list + * @op: a mask of KGSL_GPUMEM_CACHE_* values + * + * Sync the cache for memory headed to and from the GPU. Certain + * optimizations can be made on the cache operation based on the total + * size of the working set of memory to be managed. + */ +struct kgsl_gpumem_sync_cache_bulk { + unsigned int __user *id_list; + unsigned int count; + unsigned int op; +/* private: reserved for future use */ + unsigned int __pad[2]; /* For future binary compatibility */ +}; + +#define IOCTL_KGSL_GPUMEM_SYNC_CACHE_BULK \ + _IOWR(KGSL_IOC_TYPE, 0x3C, struct kgsl_gpumem_sync_cache_bulk) + +/* + * struct kgsl_cmd_syncpoint_timestamp + * @context_id: ID of a KGSL context + * @timestamp: GPU timestamp + * + * This structure defines a syncpoint comprising a context/timestamp pair. A + * list of these may be passed by IOCTL_KGSL_SUBMIT_COMMANDS to define + * dependencies that must be met before the command can be submitted to the + * hardware + */ +struct kgsl_cmd_syncpoint_timestamp { + unsigned int context_id; + unsigned int timestamp; +}; + +struct kgsl_cmd_syncpoint_fence { + int fd; +}; + +/** + * struct kgsl_cmd_syncpoint - Define a sync point for a command batch + * @type: type of sync point defined here + * @priv: Pointer to the type specific buffer + * @size: Size of the type specific buffer + * + * This structure contains pointers defining a specific command sync point. + * The pointer and size should point to a type appropriate structure. + */ +struct kgsl_cmd_syncpoint { + int type; + void __user *priv; + size_t size; +}; + +/* Flag to indicate that the cmdlist may contain memlists */ +#define KGSL_IBDESC_MEMLIST 0x1 + +/* Flag to point out the cmdbatch profiling buffer in the memlist */ +#define KGSL_IBDESC_PROFILING_BUFFER 0x2 + +/** + * struct kgsl_submit_commands - Argument to IOCTL_KGSL_SUBMIT_COMMANDS + * @context_id: KGSL context ID that owns the commands + * @flags: + * @cmdlist: User pointer to a list of kgsl_ibdesc structures + * @numcmds: Number of commands listed in cmdlist + * @synclist: User pointer to a list of kgsl_cmd_syncpoint structures + * @numsyncs: Number of sync points listed in synclist + * @timestamp: On entry the a user defined timestamp, on exist the timestamp + * assigned to the command batch + * + * This structure specifies a command to send to the GPU hardware. This is + * similar to kgsl_issueibcmds expect that it doesn't support the legacy way to + * submit IB lists and it adds sync points to block the IB until the + * dependencies are satisified. This entry point is the new and preferred way + * to submit commands to the GPU. The memory list can be used to specify all + * memory that is referrenced in the current set of commands. + */ + +struct kgsl_submit_commands { + unsigned int context_id; + unsigned int flags; + struct kgsl_ibdesc __user *cmdlist; + unsigned int numcmds; + struct kgsl_cmd_syncpoint __user *synclist; + unsigned int numsyncs; + unsigned int timestamp; +/* private: reserved for future use */ + unsigned int __pad[4]; +}; + +#define IOCTL_KGSL_SUBMIT_COMMANDS \ + _IOWR(KGSL_IOC_TYPE, 0x3D, struct kgsl_submit_commands) + +/** + * struct kgsl_device_constraint - device constraint argument + * @context_id: KGSL context ID + * @type: type of constraint i.e pwrlevel/none + * @data: constraint data + * @size: size of the constraint data + */ +struct kgsl_device_constraint { + unsigned int type; + unsigned int context_id; + void __user *data; + size_t size; +}; + +/* Constraint Type*/ +#define KGSL_CONSTRAINT_NONE 0 +#define KGSL_CONSTRAINT_PWRLEVEL 1 + +/* PWRLEVEL constraint level*/ +/* set to min frequency */ +#define KGSL_CONSTRAINT_PWR_MIN 0 +/* set to max frequency */ +#define KGSL_CONSTRAINT_PWR_MAX 1 + +struct kgsl_device_constraint_pwrlevel { + unsigned int level; +}; + +/** + * struct kgsl_syncsource_create - Argument to IOCTL_KGSL_SYNCSOURCE_CREATE + * @id: returned id for the syncsource that was created. + * + * This ioctl creates a userspace sync timeline. + */ + +struct kgsl_syncsource_create { + unsigned int id; +/* private: reserved for future use */ + unsigned int __pad[3]; +}; + +#define IOCTL_KGSL_SYNCSOURCE_CREATE \ + _IOWR(KGSL_IOC_TYPE, 0x40, struct kgsl_syncsource_create) + +/** + * struct kgsl_syncsource_destroy - Argument to IOCTL_KGSL_SYNCSOURCE_DESTROY + * @id: syncsource id to destroy + * + * This ioctl creates a userspace sync timeline. + */ + +struct kgsl_syncsource_destroy { + unsigned int id; +/* private: reserved for future use */ + unsigned int __pad[3]; +}; + +#define IOCTL_KGSL_SYNCSOURCE_DESTROY \ + _IOWR(KGSL_IOC_TYPE, 0x41, struct kgsl_syncsource_destroy) + +/** + * struct kgsl_syncsource_create_fence - Argument to + * IOCTL_KGSL_SYNCSOURCE_CREATE_FENCE + * @id: syncsource id + * @fence_fd: returned sync_fence fd + * + * Create a fence that may be signaled by userspace by calling + * IOCTL_KGSL_SYNCSOURCE_SIGNAL_FENCE. There are no order dependencies between + * these fences. + */ +struct kgsl_syncsource_create_fence { + unsigned int id; + int fence_fd; +/* private: reserved for future use */ + unsigned int __pad[4]; +}; + +/** + * struct kgsl_syncsource_signal_fence - Argument to + * IOCTL_KGSL_SYNCSOURCE_SIGNAL_FENCE + * @id: syncsource id + * @fence_fd: sync_fence fd to signal + * + * Signal a fence that was created by a IOCTL_KGSL_SYNCSOURCE_CREATE_FENCE + * call using the same syncsource id. This allows a fence to be shared + * to other processes but only signaled by the process owning the fd + * used to create the fence. + */ +#define IOCTL_KGSL_SYNCSOURCE_CREATE_FENCE \ + _IOWR(KGSL_IOC_TYPE, 0x42, struct kgsl_syncsource_create_fence) + +struct kgsl_syncsource_signal_fence { + unsigned int id; + int fence_fd; +/* private: reserved for future use */ + unsigned int __pad[4]; +}; + +#define IOCTL_KGSL_SYNCSOURCE_SIGNAL_FENCE \ + _IOWR(KGSL_IOC_TYPE, 0x43, struct kgsl_syncsource_signal_fence) + +/** + * struct kgsl_cff_sync_gpuobj - Argument to IOCTL_KGSL_CFF_SYNC_GPUOBJ + * @offset: Offset into the GPU object to sync + * @length: Number of bytes to sync + * @id: ID of the GPU object to sync + */ +struct kgsl_cff_sync_gpuobj { + uint64_t offset; + uint64_t length; + unsigned int id; +}; + +#define IOCTL_KGSL_CFF_SYNC_GPUOBJ \ + _IOW(KGSL_IOC_TYPE, 0x44, struct kgsl_cff_sync_gpuobj) + +/** + * struct kgsl_gpuobj_alloc - Argument to IOCTL_KGSL_GPUOBJ_ALLOC + * @size: Size in bytes of the object to allocate + * @flags: mask of KGSL_MEMFLAG_* bits + * @va_len: Size in bytes of the virtual region to allocate + * @mmapsize: Returns the mmap() size of the object + * @id: Returns the GPU object ID of the new object + * @metadata_len: Length of the metdata to copy from the user + * @metadata: Pointer to the user specified metadata to store for the object + */ +struct kgsl_gpuobj_alloc { + uint64_t size; + uint64_t flags; + uint64_t va_len; + uint64_t mmapsize; + unsigned int id; + unsigned int metadata_len; + uint64_t metadata; +}; + +/* Let the user know that this header supports the gpuobj metadata */ +#define KGSL_GPUOBJ_ALLOC_METADATA_MAX 64 + +#define IOCTL_KGSL_GPUOBJ_ALLOC \ + _IOWR(KGSL_IOC_TYPE, 0x45, struct kgsl_gpuobj_alloc) + +/** + * struct kgsl_gpuobj_free - Argument to IOCTL_KGLS_GPUOBJ_FREE + * @flags: Mask of: KGSL_GUPOBJ_FREE_ON_EVENT + * @priv: Pointer to the private object if KGSL_GPUOBJ_FREE_ON_EVENT is + * specified + * @id: ID of the GPU object to free + * @type: If KGSL_GPUOBJ_FREE_ON_EVENT is specified, the type of asynchronous + * event to free on + * @len: Length of the data passed in priv + */ +struct kgsl_gpuobj_free { + uint64_t flags; + uint64_t __user priv; + unsigned int id; + unsigned int type; + unsigned int len; +}; + +#define KGSL_GPUOBJ_FREE_ON_EVENT 1 + +#define KGSL_GPU_EVENT_TIMESTAMP 1 +#define KGSL_GPU_EVENT_FENCE 2 + +/** + * struct kgsl_gpu_event_timestamp - Specifies a timestamp event to free a GPU + * object on + * @context_id: ID of the timestamp event to wait for + * @timestamp: Timestamp of the timestamp event to wait for + */ +struct kgsl_gpu_event_timestamp { + unsigned int context_id; + unsigned int timestamp; +}; + +/** + * struct kgsl_gpu_event_fence - Specifies a fence ID to to free a GPU object on + * @fd: File descriptor for the fence + */ +struct kgsl_gpu_event_fence { + int fd; +}; + +#define IOCTL_KGSL_GPUOBJ_FREE \ + _IOW(KGSL_IOC_TYPE, 0x46, struct kgsl_gpuobj_free) + +/** + * struct kgsl_gpuobj_info - argument to IOCTL_KGSL_GPUOBJ_INFO + * @gpuaddr: GPU address of the object + * @flags: Current flags for the object + * @size: Size of the object + * @va_len: VA size of the object + * @va_addr: Virtual address of the object (if it is mapped) + * id - GPU object ID of the object to query + */ +struct kgsl_gpuobj_info { + uint64_t gpuaddr; + uint64_t flags; + uint64_t size; + uint64_t va_len; + uint64_t va_addr; + unsigned int id; +}; + +#define IOCTL_KGSL_GPUOBJ_INFO \ + _IOWR(KGSL_IOC_TYPE, 0x47, struct kgsl_gpuobj_info) + +/** + * struct kgsl_gpuobj_import - argument to IOCTL_KGSL_GPUOBJ_IMPORT + * @priv: Pointer to the private data for the import type + * @priv_len: Length of the private data + * @flags: Mask of KGSL_MEMFLAG_ flags + * @type: Type of the import (KGSL_USER_MEM_TYPE_*) + * @id: Returns the ID of the new GPU object + */ +struct kgsl_gpuobj_import { + uint64_t __user priv; + uint64_t priv_len; + uint64_t flags; + unsigned int type; + unsigned int id; +}; + +/** + * struct kgsl_gpuobj_import_dma_buf - import a dmabuf object + * @fd: File descriptor for the dma-buf object + */ +struct kgsl_gpuobj_import_dma_buf { + int fd; +}; + +/** + * struct kgsl_gpuobj_import_useraddr - import an object based on a useraddr + * @virtaddr: Virtual address of the object to import + */ +struct kgsl_gpuobj_import_useraddr { + uint64_t virtaddr; +}; + +#define IOCTL_KGSL_GPUOBJ_IMPORT \ + _IOWR(KGSL_IOC_TYPE, 0x48, struct kgsl_gpuobj_import) + +/** + * struct kgsl_gpuobj_sync_obj - Individual GPU object to sync + * @offset: Offset within the GPU object to sync + * @length: Number of bytes to sync + * @id: ID of the GPU object to sync + * @op: Cache operation to execute + */ + +struct kgsl_gpuobj_sync_obj { + uint64_t offset; + uint64_t length; + unsigned int id; + unsigned int op; +}; + +/** + * struct kgsl_gpuobj_sync - Argument for IOCTL_KGSL_GPUOBJ_SYNC + * @objs: Pointer to an array of kgsl_gpuobj_sync_obj structs + * @obj_len: Size of each item in the array + * @count: Number of items in the array + */ + +struct kgsl_gpuobj_sync { + uint64_t __user objs; + unsigned int obj_len; + unsigned int count; +}; + +#define IOCTL_KGSL_GPUOBJ_SYNC \ + _IOW(KGSL_IOC_TYPE, 0x49, struct kgsl_gpuobj_sync) + +/** + * struct kgsl_command_object - GPU command object + * @offset: GPU address offset of the object + * @gpuaddr: GPU address of the object + * @size: Size of the object + * @flags: Current flags for the object + * @id - GPU command object ID + */ +struct kgsl_command_object { + uint64_t offset; + uint64_t gpuaddr; + uint64_t size; + unsigned int flags; + unsigned int id; +}; + +/** + * struct kgsl_command_syncpoint - GPU syncpoint object + * @priv: Pointer to the type specific buffer + * @size: Size of the type specific buffer + * @type: type of sync point defined here + */ +struct kgsl_command_syncpoint { + uint64_t __user priv; + uint64_t size; + unsigned int type; +}; + +/** + * struct kgsl_command_object - Argument for IOCTL_KGSL_GPU_COMMAND + * @flags: Current flags for the object + * @cmdlist: List of kgsl_command_objects for submission + * @cmd_size: Size of kgsl_command_objects structure + * @numcmds: Number of kgsl_command_objects in command list + * @objlist: List of kgsl_command_objects for tracking + * @obj_size: Size of kgsl_command_objects structure + * @numobjs: Number of kgsl_command_objects in object list + * @synclist: List of kgsl_command_syncpoints + * @sync_size: Size of kgsl_command_syncpoint structure + * @numsyncs: Number of kgsl_command_syncpoints in syncpoint list + * @context_id: Context ID submittin ghte kgsl_gpu_command + * @timestamp: Timestamp for the submitted commands + */ +struct kgsl_gpu_command { + uint64_t flags; + uint64_t __user cmdlist; + unsigned int cmdsize; + unsigned int numcmds; + uint64_t __user objlist; + unsigned int objsize; + unsigned int numobjs; + uint64_t __user synclist; + unsigned int syncsize; + unsigned int numsyncs; + unsigned int context_id; + unsigned int timestamp; +}; + +#define IOCTL_KGSL_GPU_COMMAND \ + _IOWR(KGSL_IOC_TYPE, 0x4A, struct kgsl_gpu_command) + +/** + * struct kgsl_preemption_counters_query - argument to + * IOCTL_KGSL_PREEMPTIONCOUNTER_QUERY + * @counters: Return preemption counters array + * @size_user: Size allocated by userspace + * @size_priority_level: Size of preemption counters for each + * priority level + * @max_priority_level: Return max number of priority levels + * + * Query the available preemption counters. The array counters + * is used to return preemption counters. The size of the array + * is passed in so the kernel will only write at most size_user + * or max available preemption counters. The total number of + * preemption counters is returned in max_priority_level. If the + * array or size passed in are invalid, then an error is + * returned back. + */ +struct kgsl_preemption_counters_query { + uint64_t __user counters; + unsigned int size_user; + unsigned int size_priority_level; + unsigned int max_priority_level; +}; + +#define IOCTL_KGSL_PREEMPTIONCOUNTER_QUERY \ + _IOWR(KGSL_IOC_TYPE, 0x4B, struct kgsl_preemption_counters_query) + +/** + * struct kgsl_gpuobj_set_info - argument for IOCTL_KGSL_GPUOBJ_SET_INFO + * @flags: Flags to indicate which paramaters to change + * @metadata: If KGSL_GPUOBJ_SET_INFO_METADATA is set, a pointer to the new + * metadata + * @id: GPU memory object ID to change + * @metadata_len: If KGSL_GPUOBJ_SET_INFO_METADATA is set, the length of the + * new metadata string + * @type: If KGSL_GPUOBJ_SET_INFO_TYPE is set, the new type of the memory object + */ + +#define KGSL_GPUOBJ_SET_INFO_METADATA (1 << 0) +#define KGSL_GPUOBJ_SET_INFO_TYPE (1 << 1) + +struct kgsl_gpuobj_set_info { + uint64_t flags; + uint64_t metadata; + unsigned int id; + unsigned int metadata_len; + unsigned int type; +}; + +#define IOCTL_KGSL_GPUOBJ_SET_INFO \ + _IOW(KGSL_IOC_TYPE, 0x4C, struct kgsl_gpuobj_set_info) + +#endif /* _UAPI_MSM_KGSL_H */ diff --git a/system/camerad/include/msmb_camera.h b/third_party/linux/include/msmb_camera.h similarity index 100% rename from system/camerad/include/msmb_camera.h rename to third_party/linux/include/msmb_camera.h diff --git a/system/camerad/include/msmb_isp.h b/third_party/linux/include/msmb_isp.h similarity index 100% rename from system/camerad/include/msmb_isp.h rename to third_party/linux/include/msmb_isp.h diff --git a/system/camerad/include/msmb_ispif.h b/third_party/linux/include/msmb_ispif.h similarity index 100% rename from system/camerad/include/msmb_ispif.h rename to third_party/linux/include/msmb_ispif.h diff --git a/tools/gpstest/fuzzy_testing.py b/tools/gpstest/fuzzy_testing.py index df6691c558..bd204e7ae7 100755 --- a/tools/gpstest/fuzzy_testing.py +++ b/tools/gpstest/fuzzy_testing.py @@ -69,7 +69,7 @@ rc_p: Any = None def exec_remote_checker(lat, lon, duration, ip_addr): global rc_p # TODO: good enough for testing - remote_cmd = "export PYTHONPATH=/data/pythonpath:/data/pythonpath/pyextra && " + remote_cmd = "export PYTHONPATH=/data/pythonpath && " remote_cmd += "cd /data/openpilot && " remote_cmd += f"timeout {duration} /usr/local/pyenv/shims/python tools/gpstest/remote_checker.py " remote_cmd += f"{lat} {lon}" diff --git a/tools/sim/Dockerfile.sim b/tools/sim/Dockerfile.sim index 3692d48e0f..be16f8c863 100644 --- a/tools/sim/Dockerfile.sim +++ b/tools/sim/Dockerfile.sim @@ -15,7 +15,6 @@ RUN mkdir -p $HOME/openpilot COPY SConstruct $HOME/openpilot/ COPY ./third_party $HOME/openpilot/third_party -COPY ./pyextra $HOME/openpilot/pyextra COPY ./site_scons $HOME/openpilot/site_scons COPY ./rednose $HOME/openpilot/rednose COPY ./laika $HOME/openpilot/laika