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 <device@comma.ai>
old-commit-hash: cd8e03d53e
beeps
Adeeb Shihadeh
2 years ago
committed by
GitHub
parent
4d1e15b85c
commit
b3e47c691b
163 changed files with 18907 additions and 218 deletions
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
|
||||||
oid sha256:094c25031dcfe5c288666671bc2012866293b3797ea53c796af798bda4c32f01 |
|
||||||
size 6 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
|
||||||
oid sha256:477f73573a50b1ae2740849e1aed4f8d353ead59116f95b897e8f620e8dbf31b |
|
||||||
size 62 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
|
||||||
oid sha256:dc3561a615e9baefc436c341280cac6634008a3b4a1773227604db4bf15b00d8 |
|
||||||
size 1971 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
|
||||||
oid sha256:c116b7b6dae6a24ed2e3f55604edce722d6b86c9c25451e3ebe96c278f700cc9 |
|
||||||
size 14258 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
|
||||||
oid sha256:af19df99d9669672bfdc72d959bd444919a16c800f764873c923db425c26e70d |
|
||||||
size 8289 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
|
||||||
oid sha256:00a5921254ea36f5cb0c80838bbc18bd7544990f4366979152c9a81cd0f302b8 |
|
||||||
size 101018 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
|
||||||
oid sha256:88265ec648408897af39ffe030823b009dde3cf56e588d3e717c60898432461c |
|
||||||
size 84016 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
|
||||||
oid sha256:74e00646a8d51f0f4e8d0aa3d0e31160949f9a69df05b3d6b200ec3d60c560fb |
|
||||||
size 29596 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
|
||||||
oid sha256:ded40649250d2bb047cadb87584e5f2bbbd1c8b88276ca8d7909acd11454cfa9 |
|
||||||
size 12029 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
|
||||||
oid sha256:7e21d44ed88590aaa52541839e2e8a8d008ceb5d82c7e2af9736df7b0f4eb5e9 |
|
||||||
size 759 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
|
||||||
oid sha256:5969249d08301184bdfb0f90adeb23e518a892d38005c3b34ab1d32114f7820f |
|
||||||
size 18884 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
|
||||||
oid sha256:9166b8d29e88c53739da15d79f1a39ef007bac88b3219af171f738f57937286f |
|
||||||
size 4335 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
|
||||||
oid sha256:6e365ee08adc63a7d366dd283171bcd4b7f8e25825ea1d4fff09b36ba1ab754a |
|
||||||
size 5454 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
|
||||||
oid sha256:259e9620ab3810be89004b58838f7a72edd4729928a899c3938d49c05bd10359 |
|
||||||
size 14199 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
|
||||||
oid sha256:082e141c424fbec023b900efb4f8d5d4e9f13634eb9e92bfe3b076d298585e68 |
|
||||||
size 78 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
|
||||||
oid sha256:a5cfdabde7dd957124c1dcac1ad41c5144f7359b95498bbdc833d6e50bcbfea9 |
|
||||||
size 13858 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1
|
|
||||||
oid sha256:5a837cf6930efa41eb33fe32bd126ac5acea7afcac8ff3f6aee65e1ec5f974a3 |
|
||||||
size 17505 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1
|
|
||||||
oid sha256:cfab0ed0fac52bea3bb4dae81dd45f18764330f56d08dfed5c80d11923ad6b44 |
|
||||||
size 2555 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1
|
|
||||||
oid sha256:cac8dd3c940a2b07d7104aeae846f4238a711a973da1667fa7bfdf91dea65cd4 |
|
||||||
size 19922 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1
|
|
||||||
oid sha256:1b5a8e5e2f2bf2456962c53842de00d236d67000b98cb61e48c968fe760eb6a5 |
|
||||||
size 2206 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1
|
|
||||||
oid sha256:f2a7be5c4f4c3afdcb7e252b72c90a8c0f92992c63bd97977f0470745feb2b6e |
|
||||||
size 26035 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1
|
|
||||||
oid sha256:9299ceb39f13dfc8adac44ca5965713b64c76626f216de0d95505204229daed6 |
|
||||||
size 4445 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1
|
|
||||||
oid sha256:b1ee39cbcb027288fcaae01deaafba0f7ccfa863f3cf30a19a2e19860ab32e1f |
|
||||||
size 7226 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1
|
|
||||||
oid sha256:933ccc120f3eb064619c1b1b7c69dee3d48492d7bbcbede5480f5e146aae103c |
|
||||||
size 101177 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1
|
|
||||||
oid sha256:17274e2daf731a43a8e28cb8da55e767cffe907a16d9d1338271494649a77715 |
|
||||||
size 10723 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
|
||||||
oid sha256:8c2eb33568b8c3a7fdc13bab1c922329b6e7c55b0e99eb93becc3853ab9c1ae8 |
|
||||||
size 4000 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1
|
|
||||||
oid sha256:6dad085983af45f46d30dd3ac45615fe0224c4821992de7b8d20b0fff625878a |
|
||||||
size 29202 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1
|
|
||||||
oid sha256:40972ebe4461c432b5802c7f4a0d2644b1c2a4727fbc629695cb30c5b71c4d68 |
|
||||||
size 3125 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1
|
|
||||||
oid sha256:1f26921d2f60b054e9c2a5ee3cef9663540e885131c66efba6be3b36857798c5 |
|
||||||
size 3125 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1
|
|
||||||
oid sha256:0baa31610dd9fcf653e4b2289911cb5251787606125ec101739570d28e818bf3 |
|
||||||
size 3081 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1
|
|
||||||
oid sha256:ba8e07cf179230f65ec8c46905ad969f82c41ed8580fb6d45c3376b416f1e52e |
|
||||||
size 3354 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1
|
|
||||||
oid sha256:eb7fff7be0adbc647ec0af0e8bd68a2b10cbe7eacfc38d8c5665d856c66abb4e |
|
||||||
size 3403 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1
|
|
||||||
oid sha256:7d7754b835a25342af3e1bb53887469daacd53675b1cc86c0562e7f46e5819bd |
|
||||||
size 3402 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1
|
|
||||||
oid sha256:7b411cbfab08ca9ecbe1d75c80ab71f0551db4050cbcce7dae330f92c7473a4f |
|
||||||
size 3272 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1
|
|
||||||
oid sha256:d1e42330752c441b92bc0f7cc4ef73cac67f8dba664e050cb88cdc682dd545f4 |
|
||||||
size 3317 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1
|
|
||||||
oid sha256:b0476231619e9b58e9d9f1ac38b23d7b07267af02b9fc059b3e96a881f692cbb |
|
||||||
size 8044 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1
|
|
||||||
oid sha256:2a6d9e4529cb352a13c65ea30dec561c5423cd191c32510099d9a6e0c2948966 |
|
||||||
size 6326 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1
|
|
||||||
oid sha256:659efc1476d5b27bb6ade88223762fe65d2aa1e96bae0c1fdd7dec6e167fe02a |
|
||||||
size 4677 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
|
||||||
oid sha256:148d43fb0fa9fa42fc571648c94a344d69e7c8e26b47d023b323773adddbb6f7 |
|
||||||
size 4293 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
|
||||||
oid sha256:0df5010cd2c049661bb45da3ab20cfc78adeea15612bc710259e36d855a1800e |
|
||||||
size 4558 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
|
||||||
oid sha256:b8d2619181da8938064c8e867ae7b511e476002f6c33e73324f60ac88bea7f22 |
|
||||||
size 15658 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
|
||||||
oid sha256:eeb1550ed63d4d9d561a133db36c4d11c9d03634f466f2ab80ba54ac69dac83a |
|
||||||
size 4496 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
|
||||||
oid sha256:4ac71216686385bcf4dedd49a2e4b85e510f36b9d1e1748245d0a7abefad0d9d |
|
||||||
size 6185 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1
|
|
||||||
oid sha256:64753b01879d27e824a86c26ca15b29956efa3b2a5f536dbe78220df4f01fbdc |
|
||||||
size 10420 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1
|
|
||||||
oid sha256:16ef8e23d19800aaf0bc433571c4b2da5a117a1128a3011c3208192880e80d3b |
|
||||||
size 2321 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1
|
|
||||||
oid sha256:da836e689945ebed0a730f6bbcd3a6922770a07216469cc4898bfcb4bb330c14 |
|
||||||
size 658 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
|
||||||
oid sha256:e3ca280cc474c3fa50496ca7e21d3b7c918843c6c4f3737627538eb3a8847148 |
|
||||||
size 6728 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
|
||||||
oid sha256:90b41224841a3bc26fef52eed4f88b4a2c7e6c4ab08ebc3b943f45dde83847db |
|
||||||
size 3702 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
|
||||||
oid sha256:148abc2a03f578abaea318edd1042c01d3bb04fc6d9e33b7cab0bab48d9ca53f |
|
||||||
size 4418 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
|
||||||
oid sha256:df61d4a3b7c0e04bb02dab5408be15db88e293b481301febb129aedd12cd0cab |
|
||||||
size 4337 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
|
||||||
oid sha256:8ad9558c20c7bf4a526874c51204a86cc41b884d071ff55fab9c778a6f2ef999 |
|
||||||
size 4935 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
|
||||||
oid sha256:14938e8888f7fc72cbf0d1c8614d39952be8909aeb707975afb9b03e2aa817eb |
|
||||||
size 5201 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
|
||||||
oid sha256:b76eeb34b5ff74e072d53cbd08e464ebab8361e805aa6063b23de821ea400287 |
|
||||||
size 3864 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
|
||||||
oid sha256:947332713be607beb2599a93f0d999b4019d096322766564fa1bcaed01b0eebd |
|
||||||
size 778 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
|
||||||
oid sha256:122176a24fd4a96f0a14201ddea9883bf4ca279ba62aaec78c97bcf1eb3a3174 |
|
||||||
size 15953 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1
|
|
||||||
oid sha256:089b21b377325c0b0f04b96b6ed4a8e5975b1c050191598cd64dc0a3a3565a71 |
|
||||||
size 45343 |
|
||||||
@ -0,0 +1 @@ |
|||||||
|
*.pyc |
||||||
@ -0,0 +1,6 @@ |
|||||||
|
__pycache__/ |
||||||
|
|
||||||
|
# Cython intermediates |
||||||
|
*_pyx.c |
||||||
|
*_pyx.o |
||||||
|
*_pyx.so |
||||||
@ -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 * |
||||||
@ -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" |
||||||
|
] |
||||||
|
} |
||||||
|
} |
||||||
@ -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 |
||||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@ -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, <double *> 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, <double *> 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, <double *> 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, <void *> 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, <void *> &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, <void *> 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, <void *> 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", <void *> &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, <void *> &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, <void *> &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, <void *> &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, <void *> &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, <double *> 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, <void *> 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, <void *> 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, <void *> value.data) |
||||||
|
elif field_ in mem_fields: |
||||||
|
acados_solver_common.ocp_nlp_set(self.nlp_config, \ |
||||||
|
self.nlp_solver, stage, field, <void *> 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, <void *> &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, <void *> &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, <void *> 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, <void *> &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, <void *> &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, <void *> &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) |
||||||
@ -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 |
||||||
@ -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" |
||||||
|
] |
||||||
|
} |
||||||
|
} |
||||||
@ -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 |
||||||
@ -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) |
||||||
@ -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) |
||||||
@ -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 $<TARGET_OBJECTS:${MODEL_OBJ}> $<TARGET_OBJECTS:${OCP_OBJ}> |
||||||
|
{%- if solver_options.integrator_type != "DISCRETE" %} |
||||||
|
$<TARGET_OBJECTS:${SIM_OBJ}> |
||||||
|
{%- 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 $<TARGET_OBJECTS:${MODEL_OBJ}> $<TARGET_OBJECTS:${OCP_OBJ}>) |
||||||
|
# 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} $<TARGET_OBJECTS:${MODEL_OBJ}> $<TARGET_OBJECTS:${OCP_OBJ}> |
||||||
|
{%- if solver_options.integrator_type != "DISCRETE" %} |
||||||
|
$<TARGET_OBJECTS:${SIM_OBJ}> |
||||||
|
{%- 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} $<TARGET_OBJECTS:${MODEL_OBJ}> $<TARGET_OBJECTS:${SIM_OBJ}>) |
||||||
|
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 $<TARGET_OBJECTS:${MODEL_OBJ}> $<TARGET_OBJECTS:${SIM_OBJ}>) |
||||||
|
install(TARGETS ${LIB_ACADOS_SIM_SOLVER} DESTINATION ${CMAKE_INSTALL_PREFIX}) |
||||||
|
endif(${BUILD_ACADOS_SIM_SOLVER_LIB}) |
||||||
|
{%- endif %} |
||||||
|
|
||||||
@ -0,0 +1 @@ |
|||||||
|
exclude_files=[main, acados_solver, acados_solver_sfun, Makefile, model].*\.? |
||||||
@ -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 %} |
||||||
@ -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 <stdio.h> |
||||||
|
#include <stdlib.h> |
||||||
|
#include <string.h> |
||||||
|
|
||||||
|
// 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; |
||||||
|
} |
||||||
@ -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 <stdlib.h> |
||||||
|
#include <stdio.h> |
||||||
|
#include <string.h> |
||||||
|
// 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; |
||||||
|
} |
||||||
|
|
||||||
@ -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 <stdio.h> |
||||||
|
#include <stdlib.h> |
||||||
|
#include <string.h> |
||||||
|
|
||||||
|
// 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; ii<se; ii++) |
||||||
|
{ |
||||||
|
int ng = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "ug"); |
||||||
|
MEX_DIM_CHECK_MAT(fun_name, "constr_C", nrow, ncol, ng, nx); |
||||||
|
|
||||||
|
ocp_nlp_constraints_model_set(config, dims, in, ii, "C", value); |
||||||
|
} |
||||||
|
} |
||||||
|
else if (!strcmp(field, "constr_lbx")) |
||||||
|
{ |
||||||
|
for (int ii=s0; ii<se; ii++) |
||||||
|
{ |
||||||
|
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "lbx"); |
||||||
|
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); |
||||||
|
|
||||||
|
ocp_nlp_constraints_model_set(config, dims, in, ii, "lbx", value); |
||||||
|
} |
||||||
|
} |
||||||
|
else if (!strcmp(field, "constr_ubx")) |
||||||
|
{ |
||||||
|
for (int ii=s0; ii<se; ii++) |
||||||
|
{ |
||||||
|
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "ubx"); |
||||||
|
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); |
||||||
|
|
||||||
|
ocp_nlp_constraints_model_set(config, dims, in, ii, "ubx", value); |
||||||
|
} |
||||||
|
} |
||||||
|
else if (!strcmp(field, "constr_lbu")) |
||||||
|
{ |
||||||
|
for (int ii=s0; ii<se; ii++) |
||||||
|
{ |
||||||
|
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "lbu"); |
||||||
|
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); |
||||||
|
|
||||||
|
ocp_nlp_constraints_model_set(config, dims, in, ii, "lbu", value); |
||||||
|
} |
||||||
|
} |
||||||
|
else if (!strcmp(field, "constr_ubu")) |
||||||
|
{ |
||||||
|
for (int ii=s0; ii<se; ii++) |
||||||
|
{ |
||||||
|
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "ubu"); |
||||||
|
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); |
||||||
|
|
||||||
|
ocp_nlp_constraints_model_set(config, dims, in, ii, "ubu", value); |
||||||
|
} |
||||||
|
} |
||||||
|
else if (!strcmp(field, "constr_D")) |
||||||
|
{ |
||||||
|
for (int ii=s0; ii<se; ii++) |
||||||
|
{ |
||||||
|
int ng = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "ug"); |
||||||
|
MEX_DIM_CHECK_MAT(fun_name, "constr_D", nrow, ncol, ng, nu); |
||||||
|
|
||||||
|
ocp_nlp_constraints_model_set(config, dims, in, ii, "D", value); |
||||||
|
} |
||||||
|
} |
||||||
|
else if (!strcmp(field, "constr_lg")) |
||||||
|
{ |
||||||
|
for (int ii=s0; ii<se; ii++) |
||||||
|
{ |
||||||
|
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "lg"); |
||||||
|
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); |
||||||
|
|
||||||
|
ocp_nlp_constraints_model_set(config, dims, in, ii, "lg", value); |
||||||
|
} |
||||||
|
} |
||||||
|
else if (!strcmp(field, "constr_ug")) |
||||||
|
{ |
||||||
|
for (int ii=s0; ii<se; ii++) |
||||||
|
{ |
||||||
|
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "ug"); |
||||||
|
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); |
||||||
|
|
||||||
|
ocp_nlp_constraints_model_set(config, dims, in, ii, "ug", value); |
||||||
|
} |
||||||
|
} |
||||||
|
else if (!strcmp(field, "constr_lh")) |
||||||
|
{ |
||||||
|
for (int ii=s0; ii<se; ii++) |
||||||
|
{ |
||||||
|
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "lh"); |
||||||
|
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); |
||||||
|
|
||||||
|
ocp_nlp_constraints_model_set(config, dims, in, ii, "lh", value); |
||||||
|
} |
||||||
|
} |
||||||
|
else if (!strcmp(field, "constr_uh")) |
||||||
|
{ |
||||||
|
for (int ii=s0; ii<se; ii++) |
||||||
|
{ |
||||||
|
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "uh"); |
||||||
|
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); |
||||||
|
|
||||||
|
ocp_nlp_constraints_model_set(config, dims, in, ii, "uh", value); |
||||||
|
} |
||||||
|
} |
||||||
|
// cost:
|
||||||
|
else if (!strcmp(field, "cost_y_ref")) |
||||||
|
{ |
||||||
|
for (int ii=s0; ii<se; ii++) |
||||||
|
{ |
||||||
|
if ((plan->nlp_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; ii<se; ii++) |
||||||
|
{ |
||||||
|
if ((plan->nlp_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; ii<se; ii++) |
||||||
|
{ |
||||||
|
if ((plan->nlp_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; ii<se; ii++) |
||||||
|
{ |
||||||
|
if ((plan->nlp_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; ii<se; ii++) |
||||||
|
{ |
||||||
|
ocp_nlp_cost_model_set(config, dims, in, ii, "Z", value); |
||||||
|
} |
||||||
|
} |
||||||
|
else if (!strcmp(field, "cost_Zl")) |
||||||
|
{ |
||||||
|
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "Zl"); |
||||||
|
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); |
||||||
|
for (int ii=s0; ii<se; ii++) |
||||||
|
{ |
||||||
|
ocp_nlp_cost_model_set(config, dims, in, ii, "Zl", value); |
||||||
|
} |
||||||
|
} |
||||||
|
else if (!strcmp(field, "cost_Zu")) |
||||||
|
{ |
||||||
|
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "Zu"); |
||||||
|
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); |
||||||
|
for (int ii=s0; ii<se; ii++) |
||||||
|
{ |
||||||
|
ocp_nlp_cost_model_set(config, dims, in, ii, "Zu", value); |
||||||
|
} |
||||||
|
} |
||||||
|
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; ii<se; ii++) |
||||||
|
{ |
||||||
|
ocp_nlp_cost_model_set(config, dims, in, ii, "z", value); |
||||||
|
} |
||||||
|
} |
||||||
|
else if (!strcmp(field, "cost_zl")) |
||||||
|
{ |
||||||
|
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "zl"); |
||||||
|
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); |
||||||
|
for (int ii=s0; ii<se; ii++) |
||||||
|
{ |
||||||
|
ocp_nlp_cost_model_set(config, dims, in, ii, "zl", value); |
||||||
|
} |
||||||
|
} |
||||||
|
else if (!strcmp(field, "cost_zu")) |
||||||
|
{ |
||||||
|
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "zu"); |
||||||
|
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); |
||||||
|
for (int ii=s0; ii<se; ii++) |
||||||
|
{ |
||||||
|
ocp_nlp_cost_model_set(config, dims, in, ii, "zu", value); |
||||||
|
} |
||||||
|
} |
||||||
|
// constraints TODO
|
||||||
|
// // NOTE(oj): how is it with Jbx, Jbu, idxb can they be changed?!
|
||||||
|
// else if (!strcmp(field, "constr_lbx"))
|
||||||
|
// {
|
||||||
|
// // bounds at 0 are a special case.
|
||||||
|
// if (s0==0)
|
||||||
|
// {
|
||||||
|
// sprintf(buffer, "%s cannot set %s for stage 0", fun_name, field);
|
||||||
|
// mexErrMsgTxt(buffer);
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
// initializations
|
||||||
|
else if (!strcmp(field, "init_x")) |
||||||
|
{ |
||||||
|
if (nrhs!=min_nrhs) |
||||||
|
MEX_SETTER_NO_STAGE_SUPPORT(fun_name, field) |
||||||
|
|
||||||
|
acados_size = (N+1) * nx; |
||||||
|
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); |
||||||
|
for (int ii=0; ii<=N; ii++) |
||||||
|
{ |
||||||
|
ocp_nlp_out_set(config, dims, out, ii, "x", value+ii*nx); |
||||||
|
} |
||||||
|
} |
||||||
|
else if (!strcmp(field, "init_u")) |
||||||
|
{ |
||||||
|
if (nrhs!=min_nrhs) |
||||||
|
MEX_SETTER_NO_STAGE_SUPPORT(fun_name, field) |
||||||
|
|
||||||
|
acados_size = N*nu; |
||||||
|
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); |
||||||
|
for (int ii=0; ii<N; ii++) |
||||||
|
{ |
||||||
|
ocp_nlp_out_set(config, dims, out, ii, "u", value+ii*nu); |
||||||
|
} |
||||||
|
} |
||||||
|
else if (!strcmp(field, "init_z")) |
||||||
|
{ |
||||||
|
sim_solver_plan_t sim_plan = plan->sim_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; ii<N; ii++) |
||||||
|
{ |
||||||
|
ocp_nlp_set(config, solver, ii, "z_guess", value+ii*nz); |
||||||
|
} |
||||||
|
} |
||||||
|
else |
||||||
|
{ |
||||||
|
MEX_FIELD_ONLY_SUPPORTED_FOR_SOLVER(fun_name, "init_z", "irk") |
||||||
|
} |
||||||
|
} |
||||||
|
else if (!strcmp(field, "init_xdot")) |
||||||
|
{ |
||||||
|
sim_solver_plan_t sim_plan = plan->sim_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; ii<N; ii++) |
||||||
|
{ |
||||||
|
ocp_nlp_set(config, solver, ii, "xdot_guess", value+ii*nx); |
||||||
|
} |
||||||
|
} |
||||||
|
else |
||||||
|
{ |
||||||
|
MEX_FIELD_ONLY_SUPPORTED_FOR_SOLVER(fun_name, "init_z", "irk") |
||||||
|
} |
||||||
|
} |
||||||
|
else if (!strcmp(field, "init_gnsf_phi")) |
||||||
|
{ |
||||||
|
sim_solver_plan_t sim_plan = plan->sim_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; ii<N; ii++) |
||||||
|
{ |
||||||
|
ocp_nlp_set(config, solver, ii, "gnsf_phi_guess", value+ii*nx); |
||||||
|
} |
||||||
|
} |
||||||
|
else |
||||||
|
{ |
||||||
|
MEX_FIELD_ONLY_SUPPORTED_FOR_SOLVER(fun_name, "init_gnsf_phi", "irk_gnsf") |
||||||
|
} |
||||||
|
} |
||||||
|
else if (!strcmp(field, "init_pi")) |
||||||
|
{ |
||||||
|
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; ii<N; ii++) |
||||||
|
{ |
||||||
|
ocp_nlp_out_set(config, dims, out, ii, "pi", value+ii*nx); |
||||||
|
} |
||||||
|
} |
||||||
|
else if (!strcmp(field, "init_lam")) |
||||||
|
{ |
||||||
|
for (int ii=s0; ii<se; ii++) |
||||||
|
{ |
||||||
|
int nlam = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "lam"); |
||||||
|
MEX_DIM_CHECK_VEC(fun_name, "lam", nrow*ncol, nlam); |
||||||
|
|
||||||
|
ocp_nlp_out_set(config, dims, out, ii, "lam", value); |
||||||
|
} |
||||||
|
} |
||||||
|
else if (!strcmp(field, "init_t")) |
||||||
|
{ |
||||||
|
for (int ii=s0; ii<se; ii++) |
||||||
|
{ |
||||||
|
int nt = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "t"); |
||||||
|
MEX_DIM_CHECK_VEC(fun_name, "t", nrow*ncol, nt); |
||||||
|
|
||||||
|
ocp_nlp_out_set(config, dims, out, ii, "t", value); |
||||||
|
} |
||||||
|
} |
||||||
|
else if (!strcmp(field, "p")) |
||||||
|
{ |
||||||
|
if (nrhs==min_nrhs) // all stages
|
||||||
|
{ |
||||||
|
for (int ii=0; ii<=N; ii++) |
||||||
|
{ |
||||||
|
{{ model.name }}_acados_update_params(capsule, ii, value, matlab_size); |
||||||
|
} |
||||||
|
} |
||||||
|
else if (nrhs==min_nrhs+1) // one stage
|
||||||
|
{ |
||||||
|
int stage = mxGetScalar( prhs[6] ); |
||||||
|
{{ model.name }}_acados_update_params(capsule, stage, value, matlab_size); |
||||||
|
} |
||||||
|
} |
||||||
|
else if (!strcmp(field, "nlp_solver_max_iter")) |
||||||
|
{ |
||||||
|
acados_size = 1; |
||||||
|
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); |
||||||
|
int nlp_solver_max_iter = (int) value[0]; |
||||||
|
ocp_nlp_solver_opts_set(config, opts, "max_iter", &nlp_solver_max_iter); |
||||||
|
} |
||||||
|
else if (!strcmp(field, "rti_phase")) |
||||||
|
{ |
||||||
|
acados_size = 1; |
||||||
|
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); |
||||||
|
int rti_phase = (int) value[0]; |
||||||
|
if (plan->nlp_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; |
||||||
|
} |
||||||
|
|
||||||
@ -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 <stdlib.h> |
||||||
|
#include <stdio.h> |
||||||
|
#include <string.h> |
||||||
|
// 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); |
||||||
|
|
||||||
|
} |
||||||
@ -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 <stdio.h> |
||||||
|
#include <stdlib.h> |
||||||
|
|
||||||
|
// 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; |
||||||
|
}; |
||||||
|
|
||||||
@ -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_
|
||||||
@ -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 |
||||||
File diff suppressed because it is too large
Load Diff
@ -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_
|
||||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in new issue