diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 2e18856d03..6ff50ca719 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -3,6 +3,7 @@ repos: rev: v3.4.0 hooks: - id: check-ast + exclude: '^(pyextra)/' - id: check-json - id: check-xml - id: check-yaml @@ -38,7 +39,7 @@ repos: entry: cppcheck language: system types: [c++] - exclude: '^(phonelibs)|(cereal)|(opendbc)|(panda)|(tools)|(selfdrive/modeld/thneed/debug)|(selfdrive/modeld/test)|(selfdrive/camerad/test)/|(installer)' + exclude: '^(phonelibs)|(pyextra)|(cereal)|(opendbc)|(panda)|(tools)|(selfdrive/modeld/thneed/debug)|(selfdrive/modeld/test)|(selfdrive/camerad/test)/|(installer)' args: - --error-exitcode=1 - --language=c++ diff --git a/Pipfile b/Pipfile index 83cc2d346b..c6e921a2c4 100644 --- a/Pipfile +++ b/Pipfile @@ -74,6 +74,8 @@ parameterized = "*" ft4222 = "*" hypothesis = "*" inputs = "*" +casadi = "*" +future-fstrings = "*" # for acados [packages] atomicwrites = "*" diff --git a/Pipfile.lock b/Pipfile.lock index 3e81bcc7a1..fb6660726e 100644 --- a/Pipfile.lock +++ b/Pipfile.lock @@ -1,7 +1,7 @@ { "_meta": { "hash": { - "sha256": "bcb10219e933221977adebc26dc3df243463dee3065f6e773fdc328d3c337583" + "sha256": "3cc3a94f113f2dda8a55f12941780f3b1275997852ffbcf2c3a89d19de862dfd" }, "pipfile-spec": 6, "requires": { @@ -489,31 +489,31 @@ }, "protobuf": { "hashes": [ - "sha256:0b5a73fa43efda0df0191c162680ec40cef45463fa8ff69fbeaeeddda4c760f5", - "sha256:1e08da38d56b74962d9cb05d86ca5f25d2e5b78f05fd00db7900cad3faa6de00", - "sha256:38b2c6e2204731cfebdb2452ffb7addf0367172b35cff8ccda338ccea9e7c87a", - "sha256:42239d47f213f1ce12ddca62c0c70856efbac09797d715e5d606b3fbc3f16c44", - "sha256:45cc0197e9f9192693c8a4dbcba8c9227a53a2720fc3826dfe791113d9ff5e5e", - "sha256:4da073b6c1a83e4ff1294156844f21343c5094e295c194d8ecc94703c7a6f42a", - "sha256:4e62be28dcaab52c7e8e8e3fb9a7005dec6374e698f3b22d79276d95c13151e5", - "sha256:50c657a54592c1bec7b24521fdbbbd2f7b51325ba23ab505ed03e8ebf3a5aeff", - "sha256:557e16884d7276caf92c1fb188fe6dfbec47891d3507d4982db1e3d89ffd22de", - "sha256:5a3450acf046716e4a4f02a3f7adfb7b86f1b5b3ae392cec759915e79538d40d", - "sha256:6388e7f300010ea7ac77113c7491c5622645d2447fdf701cbfe026b832d728cd", - "sha256:744f5c9a3b9e7538c4c70f2b0e46f86858b684f5d17bf78643a19a6c21c7936f", - "sha256:751d71dc6559dd794d309811d8dcf179d6a128b38a1655ae7137530f33895cb4", - "sha256:816fe5e8b73c29adb13a57e1653da15f24cbb90e86ea92e6f08abe6d8c0cbdb4", - "sha256:89613ec119fdcad992f65d7a5bfe3170c17edc839982d0089fc0c9242fb8e517", - "sha256:993814b0199f22523a227696a8e20d2cd4b9cda60c76d2fb3969ce0c77eb9e0f", - "sha256:9dc01ddc3b195c4538942790c4b195cf17b52d3785a60c1f6f25b794f51e2071", - "sha256:b667ddbb77ff619fdbd18be75445d4448ee68493c9bddd1b4d0b177025e2f6f6", - "sha256:c2038dec269b65683f16057886c09ca7526471793029bdd43259282e1e0fb668", - "sha256:c303ce92c60d069237cfbd41790fde3d00066ca9500fac464454e20c2f12250c", - "sha256:c49e673436e24e925022c090a98905cfe88d056cb5dde67a8e20ae339acd8140", - "sha256:c5b512c1982f8b427a302db094cf79f8f235284014d01b23fba461aa2c1459a0", - "sha256:f3f057ad59cd4d5ea2b1bb88d36c6f009b8043007cf03d96cbd3b9c06859d4fa" - ], - "version": "==3.17.2" + "sha256:13ee7be3c2d9a5d2b42a1030976f760f28755fcf5863c55b1460fd205e6cd637", + "sha256:145ce0af55c4259ca74993ddab3479c78af064002ec8227beb3d944405123c71", + "sha256:14c1c9377a7ffbeaccd4722ab0aa900091f52b516ad89c4b0c3bb0a4af903ba5", + "sha256:1556a1049ccec58c7855a78d27e5c6e70e95103b32de9142bae0576e9200a1b0", + "sha256:26010f693b675ff5a1d0e1bdb17689b8b716a18709113288fead438703d45539", + "sha256:2ae692bb6d1992afb6b74348e7bb648a75bb0d3565a3f5eea5bec8f62bd06d87", + "sha256:2bfb815216a9cd9faec52b16fd2bfa68437a44b67c56bee59bc3926522ecb04e", + "sha256:4ffbd23640bb7403574f7aff8368e2aeb2ec9a5c6306580be48ac59a6bac8bde", + "sha256:6902a1e4b7a319ec611a7345ff81b6b004b36b0d2196ce7a748b3493da3d226d", + "sha256:6ce4d8bf0321e7b2d4395e253f8002a1a5ffbcfd7bcc0a6ba46712c07d47d0b4", + "sha256:6d847c59963c03fd7a0cd7c488cadfa10cda4fff34d8bc8cba92935a91b7a037", + "sha256:72804ea5eaa9c22a090d2803813e280fb273b62d5ae497aaf3553d141c4fdd7b", + "sha256:7a4c97961e9e5b03a56f9a6c82742ed55375c4a25f2692b625d4087d02ed31b9", + "sha256:8727ee027157516e2c311f218ebf2260a18088ffb2d29473e82add217d196b1c", + "sha256:99938f2a2d7ca6563c0ade0c5ca8982264c484fdecf418bd68e880a7ab5730b1", + "sha256:9b7a5c1022e0fa0dbde7fd03682d07d14624ad870ae52054849d8960f04bc764", + "sha256:a22b3a0dbac6544dacbafd4c5f6a29e389a50e3b193e2c70dae6bbf7930f651d", + "sha256:a981222367fb4210a10a929ad5983ae93bd5a050a0824fc35d6371c07b78caf6", + "sha256:ab6bb0e270c6c58e7ff4345b3a803cc59dbee19ddf77a4719c5b635f1d547aa8", + "sha256:c56c050a947186ba51de4f94ab441d7f04fcd44c56df6e922369cc2e1a92d683", + "sha256:e76d9686e088fece2450dbc7ee905f9be904e427341d289acbe9ad00b78ebd47", + "sha256:f0e59430ee953184a703a324b8ec52f571c6c4259d496a19d1cabcdc19dabc62", + "sha256:ffea251f5cd3c0b9b43c7a7a912777e0bc86263436a87c2555242a348817221b" + ], + "version": "==3.17.3" }, "psutil": { "hashes": [ @@ -826,11 +826,11 @@ }, "tqdm": { "hashes": [ - "sha256:736524215c690621b06fc89d0310a49822d75e599fcd0feb7cc742b98d692493", - "sha256:cd5791b5d7c3f2f1819efc81d36eb719a38e0906a7380365c556779f585ea042" + "sha256:24be966933e942be5f074c29755a95b315c69a91f839a29139bf26ffffe2d3fd", + "sha256:aa0c29f03f298951ac6318f7c8ce584e48fa22ec26396e6411e43d038243bdb2" ], "index": "pypi", - "version": "==4.61.0" + "version": "==4.61.1" }, "typing-extensions": { "hashes": [ @@ -857,11 +857,11 @@ }, "websocket-client": { "hashes": [ - "sha256:3e2bf58191d4619b161389a95bdce84ce9e0b24eb8107e7e590db682c2d0ca81", - "sha256:abf306dc6351dcef07f4d40453037e51cc5d9da2ef60d0fc5d0fe3bcda255372" + "sha256:b68e4959d704768fa20e35c9d508c8dc2bbc041fd8d267c0d7345cffe2824568", + "sha256:e5c333bfa9fa739538b652b6f8c8fc2559f1d364243c8a689d7c0e1d41c2e611" ], "index": "pypi", - "version": "==1.0.1" + "version": "==1.1.0" }, "werkzeug": { "hashes": [ @@ -1020,11 +1020,11 @@ }, "azure-cli-core": { "hashes": [ - "sha256:442762a13cb066155ef19aa483721ea81107a32331582d4c18af86cc0b42d2ec", - "sha256:9ceb8574525733171400e16cfdee279945343d27a348324b6798c5eac036681d" + "sha256:679fbaeab0224cb721d27070feaf61510c3628c4af463af518b59e30735335ae", + "sha256:a4bff7b9ff5e9e658208c712f75376035a482a80b5dae808b8c1489c982da58f" ], "index": "pypi", - "version": "==2.24.2" + "version": "==2.25.0" }, "azure-cli-telemetry": { "hashes": [ @@ -1043,11 +1043,11 @@ }, "azure-core": { "hashes": [ - "sha256:2e3ade1b6d79229ad58d4ed8775a07a0c6323a00328ee202964200f299ba929a", - "sha256:f32bb64aabe61f496255c16dd6c555a027da628109460bf27311cee0caf78f96" + "sha256:197917b98fec661c35392e32abec4f690ac2117371a814e25e57c224ce23cf1f", + "sha256:74631dff314fd44419ac6a3a38e8af68418b08a1b6e6793128555db20501dd07" ], "index": "pypi", - "version": "==1.14.0" + "version": "==1.15.0" }, "azure-mgmt-core": { "hashes": [ @@ -1142,19 +1142,63 @@ }, "boto3": { "hashes": [ - "sha256:52025e0af7935cb7036037978de190e41ad7f6716d1de1e3669976e99d084edf", - "sha256:612aa5dc27b87ae1dc695e194f97af7da0fcc9e97aa80d9740732d78ba117119" + "sha256:8e5af9c7ea16ce1c35b7c3220d073dea9735bb1790107820d475462500ae1eff", + "sha256:e61607211816c194dbe2701db48dcddc87cf19372e6f57a9ebe4dfe93dfe177c" ], "index": "pypi", - "version": "==1.17.87" + "version": "==1.17.95" }, "botocore": { "hashes": [ - "sha256:04a5594ae1886233cb15ab636b51aeecf6b5412231f72744405f11a54a8cda58", - "sha256:3dcc84855349073e0cb706e90a9e1180899deded4b8555698fb4a5a5b3357202" + "sha256:240a9ef007292e986a4e11662f9038435d9d4fd242e083db160c86eb5c24af30", + "sha256:dc215f59735a3abde6c66a61f43f10d95bc18754d310da4e2037b3b8c4d8aa2d" ], "markers": "python_version >= '2.7' and python_version not in '3.0, 3.1, 3.2, 3.3, 3.4, 3.5'", - "version": "==1.20.87" + "version": "==1.20.95" + }, + "casadi": { + "hashes": [ + "sha256:09e103bb597d46aa338fc57bc49270068a1f07be35f9494c9f796dea4b801aeb", + "sha256:13277151efc76b221de8ca6b5ab7b8bbdd2b0e139f282866840adf88dfe53bc9", + "sha256:1c451a07b2440c00d552e040b6285b6e79b677d2978212368b28b86f5d267669", + "sha256:2322748a8d5e88750fd2fc0abcdc56cfbad1a8cd538fe0e7d7b6d8ce0cb3fa62", + "sha256:24fbac649ee26572884029dcd0e108b4a2412cad003a84ed915c4e44a94ecae7", + "sha256:253569c85f881a6a8fe5e1c0758858edb1ecb4c3d8bce4aee4b52e5dc59fc091", + "sha256:292e2768280393bad406256e0ef9c30ddcd4867dbd42148b36f9d92a32d9e199", + "sha256:353a79e50aa84ac5e0d9f04bc3b2d78a2cc8edae3b842d757756449682778944", + "sha256:36db4c84d8f3aad328faaeaeaa454a633c95a854d78ea188791b147888379342", + "sha256:3aec6737c282e7fb5be41f6c7d0649e52ce49efb3508f30bada707e809bbbb5f", + "sha256:4086669280b2335d235c664373db46dcd7f6485dba4663ce1944ea01753c5e8b", + "sha256:4143803af909f284400c02f59de4d97e5ba9319de28366215ef55ef261914f9a", + "sha256:473bb86fa64ac9703d74a474514703b4665fa9a384221ced620b5025e64532a7", + "sha256:4932b2b5361013420189dbc8d30e970672d036b37cb382f1c09c3b6cfe651a37", + "sha256:49a8b713f0ff0bbc2f2af2e71c515cdced238786e25ef504f5982618c84c67a7", + "sha256:54d89442058271007ae8573dfa33360bea10e26603545481090b45e8b90c9d10", + "sha256:55df534d003efdd120c4ebfeb6b252c443d273cdc4b97a394eb0268367477795", + "sha256:5de5c3c1381ac303e71fdef75dace34af6e1d50b46ac081051cd209b8b933837", + "sha256:5f6eb8de31735c14ecc777e3ad77b57767b5f2dbea29265909ef696f51e8be92", + "sha256:6192e2ed81c15a7dab2554f5f69b134df8d1a982f8d9f13e57bdef93364d2120", + "sha256:643e48f92eaf65eb82964816bb7e7064ddb8239959210fa6168e8bce6fe6ef94", + "sha256:6ce7ac8a301a145f98d46db0bfd13bc8b3831a5bb92e8054d531a1f233bb4b93", + "sha256:7309a75b27c57f09b00a61815fb38c40da8e62e3004598e55ea1b8f713d96221", + "sha256:77f33cb95be6a49b93d8d6b81f05193676ae09857699cedf8f1a14a4285d077e", + "sha256:7a624d40c7b5ded7916f6cc65998af4585b4557c9ea65dc1e3a6273ebb2313ec", + "sha256:a06c0b96eb9d3bc88c627eec6e465726934ca0394347dc33efc742b8c91db83d", + "sha256:a4ce51e988570160af9ccfbbb1b9679546cbb1865d3a74ef0276f37fd94d91d9", + "sha256:ab6a600a9b2ea27453d56fd4464ad0db0ae69f5cea42595fcbdaabcd40396440", + "sha256:ab85c7cf772ba54f2718ebe366b836fffff868443f7c0c02389ed0a288cbde1f", + "sha256:ac45b91616e9b8afbe266ca08e80770b28e9e6d7a5852e3677fb37e42bde2047", + "sha256:adf20c34ba2cec1840a026023d93cc6d9b3581dfda6a044f434fc75b50c9a2ce", + "sha256:bd94048388b602fc30fdac2fecb986c034110ed8d2d17af7fd13b0de45c58bd7", + "sha256:c3440c90c31b61ae1df82f6c784643393f723354dc08013f9d5cedf25507c67c", + "sha256:cd630a2e6ec6df6a4977af63080fa8d63a0053ff8c06ea0200959b47ae75201c", + "sha256:d4e49cb46404cef61f83b30bb20ec9597c50ae7f55cfd6b89c17facc74675437", + "sha256:ec26244f9d9047f1bb401f1b86ff4775e1ddf638f4b4992bbc362a27a6f56673", + "sha256:f08a99e98b0a15083f06b1e221f064a29b3ed9e20617dc55aa8e823f2f732ace", + "sha256:fbf39dcd63f1d3b63c300fce59b7ea678bd5ea1d014e1e090a5226600a4132cb" + ], + "index": "pypi", + "version": "==3.5.5" }, "certifi": { "hashes": [ @@ -1469,13 +1513,21 @@ "markers": "python_version >= '2.6' and python_version not in '3.0, 3.1, 3.2, 3.3'", "version": "==0.18.2" }, + "future-fstrings": { + "hashes": [ + "sha256:6cf41cbe97c398ab5a81168ce0dbb8ad95862d3caf23c21e4430627b90844089", + "sha256:90e49598b553d8746c4dc7d9442e0359d038c3039d802c91c0a55505da318c63" + ], + "index": "pypi", + "version": "==1.2.0" + }, "geoalchemy2": { "hashes": [ - "sha256:567e105e6b0d5fd94a5a869d358b0c47956570bb3f3d19e98086344b1abefed8", - "sha256:56dff13365ac2ea5ab828a697e240984f7ac448a32a3feba25356b4aa8e24d34" + "sha256:5e54d979d49574e176b5eef6a9e7ab0d217a9f1098071dd578dbddc4b1f7ad9d", + "sha256:c32023bc2fb8fbb136f00a0e9c2feba21f3e1040af0f619c888661f6ee72dd28" ], "index": "pypi", - "version": "==0.8.5" + "version": "==0.9.0" }, "git-pylint-commit-hook": { "hashes": [ @@ -1572,27 +1624,27 @@ }, "humanfriendly": { "hashes": [ - "sha256:066562956639ab21ff2676d1fda0b5987e985c534fc76700a19bd54bcb81121d", - "sha256:d5c731705114b9ad673754f3317d9fa4c23212f36b29bdc4272a892eafc9bc72" + "sha256:332da98c24cc150efcc91b5508b19115209272bfdf4b0764a56795932f854271", + "sha256:f7dba53ac7935fd0b4a2fc9a29e316ddd9ea135fb3052d3d0279d10c18ff9c48" ], "markers": "python_version >= '2.7' and python_version not in '3.0, 3.1, 3.2, 3.3, 3.4'", - "version": "==9.1" + "version": "==9.2" }, "hypothesis": { "hashes": [ - "sha256:cca0e54d769214849755f7a125a2f759615e0386844eb42bd30ca2633d249b61", - "sha256:eeb8311d1477de4974ed3dba5c8e758023c17bb4c804f2e5d5cc4f9a065cbeba" + "sha256:27aa2af763af06b8b61ce65c09626cf1da6d3a6ff155900f3c581837b453313a", + "sha256:9bdee01ae260329b16117e9b0229a839b4a77747a985922653f595bd2a6a541a" ], "index": "pypi", - "version": "==6.13.12" + "version": "==6.14.0" }, "identify": { "hashes": [ - "sha256:3a74cf41f1a0bde21e2f196a1727a608629339d9897226bad07b22e0bbde6faa", - "sha256:7d4a73d58869a484d4ba8892793c80e30aa699e77707f26c1049486ef9be415f" + "sha256:18d0c531ee3dbc112fa6181f34faa179de3f57ea57ae2899754f16a7e0ff6421", + "sha256:5b41f71471bc738e7b586308c3fca172f78940195cb3bf6734c1e66fdac49306" ], "markers": "python_full_version >= '3.6.1'", - "version": "==2.2.8" + "version": "==2.2.10" }, "idna": { "hashes": [ @@ -1648,13 +1700,6 @@ ], "version": "==7.6.3" }, - "isodate": { - "hashes": [ - "sha256:2e364a3d5759479cdb2d37cce6b9376ea504db2ff90252a2e5b7cc89cc9ff2d8", - "sha256:aa4d33c06640f5352aca96e4b81afd8ab3b47337cc12089822d6f322ac772c81" - ], - "version": "==0.6.0" - }, "isort": { "hashes": [ "sha256:0a943902919f65c5684ac4e0154b1ad4fac6dcaa5d9f3426b732f1c8b5419be6", @@ -1969,20 +2014,6 @@ "index": "pypi", "version": "==0.5.6" }, - "msrest": { - "hashes": [ - "sha256:72661bc7bedc2dc2040e8f170b6e9ef226ee6d3892e01affd4d26b06474d68d8", - "sha256:c840511c845330e96886011a236440fafc2c9aff7b2df9c0a92041ee2dee3782" - ], - "version": "==0.6.21" - }, - "msrestazure": { - "hashes": [ - "sha256:3de50f56147ef529b31e099a982496690468ecef33f0544cb0fa0cfe1e1de5b9", - "sha256:a06f0dabc9a6f5efe3b6add4bd8fb623aeadacf816b7a35b0f89107e0544d189" - ], - "version": "==0.6.4" - }, "multidict": { "hashes": [ "sha256:018132dbd8688c7a69ad89c4a3f39ea2f9f33302ebe567a879da8f4ca73f0d0a", @@ -2028,31 +2059,32 @@ }, "mypy": { "hashes": [ - "sha256:0d0a87c0e7e3a9becdfbe936c981d32e5ee0ccda3e0f07e1ef2c3d1a817cf73e", - "sha256:25adde9b862f8f9aac9d2d11971f226bd4c8fbaa89fb76bdadb267ef22d10064", - "sha256:28fb5479c494b1bab244620685e2eb3c3f988d71fd5d64cc753195e8ed53df7c", - "sha256:2f9b3407c58347a452fc0736861593e105139b905cca7d097e413453a1d650b4", - "sha256:33f159443db0829d16f0a8d83d94df3109bb6dd801975fe86bacb9bf71628e97", - "sha256:3f2aca7f68580dc2508289c729bd49ee929a436208d2b2b6aab15745a70a57df", - "sha256:499c798053cdebcaa916eef8cd733e5584b5909f789de856b482cd7d069bdad8", - "sha256:4eec37370483331d13514c3f55f446fc5248d6373e7029a29ecb7b7494851e7a", - "sha256:552a815579aa1e995f39fd05dde6cd378e191b063f031f2acfe73ce9fb7f9e56", - "sha256:5873888fff1c7cf5b71efbe80e0e73153fe9212fafdf8e44adfe4c20ec9f82d7", - "sha256:61a3d5b97955422964be6b3baf05ff2ce7f26f52c85dd88db11d5e03e146a3a6", - "sha256:674e822aa665b9fd75130c6c5f5ed9564a38c6cea6a6432ce47eafb68ee578c5", - "sha256:7ce3175801d0ae5fdfa79b4f0cfed08807af4d075b402b7e294e6aa72af9aa2a", - "sha256:9743c91088d396c1a5a3c9978354b61b0382b4e3c440ce83cf77994a43e8c521", - "sha256:9f94aac67a2045ec719ffe6111df543bac7874cee01f41928f6969756e030564", - "sha256:a26f8ec704e5a7423c8824d425086705e381b4f1dfdef6e3a1edab7ba174ec49", - "sha256:abf7e0c3cf117c44d9285cc6128856106183938c68fd4944763003decdcfeb66", - "sha256:b09669bcda124e83708f34a94606e01b614fa71931d356c1f1a5297ba11f110a", - "sha256:cd07039aa5df222037005b08fbbfd69b3ab0b0bd7a07d7906de75ae52c4e3119", - "sha256:d23e0ea196702d918b60c8288561e722bf437d82cb7ef2edcd98cfa38905d506", - "sha256:d65cc1df038ef55a99e617431f0553cd77763869eebdf9042403e16089fe746c", - "sha256:d7da2e1d5f558c37d6e8c1246f1aec1e7349e4913d8fb3cb289a35de573fe2eb" - ], - "index": "pypi", - "version": "==0.812" + "sha256:0190fb77e93ce971954c9e54ea61de2802065174e5e990c9d4c1d0f54fbeeca2", + "sha256:0756529da2dd4d53d26096b7969ce0a47997123261a5432b48cc6848a2cb0bd4", + "sha256:2f9fedc1f186697fda191e634ac1d02f03d4c260212ccb018fabbb6d4b03eee8", + "sha256:353aac2ce41ddeaf7599f1c73fed2b75750bef3b44b6ad12985a991bc002a0da", + "sha256:3f12705eabdd274b98f676e3e5a89f247ea86dc1af48a2d5a2b080abac4e1243", + "sha256:4efc67b9b3e2fddbe395700f91d5b8deb5980bfaaccb77b306310bd0b9e002eb", + "sha256:517e7528d1be7e187a5db7f0a3e479747307c1b897d9706b1c662014faba3116", + "sha256:68a098c104ae2b75e946b107ef69dd8398d54cb52ad57580dfb9fc78f7f997f0", + "sha256:746e0b0101b8efec34902810047f26a8c80e1efbb4fc554956d848c05ef85d76", + "sha256:8be7bbd091886bde9fcafed8dd089a766fa76eb223135fe5c9e9798f78023a20", + "sha256:9236c21194fde5df1b4d8ebc2ef2c1f2a5dc7f18bcbea54274937cae2e20a01c", + "sha256:9ef5355eaaf7a23ab157c21a44c614365238a7bdb3552ec3b80c393697d974e1", + "sha256:9f1d74eeb3f58c7bd3f3f92b8f63cb1678466a55e2c4612bf36909105d0724ab", + "sha256:a26d0e53e90815c765f91966442775cf03b8a7514a4e960de7b5320208b07269", + "sha256:ae94c31bb556ddb2310e4f913b706696ccbd43c62d3331cd3511caef466871d2", + "sha256:b5ba1f0d5f9087e03bf5958c28d421a03a4c1ad260bf81556195dffeccd979c4", + "sha256:b5dfcd22c6bab08dfeded8d5b44bdcb68c6f1ab261861e35c470b89074f78a70", + "sha256:cd01c599cf9f897b6b6c6b5d8b182557fb7d99326bcdf5d449a0fbbb4ccee4b9", + "sha256:e89880168c67cf4fde4506b80ee42f1537ad66ad366c101d388b3fd7d7ce2afd", + "sha256:ebe2bc9cb638475f5d39068d2dbe8ae1d605bb8d8d3ff281c695df1670ab3987", + "sha256:f89bfda7f0f66b789792ab64ce0978e4a991a0e4dd6197349d0767b0f1095b21", + "sha256:fc4d63da57ef0e8cd4ab45131f3fe5c286ce7dd7f032650d0fbc239c6190e167", + "sha256:fd634bc17b1e2d6ce716f0e43446d0d61cdadb1efcad5c56ca211c22b246ebc8" + ], + "index": "pypi", + "version": "==0.902" }, "mypy-extensions": { "hashes": [ @@ -2146,35 +2178,31 @@ "index": "pypi", "version": "==1.20.3" }, - "oauthlib": { - "hashes": [ - "sha256:42bf6354c2ed8c6acb54d971fce6f88193d97297e18602a3a886603f9d7730cc", - "sha256:8f0215fcc533dd8dd1bee6f4c412d4f0cd7297307d43ac61666389e3bc3198a3" - ], - "markers": "python_version >= '3.6'", - "version": "==3.1.1" - }, "opencv-python": { "hashes": [ - "sha256:1a9d6dd0b1d5cb6031c38b6acf2043b2ec1717093b39592c2a0b98f5b31c8d3e", - "sha256:2cc96b8a9011fa3451d71699069a6a8c3578d91d0158c559def997a1a049ae0c", - "sha256:31d6a413d459a18ef280d88e8d9257cc5e5009657022c88bb88694a94ccce3ba", - "sha256:35d4fccd1e339df1e38eb1938af36c1972ab818684a747160403c1562008d867", - "sha256:642ff6fe8d0a8297e248bf21676172381afbda767cd15ad51ab9d10201d99078", - "sha256:68f1baa789a1ae92642ad436d29088051471b2e5cea40705cb8c9f8c9fc3c050", - "sha256:71adc9f528d54fb41bfe275d7038ed4c9c5f1a4468419d4cac12dde6e938f3b8", - "sha256:72f4e4f28169ecb9282570c4a86e992a762ea35ff67b19c05aab2c84eee212ca", - "sha256:7e8e31b0e206c3567cb96408eb0e27314bb1526eb072f9e509db77af0262ecaf", - "sha256:950242774a367efc90e3bed89e6e2fab7ff2867e2eafa440e4775b1ae57ac9f8", - "sha256:a7aa72195cfd325a51fba287fadc61166367a3f89c57e7fe3e041132e192ac06", - "sha256:b236001a5bacd18622db037e581521b38cffae070f345e05a7dd837dc3b7f2ef", - "sha256:c8ee913a45aec73c4bb14c6c72c3c6ddbb0a1dd3ca45338b68bf169220396941", - "sha256:cd231e073579192a8e4c88739e263de23bdaee0be7d85daae3afbda77e477da7", - "sha256:dfa168f827561c52f746d5d80e3b10f31d6428bf6f72652765139129f189110b", - "sha256:fd2e9e70ce30bdfdba6beeffeab117b760e6390e6b606e4b9f4ff03ccd2d7d1f" - ], - "index": "pypi", - "version": "==4.5.2.52" + "sha256:0118a086fad8d77acdf46ac68df49d4167fbb85420f8bcf2615d7b74fc03aae0", + "sha256:050227e5728ea8316ec114aca8f43d56253cbb1c50983e3b136a988254a83118", + "sha256:08327a38564786bf73e387736f080e8ad4c110b394ca4af2ecec8277b305bf44", + "sha256:0a3aef70b7c53bbd22ade86a4318b8a2ad98d3c3ed3d0c315f18bf1a2d868709", + "sha256:10325c3fd571e33a11eb5f0e5d265d73baef22dbb34c977f28df7e22de47b0bc", + "sha256:2436b71346d1eed423577fac8cd3aa9c0832ea97452444dc7f856b2f09600dba", + "sha256:4b8814d3f0cf01e8b8624125f7dcfb095893abcc04083cb4968fa1629bc81161", + "sha256:4e6c2d8320168a4f76822fbb76df3b18688ac5e068d49ac38a4ce39af0f8e1a6", + "sha256:6b2573c6367ec0052b37e375d18638a885dd7a10a5ef8dd726b391969c227f23", + "sha256:6e2070e35f2aaca3d1259093c786d4e373004b36d89a94e81943247c6ed3d4e1", + "sha256:89a2b45429bf945988a17b0404431d9d8fdc9e04fb2450b56fa01f6f9477101d", + "sha256:8cf81f53ac5ad900ca443a8252c4e0bc1256f1c2cb2d8459df2ba1ac014dfa36", + "sha256:9680ab256ab31bdafd74f6cf55eb570e5629b5604d50fd69dd1bd2a8124f0611", + "sha256:a8020cc6145c6934192189058743a55189750df6dff894396edb8b35a380cc48", + "sha256:b3bef3f2a2ab3c201784d12ec6b5c9e61c920c15b6854d8d2f62fd019e3df846", + "sha256:b724a96eeb88842bd2371b1ffe2da73b6295063ba5c029aa34139d25b8315a3f", + "sha256:c446555cbbc4f5e809f9c15ac1b6200024032d9859f5ac5a2ca7669d09e4c91c", + "sha256:d9004e2cc90bb2862cdc1d062fac5163d3def55b200081d4520d3e90b4c7197b", + "sha256:ef3102b70aa59ab3fed69df30465c1b7587d681e963dfff5146de233c75df7ba", + "sha256:f12f39c1e5001e1c00df5873e3eee6f0232b7723a60b7ef438b1e23f1341df0e" + ], + "index": "pypi", + "version": "==4.5.2.54" }, "osmium": { "hashes": [ @@ -2819,14 +2847,6 @@ "index": "pypi", "version": "==2.25.1" }, - "requests-oauthlib": { - "hashes": [ - "sha256:7f71572defaecd16372f9006f33c2ec8c077c3cfa6f5911a9a90202beb513f3d", - "sha256:b4261601a71fd721a8bd6d7aa1cc1d6a8a93b4a9f5e96626f8e4d91e8beeaa6a", - "sha256:fa6c47b933f01060936d87ae9327fead68768b69c6c9ea2109c48be30f2d4dbc" - ], - "version": "==1.3.0" - }, "reverse-geocoder": { "hashes": [ "sha256:2a2e781b5f69376d922b78fe8978f1350c84fce0ddb07e02c834ecf98b57c75c" @@ -3008,39 +3028,39 @@ }, "sqlalchemy": { "hashes": [ - "sha256:196fb6bb2733834e506c925d7532f8eabad9d2304deef738a40846e54c31e236", - "sha256:1dd77acbc19bee9c0ba858ff5e4e5d5c60895495c83b4df9bcdf4ad5e9b74f21", - "sha256:216ff28fe803885ceb5b131dcee6507d28d255808dd5bcffcb3b5fa75be2e102", - "sha256:461a4ea803ce0834822f372617a68ac97f9fa1281f2a984624554c651d7c3ae1", - "sha256:4b09191ed22af149c07a880f309b7740f3f782ff13325bae5c6168a6aa57e715", - "sha256:4c5e20666b33b03bf7f14953f0deb93007bf8c1342e985bd7c7cf25f46fac579", - "sha256:4d93b62e98248e3e1ac1e91c2e6ee1e7316f704be1f734338b350b6951e6c175", - "sha256:5732858e56d32fa7e02468f4fd2d8f01ddf709e5b93d035c637762890f8ed8b6", - "sha256:58c02d1771bb0e61bc9ced8f3b36b5714d9ece8fd4bdbe2a44a892574c3bbc3c", - "sha256:651cdb3adcee13624ba22d5ff3e96f91e16a115d2ca489ddc16a8e4c217e8509", - "sha256:6fe1c8dc26bc0005439cb78ebc78772a22cccc773f5a0e67cb3002d791f53f0f", - "sha256:7222f3236c280fab3a2d76f903b493171f0ffc29667538cc388a5d5dd0216a88", - "sha256:7dc3d3285fb682316d580d84e6e0840fdd8ffdc05cb696db74b9dd746c729908", - "sha256:7e45043fe11d503e1c3f9dcf5b42f92d122a814237cd9af68a11dae46ecfcae1", - "sha256:7eb55d5583076c03aaf1510473fad2a61288490809049cb31028af56af7068ee", - "sha256:82922a320d38d7d6aa3a8130523ec7e8c70fa95f7ca7d0fd6ec114b626e4b10b", - "sha256:8e133e2551fa99c75849848a4ac08efb79930561eb629dd7d2dc9b7ee05256e6", - "sha256:949ac299903d2ed8419086f81847381184e2264f3431a33af4679546dcc87f01", - "sha256:a2d225c8863a76d15468896dc5af36f1e196b403eb9c7e0151e77ffab9e7df57", - "sha256:a5f00a2be7d777119e15ccfb5ba0b2a92e8a193959281089d79821a001095f80", - "sha256:b0ad951a6e590bbcfbfeadc5748ef5ec8ede505a8119a71b235f7481cc08371c", - "sha256:b59b2c0a3b1d93027f6b6b8379a50c354483fe1ebe796c6740e157bb2e06d39a", - "sha256:bc89e37c359dcd4d75b744e5e81af128ba678aa2ecea4be957e80e6e958a1612", - "sha256:bde055c019e6e449ebc4ec61abd3e08690abeb028c7ada2a3b95d8e352b7b514", - "sha256:c367ed95d41df584f412a9419b5ece85b0d6c2a08a51ae13ae47ef74ff9a9349", - "sha256:dde05ae0987e43ec84e64d6722ce66305eda2a5e2b7d6fda004b37aabdfbb909", - "sha256:ee6e7ca09ff274c55d19a1e15ee6f884fa0230c0d9b8d22a456e249d08dee5bf", - "sha256:f1c68f7bd4a57ffdb85eab489362828dddf6cd565a4c18eda4c446c1d5d3059d", - "sha256:f63e1f531a8bf52184e2afb53648511f3f8534decb7575b483a583d3cd8d13ed", - "sha256:fdad4a33140b77df61d456922b7974c1f1bb2c35238f6809f078003a620c4734" - ], - "index": "pypi", - "version": "==1.4.17" + "sha256:0653d444d52f2b9a0cba1ea5cd0fc64e616ee3838ee86c1863781b2a8670fc0c", + "sha256:146af9e67d0f821b28779d602372e65d019db01532d8f7101e91202d447c14ec", + "sha256:2129d33b54da4d4771868a3639a07f461adc5887dbd9e0a80dbf560272245525", + "sha256:284b6df04bc30e886998e0fdbd700ef9ffb83bcb484ffc54d4084959240dce91", + "sha256:3690fc0fc671419debdae9b33df1434ac9253155fd76d0f66a01f7b459d56ee6", + "sha256:3a6afb7a55374329601c8fcad277f0a47793386255764431c8f6a231a6947ee9", + "sha256:45bbb935b305e381bcb542bf4d952232282ba76881e3458105e4733ba0976060", + "sha256:495cce8174c670f1d885e2259d710b0120888db2169ea14fc32d1f72e7950642", + "sha256:4cdc91bb3ee5b10e24ec59303131b791f3f82caa4dd8b36064d1918b0f4d0de4", + "sha256:4f375c52fed5f2ecd06be18756f121b3167a1fdc4543d877961fba04b1713214", + "sha256:56958dd833145f1aa75f8987dfe0cf6f149e93aa31967b7004d4eb9cb579fefc", + "sha256:5b827d3d1d982b38d2bab551edf9893c4734b5db9b852b28d3bc809ea7e179f6", + "sha256:5c62fff70348e3f8e4392540d31f3b8c251dc8eb830173692e5d61896d4309d6", + "sha256:5d4b2c23d20acf631456e645227cef014e7f84a111118d530cfa1d6053fd05a9", + "sha256:60cfe1fb59a34569816907cb25bb256c9490824679c46777377bcc01f6813a81", + "sha256:664c6cc84a5d2bad2a4a3984d146b6201b850ba0a7125b2fcd29ca06cddac4b1", + "sha256:70674f2ff315a74061da7af1225770578d23f4f6f74dd2e1964493abd8d804bc", + "sha256:77549e5ae996de50ad9f69f863c91daf04842b14233e133335b900b152bffb07", + "sha256:8924d552decf1a50d57dca4984ebd0778a55ca2cb1c0ef16df8c1fed405ff290", + "sha256:93394d68f02ecbf8c0a4355b6452793000ce0ee7aef79d2c85b491da25a88af7", + "sha256:9a62b06ad450386a2e671d0bcc5cd430690b77a5cd41c54ede4e4bf46d7a4978", + "sha256:c824d14b52000597dfcced0a4e480fd8664b09fed606e746a2c67fe5fbe8dfd9", + "sha256:cc474d0c40cef94d9b68980155d686d5ad43a9ca0834a8729052d3585f289d57", + "sha256:d25210f5f1a6b7b6b357d8fa199fc1d5be828c67cc1af517600c02e5b2727e4c", + "sha256:d76abceeb6f7c564fdbc304b1ce17ec59664ca7ed0fe6dbc6fc6a960c91370e3", + "sha256:e2aa39fdf5bff1c325a8648ac1957a0320c66763a3fa5f0f4a02457b2afcf372", + "sha256:eba098a4962e1ab0d446c814ae67e30da82c446b382cf718306cc90d4e2ad85f", + "sha256:ee3428f6100ff2b07e7ecec6357d865a4d604c801760094883587ecdbf8a3533", + "sha256:f3357948fa439eb5c7241a8856738605d7ab9d9f276ca5c5cc3220455a5f8e6c", + "sha256:ffb18eb56546aa66640fef831e5d0fe1a8dfbf11cdf5b00803826a01dbbbf3b1" + ], + "index": "pypi", + "version": "==1.4.18" }, "subprocess32": { "hashes": [ @@ -3068,11 +3088,11 @@ }, "terminado": { "hashes": [ - "sha256:048ce7b271ad1f94c48130844af1de163e54913b919f8c268c89b36a6d468d7c", - "sha256:46fd07c9dc7db7321922270d544a1f18eaa7a02fd6cd4438314f27a687cabbea" + "sha256:89d5dac2f4e2b39758a0ff9a3b643707c95a020a6df36e70583b88297cd59cbe", + "sha256:c89ace5bffd0e7268bdcf22526830eb787fd146ff9d78691a0528386f92b9ae3" ], "markers": "python_version >= '3.6'", - "version": "==0.10.0" + "version": "==0.10.1" }, "testpath": { "hashes": [ @@ -3084,11 +3104,11 @@ }, "tifffile": { "hashes": [ - "sha256:1cfc55f5b728e200142580a7bf108b72775c4097d007b4111876559fa1fb7432", - "sha256:55aa8baad38e1567c9fe450fff52160e4a21294a612f241c5e414da80f87209b" + "sha256:3201f5ba297b94328954724bd48dbf1b36ec14c4ee4cd5a2ec1aa3f83c486200", + "sha256:a2f83d82800a8d83cbd04340f9d65a6873a970874947a6b823b1b1238e84cba6" ], "markers": "python_version >= '3.7'", - "version": "==2021.4.8" + "version": "==2021.6.14" }, "toml": { "hashes": [ @@ -3153,41 +3173,6 @@ "markers": "python_version >= '3.7'", "version": "==5.0.5" }, - "typed-ast": { - "hashes": [ - "sha256:01ae5f73431d21eead5015997ab41afa53aa1fbe252f9da060be5dad2c730ace", - "sha256:067a74454df670dcaa4e59349a2e5c81e567d8d65458d480a5b3dfecec08c5ff", - "sha256:0fb71b8c643187d7492c1f8352f2c15b4c4af3f6338f21681d3681b3dc31a266", - "sha256:1b3ead4a96c9101bef08f9f7d1217c096f31667617b58de957f690c92378b528", - "sha256:2068531575a125b87a41802130fa7e29f26c09a2833fea68d9a40cf33902eba6", - "sha256:209596a4ec71d990d71d5e0d312ac935d86930e6eecff6ccc7007fe54d703808", - "sha256:2c726c276d09fc5c414693a2de063f521052d9ea7c240ce553316f70656c84d4", - "sha256:398e44cd480f4d2b7ee8d98385ca104e35c81525dd98c519acff1b79bdaac363", - "sha256:52b1eb8c83f178ab787f3a4283f68258525f8d70f778a2f6dd54d3b5e5fb4341", - "sha256:5feca99c17af94057417d744607b82dd0a664fd5e4ca98061480fd8b14b18d04", - "sha256:7538e495704e2ccda9b234b82423a4038f324f3a10c43bc088a1636180f11a41", - "sha256:760ad187b1041a154f0e4d0f6aae3e40fdb51d6de16e5c99aedadd9246450e9e", - "sha256:777a26c84bea6cd934422ac2e3b78863a37017618b6e5c08f92ef69853e765d3", - "sha256:95431a26309a21874005845c21118c83991c63ea800dd44843e42a916aec5899", - "sha256:9ad2c92ec681e02baf81fdfa056fe0d818645efa9af1f1cd5fd6f1bd2bdfd805", - "sha256:9c6d1a54552b5330bc657b7ef0eae25d00ba7ffe85d9ea8ae6540d2197a3788c", - "sha256:aee0c1256be6c07bd3e1263ff920c325b59849dc95392a05f258bb9b259cf39c", - "sha256:af3d4a73793725138d6b334d9d247ce7e5f084d96284ed23f22ee626a7b88e39", - "sha256:b36b4f3920103a25e1d5d024d155c504080959582b928e91cb608a65c3a49e1a", - "sha256:b9574c6f03f685070d859e75c7f9eeca02d6933273b5e69572e5ff9d5e3931c3", - "sha256:bff6ad71c81b3bba8fa35f0f1921fb24ff4476235a6e94a26ada2e54370e6da7", - "sha256:c190f0899e9f9f8b6b7863debfb739abcb21a5c054f911ca3596d12b8a4c4c7f", - "sha256:c907f561b1e83e93fad565bac5ba9c22d96a54e7ea0267c708bffe863cbe4075", - "sha256:cae53c389825d3b46fb37538441f75d6aecc4174f615d048321b716df2757fb0", - "sha256:dd4a21253f42b8d2b48410cb31fe501d32f8b9fbeb1f55063ad102fe9c425e40", - "sha256:dde816ca9dac1d9c01dd504ea5967821606f02e510438120091b84e852367428", - "sha256:f2362f3cb0f3172c42938946dbc5b7843c2a28aec307c49100c8b38764eb6927", - "sha256:f328adcfebed9f11301eaedfa48e15bdece9b519fb27e6a8c01aa52a17ec31b3", - "sha256:f8afcf15cc511ada719a88e013cec87c11aff7b91f019295eb4530f96fe5ef2f", - "sha256:fb1bbeac803adea29cedd70781399c99138358c26d05fcbd23c13016b7f5ec65" - ], - "version": "==1.4.3" - }, "typing-extensions": { "hashes": [ "sha256:0ac0f89795dd19de6b97debb0c6af1c70987fd80a2d62d1958f7e56fcc31b497", diff --git a/README.md b/README.md index 109be45ba8..9a11136ef7 100644 --- a/README.md +++ b/README.md @@ -344,13 +344,13 @@ Directory Structure . ├── cereal # The messaging spec and libs used for all logs ├── common # Library like functionality we've developed here - ├── installer/updater # Manages auto-updates of NEOS + ├── installer/updater # Manages updates of NEOS ├── opendbc # Files showing how to interpret data from cars ├── panda # Code used to communicate on CAN - ├── phonelibs # Libraries used on NEOS devices - ├── pyextra # Libraries used on NEOS devices + ├── phonelibs # External libraries + ├── pyextra # Extra python packages not shipped in NEOS └── selfdrive # Code needed to drive the car - ├── assets # Fonts, images and sounds for UI + ├── assets # Fonts, images, and sounds for UI ├── athena # Allows communication with the app ├── boardd # Daemon to talk to the board ├── camerad # Driver to capture images from the camera sensors @@ -364,7 +364,7 @@ Directory Structure ├── modeld # Driving and monitoring model runners ├── proclogd # Logs information from proc ├── sensord # IMU interface code - ├── test # Unit tests, system tests and a car simulator + ├── test # Unit tests, system tests, and a car simulator └── ui # The UI Licensing diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh index 1b71e0730c..ec724fd051 100755 --- a/launch_chffrplus.sh +++ b/launch_chffrplus.sh @@ -211,7 +211,7 @@ function launch { # handle pythonpath ln -sfn $(pwd) /data/pythonpath - export PYTHONPATH="$PWD" + export PYTHONPATH="$PWD:$PWD/pyextra" # hardware specific init if [ -f /EON ]; then diff --git a/phonelibs/acados/.gitignore b/phonelibs/acados/.gitignore new file mode 100644 index 0000000000..af1e27921e --- /dev/null +++ b/phonelibs/acados/.gitignore @@ -0,0 +1 @@ +acados/ diff --git a/phonelibs/acados/build.sh b/phonelibs/acados/build.sh new file mode 100755 index 0000000000..144a38c6d3 --- /dev/null +++ b/phonelibs/acados/build.sh @@ -0,0 +1,40 @@ +#!/usr/bin/bash -e + +DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)" + +ARCHNAME="x86_64" +BLAS_TARGET="X64_AUTOMATIC" +if [ -f /TICI ]; then + ARCHNAME="larch64" + BLAS_TARGET="ARMV8A_ARM_CORTEX_A57" +elif [ -f /EON ]; then + ARCHNAME="aarch64" + BLAS_TARGET="ARMV8A_ARM_CORTEX_A57" +fi + +if [ ! -d acados/ ]; then + #git clone https://github.com/acados/acados.git $DIR/acados + git clone https://github.com/commaai/acados.git $DIR/acados +fi +cd acados +git fetch +git checkout 9a1bab3f8fc4814a295fbf424fdc8125c63fdd08 +git submodule update --recursive --init + +# build +mkdir -p build +cd build +cmake -DACADOS_WITH_QPOASES=ON -UBLASFEO_TARGET -DBLASFEO_TARGET=$BLAS_TARGET .. +make -j4 install + +INSTALL_DIR="$DIR/$ARCHNAME" +rm -rf $INSTALL_DIR +mkdir -p $INSTALL_DIR + +rm $DIR/acados/lib/*.json + +cp -r $DIR/acados/include $DIR +cp -r $DIR/acados/lib $INSTALL_DIR +cp -r $DIR/acados/interfaces/acados_template/acados_template $DIR/../../pyextra + +#pip3 install -e $DIR/acados/interfaces/acados_template diff --git a/phonelibs/acados/include/acados_c/condensing_interface.h b/phonelibs/acados/include/acados_c/condensing_interface.h new file mode 100644 index 0000000000..51fe827127 --- /dev/null +++ b/phonelibs/acados/include/acados_c/condensing_interface.h @@ -0,0 +1,84 @@ +/* + * 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 INTERFACES_ACADOS_C_CONDENSING_INTERFACE_H_ +#define INTERFACES_ACADOS_C_CONDENSING_INTERFACE_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "acados/ocp_qp/ocp_qp_full_condensing.h" +#include "acados/ocp_qp/ocp_qp_partial_condensing.h" + +typedef enum { + PARTIAL_CONDENSING, + FULL_CONDENSING, +} condensing_t; + +typedef struct +{ + condensing_t condensing_type; +} condensing_plan; + +typedef struct +{ + ocp_qp_xcond_config *config; + void *dims; + void *opts; + void *mem; + void *work; +} condensing_module; + +ocp_qp_xcond_config *ocp_qp_condensing_config_create(condensing_plan *plan); +// +void *ocp_qp_condensing_opts_create(ocp_qp_xcond_config *config, void *dims_); +// +acados_size_t ocp_qp_condensing_calculate_size(ocp_qp_xcond_config *config, void *dims_, void *opts_); +// +condensing_module *ocp_qp_condensing_assign(ocp_qp_xcond_config *config, void *dims_, + void *opts_, void *raw_memory); +// +condensing_module *ocp_qp_condensing_create(ocp_qp_xcond_config *config, void *dims_, + void *opts_); +// +int ocp_qp_condense(condensing_module *module, void *qp_in, void *qp_out); +// +int ocp_qp_expand(condensing_module *module, void *qp_in, void *qp_out); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // INTERFACES_ACADOS_C_CONDENSING_INTERFACE_H_ diff --git a/phonelibs/acados/include/acados_c/dense_qp_interface.h b/phonelibs/acados/include/acados_c/dense_qp_interface.h new file mode 100644 index 0000000000..4ecc77381d --- /dev/null +++ b/phonelibs/acados/include/acados_c/dense_qp_interface.h @@ -0,0 +1,94 @@ +/* + * 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 INTERFACES_ACADOS_C_DENSE_QP_INTERFACE_H_ +#define INTERFACES_ACADOS_C_DENSE_QP_INTERFACE_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "acados/dense_qp/dense_qp_common.h" + +typedef enum { DENSE_QP_HPIPM, DENSE_QP_QORE, DENSE_QP_QPOASES, DENSE_QP_OOQP } dense_qp_solver_t; + +typedef struct +{ + dense_qp_solver_t qp_solver; +} dense_qp_solver_plan; + +typedef struct +{ + qp_solver_config *config; + void *dims; + void *opts; + void *mem; + void *work; +} dense_qp_solver; + +qp_solver_config *dense_qp_config_create(dense_qp_solver_plan *plan); +// +dense_qp_dims *dense_qp_dims_create(); +// +dense_qp_in *dense_qp_in_create(qp_solver_config *config, dense_qp_dims *dims); +// +dense_qp_out *dense_qp_out_create(qp_solver_config *config, dense_qp_dims *dims); +// +void *dense_qp_opts_create(qp_solver_config *config, dense_qp_dims *dims); +// +acados_size_t dense_qp_calculate_size(qp_solver_config *config, dense_qp_dims *dims, void *opts_); +// +dense_qp_solver *dense_qp_assign(qp_solver_config *config, dense_qp_dims *dims, void *opts_, + void *raw_memory); +// +dense_qp_solver *dense_qp_create(qp_solver_config *config, dense_qp_dims *dims, void *opts_); +// +int dense_qp_solve(dense_qp_solver *solver, dense_qp_in *qp_in, dense_qp_out *qp_out); +// +void dense_qp_inf_norm_residuals(dense_qp_dims *dims, dense_qp_in *qp_in, dense_qp_out *qp_out, + double *res); +// +bool dense_qp_set_field_double_array(const char *field, double *arr, dense_qp_in *qp_in); +// +bool dense_qp_set_field_int_array(const char *field, int *arr, dense_qp_in *qp_in); +// +bool dense_qp_get_field_double_array(const char *field, dense_qp_in *qp_in, double *arr); +// +bool dense_qp_get_field_int_array(const char *field, dense_qp_in *qp_in, int *arr); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // INTERFACES_ACADOS_C_DENSE_QP_INTERFACE_H_ diff --git a/phonelibs/acados/include/acados_c/external_function_interface.h b/phonelibs/acados/include/acados_c/external_function_interface.h new file mode 100644 index 0000000000..6838975071 --- /dev/null +++ b/phonelibs/acados/include/acados_c/external_function_interface.h @@ -0,0 +1,92 @@ +/* + * 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 INTERFACES_ACADOS_C_EXTERNAL_FUNCTION_INTERFACE_H_ +#define INTERFACES_ACADOS_C_EXTERNAL_FUNCTION_INTERFACE_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "acados/utils/external_function_generic.h" + + + +/************************************************ + * generic external parametric function + ************************************************/ + +// +void external_function_param_generic_create(external_function_param_generic *fun, int np); +// +void external_function_param_generic_free(external_function_param_generic *fun); + + + +/************************************************ + * casadi external function + ************************************************/ + +// +void external_function_casadi_create(external_function_casadi *fun); +// +void external_function_casadi_free(external_function_casadi *fun); +// +void external_function_casadi_create_array(int size, external_function_casadi *funs); +// +void external_function_casadi_free_array(int size, external_function_casadi *funs); + + + +/************************************************ + * casadi external parametric function + ************************************************/ + +// +void external_function_param_casadi_create(external_function_param_casadi *fun, int np); +// +void external_function_param_casadi_free(external_function_param_casadi *fun); +// +void external_function_param_casadi_create_array(int size, external_function_param_casadi *funs, + int np); +// +void external_function_param_casadi_free_array(int size, external_function_param_casadi *funs); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // INTERFACES_ACADOS_C_EXTERNAL_FUNCTION_INTERFACE_H_ diff --git a/phonelibs/acados/include/acados_c/ocp_nlp_interface.h b/phonelibs/acados/include/acados_c/ocp_nlp_interface.h new file mode 100644 index 0000000000..d4872f82e1 --- /dev/null +++ b/phonelibs/acados/include/acados_c/ocp_nlp_interface.h @@ -0,0 +1,399 @@ +/* + * 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 INTERFACES_ACADOS_C_OCP_NLP_INTERFACE_H_ +#define INTERFACES_ACADOS_C_OCP_NLP_INTERFACE_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// acados +#include "acados/ocp_nlp/ocp_nlp_common.h" +#include "acados/ocp_nlp/ocp_nlp_constraints_bgh.h" +#include "acados/sim/sim_erk_integrator.h" +#include "acados/sim/sim_irk_integrator.h" +#include "acados/sim/sim_lifted_irk_integrator.h" +#include "acados/sim/sim_gnsf.h" +// acados_c +#include "acados_c/ocp_qp_interface.h" +#include "acados_c/sim_interface.h" + + +/// Solution methods for optimal control problems. +typedef enum +{ + SQP, + SQP_RTI, + INVALID_NLP_SOLVER, +} ocp_nlp_solver_t; + + +/// Types of the cost function. +typedef enum +{ + LINEAR_LS, + NONLINEAR_LS, + EXTERNAL, + INVALID_COST, +} ocp_nlp_cost_t; + + +/// Types of the system dynamics, discrete or continuous time. +typedef enum +{ + CONTINUOUS_MODEL, + DISCRETE_MODEL, + INVALID_DYNAMICS, +} ocp_nlp_dynamics_t; + + +/// Constraint types +typedef enum +{ + /// Comprises simple bounds, polytopic constraints, + /// general non-linear constraints. + BGH, + + /// Comprises simple bounds, polytopic constraints, + /// general non-linear constraints, and positive definite constraints. + BGP, + + INVALID_CONSTRAINT, +} ocp_nlp_constraints_t; + + +/// Regularization types +typedef enum +{ + NO_REGULARIZE, + MIRROR, + PROJECT, + PROJECT_REDUC_HESS, + CONVEXIFY, + INVALID_REGULARIZE, +} ocp_nlp_reg_t; + + +/// Structure to store the configuration of a non-linear program +typedef struct ocp_nlp_plan +{ + /// QP solver configuration. + ocp_qp_solver_plan ocp_qp_solver_plan; + + /// Simulation solver configuration for each stage. + sim_solver_plan *sim_solver_plan; + + /// Nlp solver type. + ocp_nlp_solver_t nlp_solver; + + /// Regularization type, defaults to no regularization. + ocp_nlp_reg_t regularization; + + /// Cost type for each stage. + ocp_nlp_cost_t *nlp_cost; + + /// Dynamics type for each stage. + ocp_nlp_dynamics_t *nlp_dynamics; + + /// Constraints type for each stage. + ocp_nlp_constraints_t *nlp_constraints; + + /// Horizon length. + int N; + +} ocp_nlp_plan; + + +/// Structure to store the state/configuration for the non-linear programming solver +typedef struct ocp_nlp_solver +{ + ocp_nlp_config *config; + void *dims; + void *opts; + void *mem; + void *work; +} ocp_nlp_solver; + + +/// Constructs an empty plan struct (user nlp configuration), all fields are set to a +/// default/invalid state. +/// +/// \param N Horizon length +ocp_nlp_plan *ocp_nlp_plan_create(int N); + +/// Destructor for plan struct, frees memory. +/// +/// \param plan_ The plan struct to destroy. +void ocp_nlp_plan_destroy(void* plan_); + + +/// Constructs an nlp configuration struct from a plan. +/// +/// \param plan The plan (user nlp configuration). +ocp_nlp_config *ocp_nlp_config_create(ocp_nlp_plan plan); + +/// Desctructor of the nlp configuration. +/// +/// \param config_ The configuration struct. +void ocp_nlp_config_destroy(void *config_); + + +/// Constructs an struct that contains the dimensions of the variables. +/// +/// \param config_ The configuration struct. +ocp_nlp_dims *ocp_nlp_dims_create(void *config_); + +/// Destructor of The dimension struct. +/// +/// \param dims_ The dimension struct. +void ocp_nlp_dims_destroy(void *dims_); + + +/// Constructs an input struct for a non-linear programs. +/// +/// \param config The configuration struct. +/// \param dims The dimension struct. +ocp_nlp_in *ocp_nlp_in_create(ocp_nlp_config *config, ocp_nlp_dims *dims); + +/// Destructor of the inputs struct. +/// +/// \param in The inputs struct. +void ocp_nlp_in_destroy(void *in); + + +/// Sets the sampling times for the given stage. +/// +/// \param config The configuration struct. +/// \param dims The dimension struct. +/// \param in The inputs struct. +/// \param stage Stage number. +/// \param field Has to be "Ts" (TBC other options). +/// \param value The sampling times (floating point). +void ocp_nlp_in_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, int stage, + const char *field, void *value); + + +/// Sets the function pointers to the dynamics functions for the given stage. +/// +/// \param config The configuration struct. +/// \param dims The dimension struct. +/// \param in The inputs struct. +/// \param stage Stage number. +/// \param fun_type The name of the function type, either impl_ode_fun, +/// impl_ode_fun_jac_x_xdot, impl_ode_jac_x_xdot_u (TBC) +/// \param fun_ptr Function pointer to the dynamics function. +int ocp_nlp_dynamics_model_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, + int stage, const char *fun_type, void *fun_ptr); + + +/// Sets the function pointers to the cost functions for the given stage. +/// +/// \param config The configuration struct. +/// \param dims The dimension struct. +/// \param in The inputs struct. +/// \param stage Stage number. +/// \param field The name of the field, either nls_res_jac, +/// y_ref, W (others TBC) +/// \param value Cost values. +int ocp_nlp_cost_model_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, + int stage, const char *field, void *value); + + +/// Sets the function pointers to the constraints functions for the given stage. +/// +/// \param config The configuration struct. +/// \param dims The dimension struct. +/// \param in The inputs struct. +/// \param stage Stage number. +/// \param field The name of the field, either lb, ub (others TBC) +/// \param value Constraints function or values. +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 */ + +/// Constructs an output struct for the non-linear program. +/// +/// \param config The configuration struct. +/// \param dims The dimension struct. +ocp_nlp_out *ocp_nlp_out_create(ocp_nlp_config *config, ocp_nlp_dims *dims); + +/// Destructor of the output struct. +/// +/// \param out The output struct. +void ocp_nlp_out_destroy(void *out); + + +/// Sets fields in the output struct of an nlp solver, used to initialize the solver. +/// +/// \param config The configuration struct. +/// \param dims The dimension struct. +/// \param out The output struct. +/// \param stage Stage number. +/// \param field The name of the field, either x, u, pi. +/// \param value Initialization values. +void ocp_nlp_out_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, + int stage, const char *field, void *value); + + +/// Gets values of fields in the output struct of an nlp solver. +/// +/// \param config The configuration struct. +/// \param dims The dimension struct. +/// \param out The output struct. +/// \param stage Stage number. +/// \param field The name of the field, either x, u, z, pi. +/// \param value Pointer to the output memory. +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); + +// TODO(andrea): remove this once/if the MATLAB interface uses the new setters below? +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); + + +/* opts */ + +/// Creates an options struct for the non-linear program. +/// +/// \param config The configuration struct. +/// \param dims The dimension struct. +void *ocp_nlp_solver_opts_create(ocp_nlp_config *config, ocp_nlp_dims *dims); + +/// Destructor of the options. +/// +/// \param opts The options struct. +void ocp_nlp_solver_opts_destroy(void *opts); + +/// Sets an option. +/// +/// \param config The configuration struct. +/// \param opts_ The options struct. +/// \param field Name of the option. +/// \param value Value of the option. +void ocp_nlp_solver_opts_set(ocp_nlp_config *config, void *opts_, const char *field, void* value); + + +void ocp_nlp_solver_opts_set_at_stage(ocp_nlp_config *config, void *opts_, int stage, const char *field, void* value); + + +/// TBC +/// Updates the options. +/// +/// \param config The configuration struct. +/// \param dims The dimension struct. +/// \param opts_ The options struct. +void ocp_nlp_solver_opts_update(ocp_nlp_config *config, ocp_nlp_dims *dims, void *opts_); + + +/* solver */ + +/// Creates an ocp solver. +/// +/// \param config The configuration struct. +/// \param dims The dimension struct. +/// \param opts_ The options struct. +/// \return The solver. +ocp_nlp_solver *ocp_nlp_solver_create(ocp_nlp_config *config, ocp_nlp_dims *dims, void *opts_); + +/// Destructor of the solver. +/// +/// \param solver The solver struct. +void ocp_nlp_solver_destroy(void *solver); + +/// Solves the optimal control problem. Call ocp_nlp_precompute before +/// calling this functions (TBC). +/// +/// \param solver The solver struct. +/// \param nlp_in The inputs struct. +/// \param nlp_out The output struct. +int ocp_nlp_solve(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out); + +/// Performs precomputations for the solver. Needs to be called before +/// ocl_nlp_solve (TBC). +/// +/// \param solver The solver struct. +/// \param nlp_in The inputs struct. +/// \param nlp_out The output struct. +int ocp_nlp_precompute(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out); + + +/// Computes cost function value. +/// +/// \param solver The solver struct. +/// \param nlp_in The inputs struct. +/// \param nlp_out The output struct. +void ocp_nlp_eval_cost(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); + +/* get */ +/// \param config The configuration struct. +/// \param solver The solver struct. +/// \param field Supports "sqp_iter", "status", "nlp_res", "time_tot", ... +/// \param return_value_ Pointer to the output memory. +void ocp_nlp_get(ocp_nlp_config *config, ocp_nlp_solver *solver, + const char *field, void *return_value_); + +/* set */ +/// Sets the initial guesses for the integrator for the given stage. +/// +/// \param config The configuration struct. +/// \param solver The ocp_nlp_solver struct. +/// \param stage Stage number. +/// \param field Supports "z_guess", "xdot_guess" (IRK), "phi_guess" (GNSF-IRK) +/// \param value The initial guess for the algebraic variables in the integrator (if continuous model is used). +void ocp_nlp_set(ocp_nlp_config *config, ocp_nlp_solver *solver, + int stage, const char *field, void *value); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // INTERFACES_ACADOS_C_OCP_NLP_INTERFACE_H_ diff --git a/phonelibs/acados/include/acados_c/ocp_qp_interface.h b/phonelibs/acados/include/acados_c/ocp_qp_interface.h new file mode 100644 index 0000000000..a567ebeb35 --- /dev/null +++ b/phonelibs/acados/include/acados_c/ocp_qp_interface.h @@ -0,0 +1,261 @@ +/* + * 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 INTERFACES_ACADOS_C_OCP_QP_INTERFACE_H_ +#define INTERFACES_ACADOS_C_OCP_QP_INTERFACE_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "acados/ocp_qp/ocp_qp_common.h" +#include "acados/ocp_qp/ocp_qp_xcond_solver.h" + + +/// QP solver types (Enumeration). +/// +/// Full list of fields: +/// PARTIAL_CONDENSING_HPIPM +/// PARTIAL_CONDENSING_HPMPC +/// PARTIAL_CONDENSING_OOQP +/// PARTIAL_CONDENSING_OSQP +/// PARTIAL_CONDENSING_QPDUNES +/// FULL_CONDENSING_HPIPM +/// FULL_CONDENSING_QPOASES +/// FULL_CONDENSING_QORE +/// FULL_CONDENSING_OOQP +/// INVALID_QP_SOLVER +/// +/// Note: In this enumeration the partial condensing solvers have to be +/// specified before the full condensing solvers. +typedef enum { + PARTIAL_CONDENSING_HPIPM, +#ifdef ACADOS_WITH_HPMPC + PARTIAL_CONDENSING_HPMPC, +#else + PARTIAL_CONDENSING_HPMPC_NOT_AVAILABLE, +#endif +#ifdef ACADOS_WITH_OOQP + PARTIAL_CONDENSING_OOQP, +#else + PARTIAL_CONDENSING_OOQP_NOT_AVAILABLE, +#endif +#ifdef ACADOS_WITH_OSQP + PARTIAL_CONDENSING_OSQP, +#else + PARTIAL_CONDENSING_OSQP_NOT_AVAILABLE, +#endif +#ifdef ACADOS_WITH_QPDUNES + PARTIAL_CONDENSING_QPDUNES, +#else + PARTIAL_CONDENSING_QPDUNES_NOT_AVAILABLE, +#endif + FULL_CONDENSING_HPIPM, +#ifdef ACADOS_WITH_QPOASES + FULL_CONDENSING_QPOASES, +#else + FULL_CONDENSING_QPOASES_NOT_AVAILABLE, +#endif +#ifdef ACADOS_WITH_QORE + FULL_CONDENSING_QORE, +#else + FULL_CONDENSING_QORE_NOT_AVAILABLE, +#endif +#ifdef ACADOS_WITH_OOQP + FULL_CONDENSING_OOQP, +#else + FULL_CONDENSING_OOQP_NOT_AVAILABLE, +#endif + INVALID_QP_SOLVER, +} ocp_qp_solver_t; + + +/// Struct containing qp solver +typedef struct +{ + ocp_qp_solver_t qp_solver; +} ocp_qp_solver_plan; + + +/// Linear ocp configuration. +typedef struct +{ + ocp_qp_xcond_solver_config *config; + ocp_qp_xcond_solver_dims *dims; + void *opts; + void *mem; + void *work; +} ocp_qp_solver; + + +/// Initializes the qp solver configuration. +/// TBC should this be private/static - no, used in ocp_nlp +void ocp_qp_xcond_solver_config_initialize_from_plan( + ocp_qp_solver_t solver_name, ocp_qp_xcond_solver_config *solver_config); + +/// Constructs a qp solver config and Initializes with default values. +/// +/// \param plan The qp solver plan struct. +ocp_qp_xcond_solver_config *ocp_qp_xcond_solver_config_create(ocp_qp_solver_plan plan); + +/// Destructor for config struct, frees memory. +/// +/// \param config The config object to destroy. +void ocp_qp_xcond_solver_config_free(ocp_qp_xcond_solver_config *config); + + +/// Constructs a struct that contains the dimensions for the variables of the qp. +/// +/// \param N The number of variables. +ocp_qp_dims *ocp_qp_dims_create(int N); + +/// Destructor of The dimension struct. +/// +/// \param dims The dimension struct. +void ocp_qp_dims_free(void *dims); + +// +ocp_qp_xcond_solver_dims *ocp_qp_xcond_solver_dims_create(ocp_qp_xcond_solver_config *config, int N); +// +ocp_qp_xcond_solver_dims *ocp_qp_xcond_solver_dims_create_from_ocp_qp_dims( + ocp_qp_xcond_solver_config *config, ocp_qp_dims *dims); +// +void ocp_qp_xcond_solver_dims_free(ocp_qp_xcond_solver_dims *dims_); + +void ocp_qp_xcond_solver_dims_set(void *config_, ocp_qp_xcond_solver_dims *dims, + int stage, const char *field, int* value); + + +/// Constructs an input object for the qp. +/// +/// \param dims The dimension struct. +ocp_qp_in *ocp_qp_in_create(ocp_qp_dims *dims); + + +void ocp_qp_in_set(ocp_qp_xcond_solver_config *config, ocp_qp_in *in, + int stage, char *field, void *value); + +/// Destructor of the inputs struct. +/// +/// \param in_ The inputs struct. +void ocp_qp_in_free(void *in_); + + +/// Constructs an outputs object for the qp. +/// +/// \param dims The dimension struct. +ocp_qp_out *ocp_qp_out_create(ocp_qp_dims *dims); + +/// Destructor of the output struct. +/// +/// \param out_ The output struct. +void ocp_qp_out_free(void *out_); + + +/// Getter of output struct +void ocp_qp_out_get(ocp_qp_out *out, const char *field, void *value); + + +/// Constructs an options object for the qp. +/// +/// \param config The configuration struct. +/// \param dims The dimension struct. +void *ocp_qp_xcond_solver_opts_create(ocp_qp_xcond_solver_config *config, + ocp_qp_xcond_solver_dims *dims); + +/// Destructor of the options struct. +/// +/// \param opts The options struct to destroy. +void ocp_qp_xcond_solver_opts_free(ocp_qp_xcond_solver_opts *opts); + + +/// Setter of the options struct. +/// +/// \param opts The options struct. +void ocp_qp_xcond_solver_opts_set(ocp_qp_xcond_solver_config *config, + ocp_qp_xcond_solver_opts *opts, const char *field, void* value); + +/// TBC Should be private/static? +acados_size_t ocp_qp_calculate_size(ocp_qp_xcond_solver_config *config, ocp_qp_xcond_solver_dims *dims, void *opts_); + + +/// TBC Reserves memory? TBC Should this be private? +/// +/// \param config The configuration struct. +/// \param dims The dimension struct. +/// \param opts_ The options struct. +/// \param raw_memory Pointer to raw memory to assign to qp solver. +ocp_qp_solver *ocp_qp_assign(ocp_qp_xcond_solver_config *config, ocp_qp_xcond_solver_dims *dims, + void *opts_, void *raw_memory); + +/// Creates a qp solver. Reserves memory. +/// +/// \param config The configuration struct. +/// \param dims The dimension struct. +/// \param opts_ The options struct. +ocp_qp_solver *ocp_qp_create(ocp_qp_xcond_solver_config *config, + ocp_qp_xcond_solver_dims *dims, void *opts_); + + +/// Destroys a qp solver. Frees memory. +/// +/// \param solver The qp solver +void ocp_qp_solver_destroy(ocp_qp_solver *solver); + +void ocp_qp_x_cond_solver_free(ocp_qp_xcond_solver_config *config, + ocp_qp_xcond_solver_dims *dims, void *opts_); + + +/// Solves the qp. +/// +/// \param solver The solver. +/// \param qp_in The inputs struct. +/// \param qp_out The output struct. +int ocp_qp_solve(ocp_qp_solver *solver, ocp_qp_in *qp_in, ocp_qp_out *qp_out); + + +/// Calculates the infinity norm of the residuals. +/// +/// \param dims The dimension struct. +/// \param qp_in The inputs struct. +/// \param qp_out The output struct. +/// \param res Output array for the residuals. +void ocp_qp_inf_norm_residuals(ocp_qp_dims *dims, ocp_qp_in *qp_in, ocp_qp_out *qp_out, + double *res); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // INTERFACES_ACADOS_C_OCP_QP_INTERFACE_H_ diff --git a/phonelibs/acados/include/acados_c/sim_interface.h b/phonelibs/acados/include/acados_c/sim_interface.h new file mode 100644 index 0000000000..ba2e67bd6a --- /dev/null +++ b/phonelibs/acados/include/acados_c/sim_interface.h @@ -0,0 +1,140 @@ +/* + * 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 INTERFACES_ACADOS_C_SIM_INTERFACE_H_ +#define INTERFACES_ACADOS_C_SIM_INTERFACE_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "acados/sim/sim_common.h" + + + +typedef enum +{ + ERK, + IRK, + GNSF, + LIFTED_IRK, + INVALID_SIM_SOLVER, +} sim_solver_t; + + + +typedef struct +{ + sim_solver_t sim_solver; +} sim_solver_plan; + + + +typedef struct +{ + sim_config *config; + void *dims; + void *opts; + void *mem; + void *work; +} sim_solver; + + + +/* config */ +// +sim_config *sim_config_create(sim_solver_plan plan); +// +void sim_config_destroy(void *config); + +/* dims */ +// +void *sim_dims_create(void *config_); +// +void sim_dims_destroy(void *dims); +// +void sim_dims_set(sim_config *config, void *dims, const char *field, const int* value); +// +void sim_dims_get(sim_config *config, void *dims, const char *field, int* value); +// +void sim_dims_get_from_attr(sim_config *config, void *dims, const char *field, int *dims_out); + +/* in */ +// +sim_in *sim_in_create(sim_config *config, void *dims); +// +void sim_in_destroy(void *out); +// +int sim_in_set(void *config_, void *dims_, sim_in *in, const char *field, void *value); + + +/* out */ +// +sim_out *sim_out_create(sim_config *config, void *dims); +// +void sim_out_destroy(void *out); +// +int sim_out_get(void *config, void *dims, sim_out *out, const char *field, void *value); + +/* opts */ +// +void *sim_opts_create(sim_config *config, void *dims); +// +void sim_opts_destroy(void *opts); +// +void sim_opts_set(sim_config *config, void *opts, const char *field, void *value); +// +void sim_opts_get(sim_config *config, void *opts, const char *field, void *value); + +/* solver */ +// +acados_size_t sim_calculate_size(sim_config *config, void *dims, void *opts_); +// +sim_solver *sim_assign(sim_config *config, void *dims, void *opts_, void *raw_memory); +// +sim_solver *sim_solver_create(sim_config *config, void *dims, void *opts_); +// +void sim_solver_destroy(void *solver); +// +int sim_solve(sim_solver *solver, sim_in *in, sim_out *out); +// +int sim_precompute(sim_solver *solver, sim_in *in, sim_out *out); +// +int sim_solver_set(sim_solver *solver, const char *field, void *value); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // INTERFACES_ACADOS_C_SIM_INTERFACE_H_ diff --git a/phonelibs/acados/include/blasfeo/include/blasfeo.h b/phonelibs/acados/include/blasfeo/include/blasfeo.h new file mode 100644 index 0000000000..c854918193 --- /dev/null +++ b/phonelibs/acados/include/blasfeo/include/blasfeo.h @@ -0,0 +1,52 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#include "blasfeo_processor_features.h" +#include "blasfeo_target.h" +#include "blasfeo_block_size.h" +#include "blasfeo_stdlib.h" +#include "blasfeo_common.h" +#include "blasfeo_d_aux.h" +#include "blasfeo_d_aux_ext_dep.h" +#include "blasfeo_d_kernel.h" +#include "blasfeo_d_blas.h" +#include "blasfeo_s_aux.h" +#include "blasfeo_s_aux_ext_dep.h" +#include "blasfeo_s_kernel.h" +#include "blasfeo_s_blas.h" +#include "blasfeo_i_aux_ext_dep.h" +#include "blasfeo_v_aux_ext_dep.h" +#include "blasfeo_timing.h" +#include "blasfeo_memory.h" diff --git a/phonelibs/acados/include/blasfeo/include/blasfeo_block_size.h b/phonelibs/acados/include/blasfeo/include/blasfeo_block_size.h new file mode 100644 index 0000000000..182cc71b14 --- /dev/null +++ b/phonelibs/acados/include/blasfeo/include/blasfeo_block_size.h @@ -0,0 +1,371 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_BLOCK_SIZE_H_ +#define BLASFEO_BLOCK_SIZE_H_ + + + +#define D_EL_SIZE 8 // double precision +#define S_EL_SIZE 4 // single precision + + + +#if defined( TARGET_X64_INTEL_HASWELL ) +// common +#define CACHE_LINE_SIZE 64 // data cache size: 64 bytes +#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 kB, 8-way +#define L2_CACHE_SIZE (256*1024) // L2 data cache size: 256 kB ; DTLB1 64*4 kB = 256 kB +#define LLC_CACHE_SIZE (6*1024*1024) // LLC cache size: 6 MB ; TLB 1024*4 kB = 4 MB +// double +#define D_PS 4 // panel size +#define D_PLD 4 // 2 // GCD of panel length +#define D_M_KERNEL 12 // max kernel size +#define D_KC 256 // 192 +#define D_NC 72 //96 //72 // 120 // 512 +#define D_MC 1500 // 6000 +// single +#define S_PS 8 // panel size +#define S_PLD 4 // 2 // GCD of panel length +#define S_M_KERNEL 24 // max kernel size +#define S_KC 256 +#define S_NC 144 +#define S_MC 3000 + +#elif defined( TARGET_X64_INTEL_SANDY_BRIDGE ) +// common +#define CACHE_LINE_SIZE 64 // data cache size: 64 bytes +#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 kB, 8-way +#define L2_CACHE_SIZE (256*1024) // L2 data cache size: 256 kB ; DTLB1 64*4 kB = 256 kB +#define LLC_CACHE_SIZE (4*1024*1024) // LLC cache size: 4 MB ; TLB 1024*4 kB = 4 MB +// double +#define D_PS 4 // panel size +#define D_PLD 4 // 2 // GCD of panel length +#define D_M_KERNEL 8 // max kernel size +#define D_KC 256 //320 //256 //320 +#define D_NC 72 //64 //72 //60 // 120 +#define D_MC 1000 // 800 +// single +#define S_PS 8 // panel size +#define S_PLD 4 // 2 // GCD of panel length +#define S_M_KERNEL 16 // max kernel size +#define S_KC 256 +#define S_NC 144 +#define S_MC 2000 + +#elif defined( TARGET_X64_INTEL_CORE ) +// common +#define CACHE_LINE_SIZE 64 +#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 kB +// double +#define D_PS 4 // panel size +#define D_PLD 4 // 2 // GCD of panel length +#define D_M_KERNEL 4 // max kernel size +#define D_KC 256 +#define D_NC 128 // TODO these are just dummy +#define D_MC 3000 // TODO these are just dummy +// single +#define S_PS 4 +#define S_PLD 4 //2 +#define S_M_KERNEL 4 // max kernel size +#define S_KC 256 +#define S_NC 128 // TODO these are just dummy +#define S_MC 3000 // TODO these are just dummy + +#elif defined( TARGET_X64_AMD_BULLDOZER ) +// common +#define CACHE_LINE_SIZE 64 +#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 kB +// double +#define D_PS 4 // panel size +#define D_PLD 4 // 2 // GCD of panel length +#define D_M_KERNEL 4 // max kernel size +#define D_KC 256 +#define D_NC 128 // TODO these are just dummy +#define D_MC 3000 // TODO these are just dummy +// single +#define S_PS 4 +#define S_PLD 4 //2 +#define S_M_KERNEL 4 // max kernel size +#define S_KC 256 +#define S_NC 128 // TODO these are just dummy +#define S_MC 3000 // TODO these are just dummy + + +#elif defined( TARGET_X86_AMD_JAGUAR ) +// common +#define CACHE_LINE_SIZE 64 +#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 kB +// double +#define D_PS 4 // panel size +#define D_PLD 4 // 2 // GCD of panel length +#define D_M_KERNEL 4 // max kernel size +#define D_KC 256 +#define D_NC 128 // TODO these are just dummy +#define D_MC 3000 // TODO these are just dummy +// single +#define S_PS 4 +#define S_PLD 4 //2 +#define S_M_KERNEL 4 // max kernel size +#define S_KC 256 +#define S_NC 128 // TODO these are just dummy +#define S_MC 3000 // TODO these are just dummy + + +#elif defined( TARGET_X86_AMD_BARCELONA ) +// common +#define CACHE_LINE_SIZE 64 +#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 kB +// double +#define D_PS 4 // panel size +#define D_PLD 4 // 2 // GCD of panel length +#define D_M_KERNEL 4 // max kernel size +#define D_KC 256 +#define D_NC 128 // TODO these are just dummy +#define D_MC 3000 // TODO these are just dummy +// single +#define S_PS 4 +#define S_PLD 4 //2 +#define S_M_KERNEL 4 // max kernel size +#define S_KC 256 +#define S_NC 128 // TODO these are just dummy +#define S_MC 3000 // TODO these are just dummy + + +#elif defined(TARGET_ARMV8A_ARM_CORTEX_A76) +// common +#define CACHE_LINE_SIZE 64 +#define L1_CACHE_SIZE (64*1024) // L1 data cache size: 64 kB, 4-way ; DTLB1 48*4 kB = 192 kB +#define LLC_CACHE_SIZE (1*1024*1024) // LLC cache size: 1 MB +// double +#define D_PS 4 // panel size +#define D_PLD 4 // 2 // GCD of panel length +#define D_M_KERNEL 8 // max kernel size +#define D_KC 512 //256 +#define D_NC 128 //256 +#define D_MC 6000 +// single +#define S_PS 4 +#define S_PLD 4 //2 +#define S_M_KERNEL 8 // max kernel size +#define S_KC 512 +#define S_NC 256 +#define S_MC 6000 + + +#elif defined(TARGET_ARMV8A_ARM_CORTEX_A73) +// common +#define CACHE_LINE_SIZE 64 +#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 (64?) kB, 4-way, seen as 8-(16-)way ; DTLB1 48*4 kB = 192 kB +#define LLC_CACHE_SIZE (1*1024*1024) // LLC cache size: 1 MB +// double +#define D_PS 4 // panel size +#define D_PLD 4 // 2 // GCD of panel length +#define D_M_KERNEL 8 // max kernel size +#define D_KC 320 +#define D_NC 256 +#define D_MC 6000 +// single +#define S_PS 4 +#define S_PLD 4 //2 +#define S_M_KERNEL 8 // max kernel size +#define S_KC 256 +#define S_NC 128 // TODO these are just dummy +#define S_MC 3000 // TODO these are just dummy + + +#elif defined(TARGET_ARMV8A_ARM_CORTEX_A57) +// common +#define CACHE_LINE_SIZE 64 +#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 kB, 2-way ; DTLB1 32*4 kB = 128 kB +#define LLC_CACHE_SIZE (1*1024*1024) // LLC cache size: 1 MB // 2 MB ??? +// double +#define D_PS 4 // panel size +#define D_PLD 4 // 2 // GCD of panel length +#define D_M_KERNEL 8 // max kernel size +#define D_KC 224 //256 //192 +#define D_NC 40 //36 //48 +#define D_MC 512 //488 //600 +// single +#define S_PS 4 +#define S_PLD 4 //2 +#define S_M_KERNEL 8 // max kernel size +#define S_KC 256 +#define S_NC 128 // TODO these are just dummy +#define S_MC 3000 // TODO these are just dummy + + +#elif defined(TARGET_ARMV8A_ARM_CORTEX_A55) +// common +#define CACHE_LINE_SIZE 64 +#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 kB, 4-way ; DTLB1 16*4 kB = 64 kB +#define LLC_CACHE_SIZE (512*1024) // LLC cache size: 512 kB +// double +#define D_PS 4 // panel size +#define D_PLD 4 // 2 // GCD of panel length +#define D_M_KERNEL 12 // max kernel size +#define D_KC 224 +#define D_NC 160 +#define D_MC 6000 +// single +#define S_PS 4 +#define S_PLD 4 //2 +#define S_M_KERNEL 8 // max kernel size +#define S_KC 256 +#define S_NC 128 // TODO these are just dummy +#define S_MC 3000 // TODO these are just dummy + + +#elif defined(TARGET_ARMV8A_ARM_CORTEX_A53) +// common +#define CACHE_LINE_SIZE 64 +#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 kB, 4-way ??? ; DTLB1 10*4 kB = 40 kB +#define LLC_CACHE_SIZE (256*1024) // LLC cache size: 256 kB +// double +#define D_PS 4 // panel size +#define D_PLD 4 // 2 // GCD of panel length +#define D_M_KERNEL 12 // max kernel size +#define D_KC 160 +#define D_NC 128 +#define D_MC 6000 +// single +#define S_PS 4 +#define S_PLD 4 //2 +#define S_M_KERNEL 8 // max kernel size +#define S_KC 256 +#define S_NC 128 // TODO these are just dummy +#define S_MC 3000 // TODO these are just dummy + + +#elif defined( TARGET_ARMV7A_ARM_CORTEX_A15 ) +// common +#define CACHE_LINE_SIZE 64 +#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 kB +// double +#define D_PS 4 // panel size +#define D_PLD 4 // 2 // GCD of panel length +#define D_M_KERNEL 4 // max kernel size +#define D_KC 256 +#define D_NC 128 // TODO these are just dummy +#define D_MC 3000 // TODO these are just dummy +// single +#define S_PS 4 +#define S_PLD 4 //2 +#define S_M_KERNEL 4 // max kernel size +#define S_KC 256 +#define S_NC 128 // TODO these are just dummy +#define S_MC 3000 // TODO these are just dummy + + +#elif defined( TARGET_ARMV7A_ARM_CORTEX_A7 ) +// common +#define CACHE_LINE_SIZE 64 +#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 kB +// double +#define D_PS 4 // panel size +#define D_PLD 4 // 2 // GCD of panel length +#define D_M_KERNEL 4 // max kernel size +#define D_KC 256 +#define D_NC 128 // TODO these are just dummy +#define D_MC 3000 // TODO these are just dummy +// single +#define S_PS 4 +#define S_PLD 4 //2 +#define S_M_KERNEL 4 // max kernel size +#define S_KC 256 +#define S_NC 128 // TODO these are just dummy +#define S_MC 3000 // TODO these are just dummy + + +#elif defined( TARGET_ARMV7A_ARM_CORTEX_A9 ) +// common +#define CACHE_LINE_SIZE 32 +#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 kB +// double +#define D_PS 4 // panel size +#define D_PLD 4 // 2 // GCD of panel length +#define D_M_KERNEL 4 // max kernel size +#define D_KC 256 +#define D_NC 128 // TODO these are just dummy +#define D_MC 3000 // TODO these are just dummy +// single +#define S_PS 4 +#define S_PLD 4 //2 +#define S_M_KERNEL 4 // max kernel size +#define S_KC 256 +#define S_NC 128 // TODO these are just dummy +#define S_MC 3000 // TODO these are just dummy + + +#elif defined( TARGET_GENERIC ) +// common +#define CACHE_LINE_SIZE 64 +#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 kB +// double +#define D_PS 4 // panel size +#define D_PLD 4 // 2 // GCD of panel length +#define D_M_KERNEL 4 // max kernel size +#define D_KC 256 +#define D_NC 128 // TODO these are just dummy +#define D_MC 3000 // TODO these are just dummy + +// single +#define S_PS 4 +#define S_PLD 4 //2 +#define S_M_KERNEL 4 // max kernel size +#define S_KC 256 +#define S_NC 128 // TODO these are just dummy +#define S_MC 3000 // TODO these are just dummy + + +#else +#error "Unknown architecture" +#endif + + + +#define D_CACHE_LINE_EL (CACHE_LINE_SIZE/D_EL_SIZE) +#define D_L1_CACHE_EL (L1_CACHE_SIZE/D_EL_SIZE) +#define D_L2_CACHE_EL (L2_CACHE_SIZE/D_EL_SIZE) +#define D_LLC_CACHE_EL (LLC_CACHE_SIZE/D_EL_SIZE) + +#define S_CACHE_LINE_EL (CACHE_LINE_SIZE/S_EL_SIZE) +#define S_L1_CACHE_EL (L1_CACHE_SIZE/S_EL_SIZE) +#define S_L2_CACHE_EL (L2_CACHE_SIZE/S_EL_SIZE) +#define S_LLC_CACHE_EL (LLC_CACHE_SIZE/S_EL_SIZE) + + + +#endif // BLASFEO_BLOCK_SIZE_H_ diff --git a/phonelibs/acados/include/blasfeo/include/blasfeo_common.h b/phonelibs/acados/include/blasfeo/include/blasfeo_common.h new file mode 100644 index 0000000000..a648142ba0 --- /dev/null +++ b/phonelibs/acados/include/blasfeo/include/blasfeo_common.h @@ -0,0 +1,236 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_COMMON_H_ +#define BLASFEO_COMMON_H_ + + + +#include "blasfeo_target.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +#if defined(__GNUC__) || defined(__clang__) || defined(__INTEL_COMPILER) || defined(__ICL) || defined(__ICC) || defined(__INTEL_LLVM_COMPILER) +#define ALIGNED(VEC, BYTES) VEC __attribute__ ((aligned ( BYTES ))) +#elif defined (_MSC_VER) +#define ALIGNED(VEC, BYTES) __declspec(align( BYTES )) VEC +#else +#define ALIGNED(VEC, BYTES) VEC +#endif + + + + +#if ( defined(LA_HIGH_PERFORMANCE) & defined(MF_PANELMAJ) ) | ( defined(LA_REFERENCE) & defined(MF_PANELMAJ) ) + +#include "blasfeo_block_size.h" + +// matrix structure +struct blasfeo_dmat + { + double *mem; // pointer to passed chunk of memory + double *pA; // pointer to a pm*pn array of doubles, the first is aligned to cache line size + double *dA; // pointer to a min(m,n) (or max???) array of doubles + int m; // rows + int n; // cols + int pm; // packed number or rows + int cn; // packed number or cols + int use_dA; // flag to tell if dA can be used + int memsize; // size of needed memory + }; + +struct blasfeo_smat + { + float *mem; // pointer to passed chunk of memory + float *pA; // pointer to a pm*pn array of floats, the first is aligned to cache line size + float *dA; // pointer to a min(m,n) (or max???) array of floats + int m; // rows + int n; // cols + int pm; // packed number or rows + int cn; // packed number or cols + int use_dA; // flag to tell if dA can be used + int memsize; // size of needed memory + }; + +// vector structure +struct blasfeo_dvec + { + double *mem; // pointer to passed chunk of memory + double *pa; // pointer to a pm array of doubles, the first is aligned to cache line size + int m; // size + int pm; // packed size + int memsize; // size of needed memory + }; + +struct blasfeo_svec + { + float *mem; // pointer to passed chunk of memory + float *pa; // pointer to a pm array of floats, the first is aligned to cache line size + int m; // size + int pm; // packed size + int memsize; // size of needed memory + }; + +#define BLASFEO_DMATEL(sA,ai,aj) ((sA)->pA[((ai)-((ai)&(D_PS-1)))*(sA)->cn+(aj)*D_PS+((ai)&(D_PS-1))]) +#define BLASFEO_SMATEL(sA,ai,aj) ((sA)->pA[((ai)-((ai)&(S_PS-1)))*(sA)->cn+(aj)*S_PS+((ai)&(S_PS-1))]) +#define BLASFEO_DVECEL(sa,ai) ((sa)->pa[ai]) +#define BLASFEO_SVECEL(sa,ai) ((sa)->pa[ai]) + +#elif ( defined(LA_HIGH_PERFORMANCE) & defined(MF_COLMAJ) ) | ( defined(LA_REFERENCE) & defined(MF_COLMAJ) ) | defined(LA_EXTERNAL_BLAS_WRAPPER) + +// matrix structure +struct blasfeo_dmat + { + double *mem; // pointer to passed chunk of memory + double *pA; // pointer to a m*n array of doubles + double *dA; // pointer to a min(m,n) (or max???) array of doubles + int m; // rows + int n; // cols + int use_dA; // flag to tell if dA can be used + int memsize; // size of needed memory + }; + +struct blasfeo_smat + { + float *mem; // pointer to passed chunk of memory + float *pA; // pointer to a m*n array of floats + float *dA; // pointer to a min(m,n) (or max???) array of floats + int m; // rows + int n; // cols + int use_dA; // flag to tell if dA can be used + int memsize; // size of needed memory + }; + +// vector structure +struct blasfeo_dvec + { + double *mem; // pointer to passed chunk of memory + double *pa; // pointer to a m array of doubles, the first is aligned to cache line size + int m; // size + int memsize; // size of needed memory + }; + +struct blasfeo_svec + { + float *mem; // pointer to passed chunk of memory + float *pa; // pointer to a m array of floats, the first is aligned to cache line size + int m; // size + int memsize; // size of needed memory + }; + +#define BLASFEO_DMATEL(sA,ai,aj) ((sA)->pA[(ai)+(aj)*(sA)->m]) +#define BLASFEO_SMATEL(sA,ai,aj) ((sA)->pA[(ai)+(aj)*(sA)->m]) +#define BLASFEO_DVECEL(sa,ai) ((sa)->pa[ai]) +#define BLASFEO_SVECEL(sa,ai) ((sa)->pa[ai]) + +#else + +#error : wrong LA or MF choice + +#endif + + + +// Explicitly panel-major matrix structure +struct blasfeo_pm_dmat + { + double *mem; // pointer to passed chunk of memory + double *pA; // pointer to a pm*pn array of doubles, the first is aligned to cache line size + double *dA; // pointer to a min(m,n) (or max???) array of doubles + int m; // rows + int n; // cols + int pm; // packed number or rows + int cn; // packed number or cols + int use_dA; // flag to tell if dA can be used + int ps; // panel size + int memsize; // size of needed memory + }; + +struct blasfeo_pm_smat + { + float *mem; // pointer to passed chunk of memory + float *pA; // pointer to a pm*pn array of floats, the first is aligned to cache line size + float *dA; // pointer to a min(m,n) (or max???) array of floats + int m; // rows + int n; // cols + int pm; // packed number or rows + int cn; // packed number or cols + int use_dA; // flag to tell if dA can be used + int ps; // panel size + int memsize; // size of needed memory + }; + +// Explicitly column-major matrix structure +struct blasfeo_cm_dmat + { + double *mem; // pointer to passed chunk of memory + double *pA; // pointer to a m*n array of doubles + double *dA; // pointer to a min(m,n) (or max???) array of doubles + int m; // rows + int n; // cols + int use_dA; // flag to tell if dA can be used + int memsize; // size of needed memory + }; + +struct blasfeo_cm_smat + { + float *mem; // pointer to passed chunk of memory + float *pA; // pointer to a m*n array of floats + float *dA; // pointer to a min(m,n) (or max???) array of floats + int m; // rows + int n; // cols + int use_dA; // flag to tell if dA can be used + int memsize; // size of needed memory + }; + + +#define BLASFEO_PM_DMATEL(sA,ai,aj) ((sA)->pA[((ai)-((ai)&((sA)->ps-1)))*(sA)->cn+(aj)*((sA)->ps)+((ai)&((sA)->ps-1))]) +#define BLASFEO_PM_SMATEL(sA,ai,aj) ((sA)->pA[((ai)-((ai)&((sA)->ps-1)))*(sA)->cn+(aj)*((sA)->ps)+((ai)&((sA)->ps-1))]) +#define BLASFEO_CM_DMATEL(sA,ai,aj) ((sA)->pA[(ai)+(aj)*(sA)->m]) +#define BLASFEO_CM_SMATEL(sA,ai,aj) ((sA)->pA[(ai)+(aj)*(sA)->m]) + + + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_COMMON_H_ diff --git a/phonelibs/acados/include/blasfeo/include/blasfeo_d_aux.h b/phonelibs/acados/include/blasfeo/include/blasfeo_d_aux.h new file mode 100644 index 0000000000..7a9415a59d --- /dev/null +++ b/phonelibs/acados/include/blasfeo/include/blasfeo_d_aux.h @@ -0,0 +1,239 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +/* + * auxiliary algebra operations header + * + * include/blasfeo_aux_lib*.h + * + */ + +#ifndef BLASFEO_D_AUX_H_ +#define BLASFEO_D_AUX_H_ + + + +#include + +#include "blasfeo_common.h" +#include "blasfeo_d_aux_old.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + +// --- memory size calculations +// +// returns the memory size (in bytes) needed for a dmat +size_t blasfeo_memsize_dmat(int m, int n); +// returns the memory size (in bytes) needed for the diagonal of a dmat +size_t blasfeo_memsize_diag_dmat(int m, int n); +// returns the memory size (in bytes) needed for a dvec +size_t blasfeo_memsize_dvec(int m); + +// --- creation +// +// create a strmat for a matrix of size m*n by using memory passed by a pointer (pointer is not updated) +void blasfeo_create_dmat(int m, int n, struct blasfeo_dmat *sA, void *memory); +// create a strvec for a vector of size m by using memory passed by a pointer (pointer is not updated) +void blasfeo_create_dvec(int m, struct blasfeo_dvec *sA, void *memory); + +// --- packing +// pack the column-major matrix A into the matrix struct B +void blasfeo_pack_dmat(int m, int n, double *A, int lda, struct blasfeo_dmat *sB, int bi, int bj); +// pack the lower-triangular column-major matrix A into the matrix struct B +void blasfeo_pack_l_dmat(int m, int n, double *A, int lda, struct blasfeo_dmat *sB, int bi, int bj); +// pack the upper-triangular column-major matrix A into the matrix struct B +void blasfeo_pack_u_dmat(int m, int n, double *A, int lda, struct blasfeo_dmat *sB, int bi, int bj); +// transpose and pack the column-major matrix A into the matrix struct B +void blasfeo_pack_tran_dmat(int m, int n, double *A, int lda, struct blasfeo_dmat *sB, int bi, int bj); +// pack the vector x into the vector structure y +void blasfeo_pack_dvec(int m, double *x, int xi, struct blasfeo_dvec *sy, int yi); +// unpack the matrix structure A into the column-major matrix B +void blasfeo_unpack_dmat(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, double *B, int ldb); +// transpose and unpack the matrix structure A into the column-major matrix B +void blasfeo_unpack_tran_dmat(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, double *B, int ldb); +// pack the vector structure x into the vector y +void blasfeo_unpack_dvec(int m, struct blasfeo_dvec *sx, int xi, double *y, int yi); + +// --- cast +// +//void d_cast_mat2strmat(double *A, struct blasfeo_dmat *sA); // TODO +//void d_cast_diag_mat2strmat(double *dA, struct blasfeo_dmat *sA); // TODO +//void d_cast_vec2vecmat(double *a, struct blasfeo_dvec *sx); // TODO + + +// ge +// --- insert/extract +// +// sA[ai, aj] <= a +void blasfeo_dgein1(double a, struct blasfeo_dmat *sA, int ai, int aj); +// <= sA[ai, aj] +double blasfeo_dgeex1(struct blasfeo_dmat *sA, int ai, int aj); + +// --- set +// A <= alpha +void blasfeo_dgese(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj); + +// --- copy / scale +// B <= A +void blasfeo_dgecp(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj); +// A <= alpha*A +void blasfeo_dgesc(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj); +// B <= alpha*A +void blasfeo_dgecpsc(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj); +// B <= A, A lower triangular +void blasfeo_dtrcp_l(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj); +void blasfeo_dtrcpsc_l(int m, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj); +void blasfeo_dtrsc_l(int m, double alpha, struct blasfeo_dmat *sA, int ai, int aj); + +// --- sum +// B <= B + alpha*A +void blasfeo_dgead(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj); +// y <= y + alpha*x +void blasfeo_dvecad(int m, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi); + +// --- traspositions +// B <= A' +void blasfeo_dgetr(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj); +// B <= A', A lower triangular +void blasfeo_dtrtr_l(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj); +// B <= A', A upper triangular +void blasfeo_dtrtr_u(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj); + +// dia +// diag(A) += alpha +void blasfeo_ddiare(int kmax, double alpha, struct blasfeo_dmat *sA, int ai, int aj); +// diag(A) <= alpha*x +void blasfeo_ddiain(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj); +// diag(A)[idx] <= alpha*x +void blasfeo_ddiain_sp(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dmat *sD, int di, int dj); +// x <= diag(A) +void blasfeo_ddiaex(int kmax, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi); +// x <= diag(A)[idx] +void blasfeo_ddiaex_sp(int kmax, double alpha, int *idx, struct blasfeo_dmat *sD, int di, int dj, struct blasfeo_dvec *sx, int xi); +// diag(A) += alpha*x +void blasfeo_ddiaad(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj); +// diag(A)[idx] += alpha*x +void blasfeo_ddiaad_sp(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dmat *sD, int di, int dj); +// diag(A)[idx] = y + alpha*x +void blasfeo_ddiaadin_sp(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi, int *idx, struct blasfeo_dmat *sD, int di, int dj); + +// row +void blasfeo_drowin(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj); +void blasfeo_drowex(int kmax, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi); +void blasfeo_drowad(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj); +void blasfeo_drowad_sp(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dmat *sD, int di, int dj); +void blasfeo_drowsw(int kmax, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sC, int ci, int cj); +void blasfeo_drowpe(int kmax, int *ipiv, struct blasfeo_dmat *sA); +void blasfeo_drowpei(int kmax, int *ipiv, struct blasfeo_dmat *sA); + +// col +void blasfeo_dcolex(int kmax, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi); +void blasfeo_dcolin(int kmax, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj); +void blasfeo_dcolad(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj); +void blasfeo_dcolsc(int kmax, double alpha, struct blasfeo_dmat *sA, int ai, int aj); +void blasfeo_dcolsw(int kmax, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sC, int ci, int cj); +void blasfeo_dcolpe(int kmax, int *ipiv, struct blasfeo_dmat *sA); +void blasfeo_dcolpei(int kmax, int *ipiv, struct blasfeo_dmat *sA); + +// vec +// a <= alpha +void blasfeo_dvecse(int m, double alpha, struct blasfeo_dvec *sx, int xi); +// sx[xi] <= a +void blasfeo_dvecin1(double a, struct blasfeo_dvec *sx, int xi); +// <= sx[xi] +double blasfeo_dvecex1(struct blasfeo_dvec *sx, int xi); +// y <= x +void blasfeo_dveccp(int m, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi); +// x <= alpha*x +void blasfeo_dvecsc(int m, double alpha, struct blasfeo_dvec *sx, int xi); +// y <= alpha*x +void blasfeo_dveccpsc(int m, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi); +void blasfeo_dvecad_sp(int m, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dvec *sz, int zi); +void blasfeo_dvecin_sp(int m, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dvec *sz, int zi); +void blasfeo_dvecex_sp(int m, double alpha, int *idx, struct blasfeo_dvec *sx, int x, struct blasfeo_dvec *sz, int zi); + +void blasfeo_dveccl(int m, + struct blasfeo_dvec *sxm, int xim, struct blasfeo_dvec *sx, int xi, + struct blasfeo_dvec *sxp, int xip, struct blasfeo_dvec *sz, int zi); + +void blasfeo_dveccl_mask(int m, + struct blasfeo_dvec *sxm, int xim, struct blasfeo_dvec *sx, int xi, + struct blasfeo_dvec *sxp, int xip, struct blasfeo_dvec *sz, int zi, + struct blasfeo_dvec *sm, int mi); + +void blasfeo_dvecze(int m, struct blasfeo_dvec *sm, int mi, struct blasfeo_dvec *sv, int vi, struct blasfeo_dvec *se, int ei); +void blasfeo_dvecnrm_inf(int m, struct blasfeo_dvec *sx, int xi, double *ptr_norm); +void blasfeo_dvecnrm_2(int m, struct blasfeo_dvec *sx, int xi, double *ptr_norm); +void blasfeo_dvecpe(int kmax, int *ipiv, struct blasfeo_dvec *sx, int xi); +void blasfeo_dvecpei(int kmax, int *ipiv, struct blasfeo_dvec *sx, int xi); + + + + + +/* +* Explicitly panel-major matrix format +*/ + +// returns the memory size (in bytes) needed for a dmat +size_t blasfeo_pm_memsize_dmat(int ps, int m, int n); +// create a strmat for a matrix of size m*n by using memory passed by a pointer (pointer is not updated) +void blasfeo_pm_create_dmat(int ps, int m, int n, struct blasfeo_pm_dmat *sA, void *memory); +// print +void blasfeo_pm_print_dmat(int m, int n, struct blasfeo_pm_dmat *sA, int ai, int aj); + + + +/* +* Explicitly panel-major matrix format +*/ + +// returns the memory size (in bytes) needed for a dmat +size_t blasfeo_cm_memsize_dmat(int m, int n); +// create a strmat for a matrix of size m*n by using memory passed by a pointer (pointer is not updated) +void blasfeo_cm_create_dmat(int m, int n, struct blasfeo_pm_dmat *sA, void *memory); + + + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_D_AUX_H_ diff --git a/phonelibs/acados/include/blasfeo/include/blasfeo_d_aux_ext_dep.h b/phonelibs/acados/include/blasfeo/include/blasfeo_d_aux_ext_dep.h new file mode 100644 index 0000000000..bee9e986f1 --- /dev/null +++ b/phonelibs/acados/include/blasfeo/include/blasfeo_d_aux_ext_dep.h @@ -0,0 +1,145 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +/* + * auxiliary algebra operation external dependancies header + * + * include/blasfeo_d_aux_ext_dep.h + * + * - dynamic memory allocation + * - print + * + */ + +#ifndef BLASFEO_D_AUX_EXT_DEP_H_ +#define BLASFEO_D_AUX_EXT_DEP_H_ + + + +#include + + + +#include "blasfeo_common.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +#ifdef EXT_DEP + +/* column-major matrices */ + +// dynamically allocate row*col doubles of memory and set accordingly a pointer to double; set allocated memory to zero +void d_zeros(double **pA, int row, int col); +// dynamically allocate row*col doubles of memory aligned to 64-byte boundaries and set accordingly a pointer to double; set allocated memory to zero +void d_zeros_align(double **pA, int row, int col); +// dynamically allocate size bytes of memory aligned to 64-byte boundaries and set accordingly a pointer to double; set allocated memory to zero +void d_zeros_align_bytes(double **pA, int size); +// free the memory allocated by d_zeros +void d_free(double *pA); +// free the memory allocated by d_zeros_align or d_zeros_align_bytes +void d_free_align(double *pA); +// print a column-major matrix +void d_print_mat(int m, int n, double *A, int lda); +// print the transposed of a column-major matrix +void d_print_tran_mat(int row, int col, double *A, int lda); +// print to file a column-major matrix +void d_print_to_file_mat(FILE *file, int row, int col, double *A, int lda); +// print to file a column-major matrix in exponential format +void d_print_to_file_exp_mat(FILE *file, int row, int col, double *A, int lda); +// print to string a column-major matrix +void d_print_to_string_mat(char **buf_out, int row, int col, double *A, int lda); +// print to file the transposed of a column-major matrix +void d_print_tran_to_file_mat(FILE *file, int row, int col, double *A, int lda); +// print to file the transposed of a column-major matrix in exponential format +void d_print_tran_to_file_exp_mat(FILE *file, int row, int col, double *A, int lda); +// print in exponential notation a column-major matrix +void d_print_exp_mat(int m, int n, double *A, int lda); +// print in exponential notation the transposed of a column-major matrix +void d_print_exp_tran_mat(int row, int col, double *A, int lda); + +/* strmat and strvec */ + +// create a strmat for a matrix of size m*n by dynamically allocating memory +void blasfeo_allocate_dmat(int m, int n, struct blasfeo_dmat *sA); +// create a strvec for a vector of size m by dynamically allocating memory +void blasfeo_allocate_dvec(int m, struct blasfeo_dvec *sa); +// free the memory allocated by blasfeo_allocate_dmat +void blasfeo_free_dmat(struct blasfeo_dmat *sA); +// free the memory allocated by blasfeo_allocate_dvec +void blasfeo_free_dvec(struct blasfeo_dvec *sa); +// print a strmat +void blasfeo_print_dmat(int m, int n, struct blasfeo_dmat *sA, int ai, int aj); +// print in exponential notation a strmat +void blasfeo_print_exp_dmat(int m, int n, struct blasfeo_dmat *sA, int ai, int aj); +// print to file a strmat +void blasfeo_print_to_file_dmat(FILE *file, int m, int n, struct blasfeo_dmat *sA, int ai, int aj); +// print to file a strmat in exponential format +void blasfeo_print_to_file_exp_dmat(FILE *file, int m, int n, struct blasfeo_dmat *sA, int ai, int aj); +// print to string a strmat +void blasfeo_print_to_string_dmat(char **buf_out, int m, int n, struct blasfeo_dmat *sA, int ai, int aj); +// print the transposed of a strmat +void blasfeo_print_tran_dmat(int m, int n, struct blasfeo_dmat *sA, int ai, int aj); +// print a strvec +void blasfeo_print_dvec(int m, struct blasfeo_dvec *sa, int ai); +// print in exponential notation a strvec +void blasfeo_print_exp_dvec(int m, struct blasfeo_dvec *sa, int ai); +// print to file a strvec +void blasfeo_print_to_file_dvec(FILE *file, int m, struct blasfeo_dvec *sa, int ai); +// print to string a strvec +void blasfeo_print_to_string_dvec(char **buf_out, int m, struct blasfeo_dvec *sa, int ai); +// print the transposed of a strvec +void blasfeo_print_tran_dvec(int m, struct blasfeo_dvec *sa, int ai); +// print in exponential notation the transposed of a strvec +void blasfeo_print_exp_tran_dvec(int m, struct blasfeo_dvec *sa, int ai); +// print to file the transposed of a strvec +void blasfeo_print_to_file_tran_dvec(FILE *file, int m, struct blasfeo_dvec *sa, int ai); +// print to string the transposed of a strvec +void blasfeo_print_to_string_tran_dvec(char **buf_out, int m, struct blasfeo_dvec *sa, int ai); + +#endif // EXT_DEP + + + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_D_AUX_EXT_DEP_H_ diff --git a/phonelibs/acados/include/blasfeo/include/blasfeo_d_aux_ext_dep_ref.h b/phonelibs/acados/include/blasfeo/include/blasfeo_d_aux_ext_dep_ref.h new file mode 100644 index 0000000000..81b811f032 --- /dev/null +++ b/phonelibs/acados/include/blasfeo/include/blasfeo_d_aux_ext_dep_ref.h @@ -0,0 +1,84 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +/* + * auxiliary algebra operation external dependancies header + * + * include/blasfeo_d_aux_ext_dep.h + * + * - dynamic memory allocation + * - print + * + */ + +#ifndef BLASFEO_D_AUX_EXT_DEP_REF_H_ +#define BLASFEO_D_AUX_EXT_DEP_REF_H_ + + +#include + +#include "blasfeo_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +// expose reference BLASFEO for testing +// see blasfeo_d_aux_exp_dep.h for help + +void blasfeo_print_dmat_ref(int m, int n, struct blasfeo_dmat_ref *sA, int ai, int aj); +void blasfeo_allocate_dmat_ref(int m, int n, struct blasfeo_dmat_ref *sA); +void blasfeo_allocate_dvec_ref(int m, struct blasfeo_dvec_ref *sa); +void blasfeo_free_dmat_ref(struct blasfeo_dmat_ref *sA); +void blasfeo_free_dvec_ref(struct blasfeo_dvec_ref *sa); +void blasfeo_print_dmat_ref(int m, int n, struct blasfeo_dmat_ref *sA, int ai, int aj); +void blasfeo_print_exp_dmat_ref(int m, int n, struct blasfeo_dmat_ref *sA, int ai, int aj); +void blasfeo_print_to_file_dmat_ref(FILE *file, int m, int n, struct blasfeo_dmat_ref *sA, int ai, int aj); +void blasfeo_print_to_file_exp_dmat_ref(FILE *file, int m, int n, struct blasfeo_dmat_ref *sA, int ai, int aj); +void blasfeo_print_to_string_dmat_ref(char **buf_out, int m, int n, struct blasfeo_dmat_ref *sA, int ai, int aj); +void blasfeo_print_dvec(int m, struct blasfeo_dvec *sa, int ai); +void blasfeo_print_exp_dvec(int m, struct blasfeo_dvec *sa, int ai); +void blasfeo_print_to_file_dvec(FILE *file, int m, struct blasfeo_dvec *sa, int ai); +void blasfeo_print_to_string_dvec(char **buf_out, int m, struct blasfeo_dvec *sa, int ai); +void blasfeo_print_tran_dvec(int m, struct blasfeo_dvec *sa, int ai); +void blasfeo_print_exp_tran_dvec(int m, struct blasfeo_dvec *sa, int ai); +void blasfeo_print_to_file_tran_dvec(FILE *file, int m, struct blasfeo_dvec *sa, int ai); +void blasfeo_print_to_string_tran_dvec(char **buf_out, int m, struct blasfeo_dvec *sa, int ai); + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_D_AUX_EXT_DEP_REF_H_ diff --git a/phonelibs/acados/include/blasfeo/include/blasfeo_d_aux_old.h b/phonelibs/acados/include/blasfeo/include/blasfeo_d_aux_old.h new file mode 100644 index 0000000000..3a1847a6a0 --- /dev/null +++ b/phonelibs/acados/include/blasfeo/include/blasfeo_d_aux_old.h @@ -0,0 +1,75 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +/* + * ----------- TOMOVE + * + * expecting column major matrices + * + */ + +#include "blasfeo_common.h" + + +void dtrcp_l_lib(int m, double alpha, int offsetA, double *A, int sda, int offsetB, double *B, int sdb); +void dgead_lib(int m, int n, double alpha, int offsetA, double *A, int sda, int offsetB, double *B, int sdb); +// TODO remove ??? +void ddiain_sqrt_lib(int kmax, double *x, int offset, double *pD, int sdd); +// TODO ddiaad1 +void ddiareg_lib(int kmax, double reg, int offset, double *pD, int sdd); + + +void dgetr_lib(int m, int n, double alpha, int offsetA, double *pA, int sda, int offsetC, double *pC, int sdc); +void dtrtr_l_lib(int m, double alpha, int offsetA, double *pA, int sda, int offsetC, double *pC, int sdc); +void dtrtr_u_lib(int m, double alpha, int offsetA, double *pA, int sda, int offsetC, double *pC, int sdc); +void ddiaex_lib(int kmax, double alpha, int offset, double *pD, int sdd, double *x); +void ddiaad_lib(int kmax, double alpha, double *x, int offset, double *pD, int sdd); +void ddiain_libsp(int kmax, int *idx, double alpha, double *x, double *pD, int sdd); +void ddiaex_libsp(int kmax, int *idx, double alpha, double *pD, int sdd, double *x); +void ddiaad_libsp(int kmax, int *idx, double alpha, double *x, double *pD, int sdd); +void ddiaadin_libsp(int kmax, int *idx, double alpha, double *x, double *y, double *pD, int sdd); +void drowin_lib(int kmax, double alpha, double *x, double *pD); +void drowex_lib(int kmax, double alpha, double *pD, double *x); +void drowad_lib(int kmax, double alpha, double *x, double *pD); +void drowin_libsp(int kmax, double alpha, int *idx, double *x, double *pD); +void drowad_libsp(int kmax, int *idx, double alpha, double *x, double *pD); +void drowadin_libsp(int kmax, int *idx, double alpha, double *x, double *y, double *pD); +void dcolin_lib(int kmax, double *x, int offset, double *pD, int sdd); +void dcolad_lib(int kmax, double alpha, double *x, int offset, double *pD, int sdd); +void dcolin_libsp(int kmax, int *idx, double *x, double *pD, int sdd); +void dcolad_libsp(int kmax, double alpha, int *idx, double *x, double *pD, int sdd); +void dcolsw_lib(int kmax, int offsetA, double *pA, int sda, int offsetC, double *pC, int sdc); +void dvecin_libsp(int kmax, int *idx, double *x, double *y); +void dvecad_libsp(int kmax, int *idx, double alpha, double *x, double *y); diff --git a/phonelibs/acados/include/blasfeo/include/blasfeo_d_aux_ref.h b/phonelibs/acados/include/blasfeo/include/blasfeo_d_aux_ref.h new file mode 100644 index 0000000000..448234044a --- /dev/null +++ b/phonelibs/acados/include/blasfeo/include/blasfeo_d_aux_ref.h @@ -0,0 +1,206 @@ + +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_D_AUX_REF_H_ +#define BLASFEO_D_AUX_REF_H_ + + + +#include + +#include "blasfeo_common.h" +#include "blasfeo_d_aux_old.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + +// --- memory calculations +// +// returns the memory size (in bytes) needed for a dmat +size_t blasfeo_ref_memsize_dmat(int m, int n); +// returns the memory size (in bytes) needed for the diagonal of a dmat +size_t blasfeo_ref_memsize_diag_dmat(int m, int n); +// returns the memory size (in bytes) needed for a dvec +size_t blasfeo_ref_memsize_dvec(int m); + +// --- creation +// +// create a strmat for a matrix of size m*n by using memory passed by a pointer (pointer is not updated) +void blasfeo_ref_create_dmat(int m, int n, struct blasfeo_dmat *sA, void *memory); +// create a strvec for a vector of size m by using memory passed by a pointer (pointer is not updated) +void blasfeo_ref_create_dvec(int m, struct blasfeo_dvec *sA, void *memory); + +// --- packing +// pack the column-major matrix A into the matrix struct B +void blasfeo_ref_pack_dmat(int m, int n, double *A, int lda, struct blasfeo_dmat *sB, int bi, int bj); +// pack the lower-triangular column-major matrix A into the matrix struct B +void blasfeo_ref_pack_l_dmat(int m, int n, double *A, int lda, struct blasfeo_dmat *sB, int bi, int bj); +// pack the upper-triangular column-major matrix A into the matrix struct B +void blasfeo_ref_pack_u_dmat(int m, int n, double *A, int lda, struct blasfeo_dmat *sB, int bi, int bj); +// transpose and pack the column-major matrix A into the matrix struct B +void blasfeo_ref_pack_tran_dmat(int m, int n, double *A, int lda, struct blasfeo_dmat *sB, int bi, int bj); +// pack the vector x into the vector structure y +void blasfeo_ref_pack_dvec(int m, double *x, int xi, struct blasfeo_dvec *sy, int yi); +// unpack the matrix structure A into the column-major matrix B +void blasfeo_ref_unpack_dmat(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, double *B, int ldb); +// transpose and unpack the matrix structure A into the column-major matrix B +void blasfeo_ref_unpack_tran_dmat(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, double *B, int ldb); +// pack the vector structure x into the vector y +void blasfeo_ref_unpack_dvec(int m, struct blasfeo_dvec *sx, int xi, double *y, int yi); + +// --- cast +// +void ref_d_cast_mat2strmat(double *A, struct blasfeo_dmat *sA); // TODO +void ref_d_cast_diag_mat2strmat(double *dA, struct blasfeo_dmat *sA); // TODO +void ref_d_cast_vec2vecmat(double *a, struct blasfeo_dvec *sx); // TODO + + +// ge +// --- insert/extract +// +// sA[ai, aj] <= a +void blasfeo_ref_dgein1(double a, struct blasfeo_dmat *sA, int ai, int aj); +// <= sA[ai, aj] +double blasfeo_ref_dgeex1(struct blasfeo_dmat *sA, int ai, int aj); + +// --- set +// A <= alpha +void blasfeo_ref_dgese(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj); + +// --- copy / scale +// B <= A +void blasfeo_ref_dgecp(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj); +// A <= alpha*A +void blasfeo_ref_dgesc(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj); +// B <= alpha*A +void blasfeo_ref_dgecpsc(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj); +// B <= A, A lower triangular +void blasfeo_ref_dtrcp_l(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj); +void blasfeo_ref_dtrcpsc_l(int m, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj); +void blasfeo_ref_dtrsc_l(int m, double alpha, struct blasfeo_dmat *sA, int ai, int aj); + +// --- sum +// B <= B + alpha*A +void blasfeo_ref_dgead(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sC, int yi, int cj); +// y <= y + alpha*x +void blasfeo_ref_dvecad(int m, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi); + +// --- traspositions +// B <= A' +void blasfeo_ref_dgetr(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj); +// B <= A', A lower triangular +void blasfeo_ref_dtrtr_l(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj); +// B <= A', A upper triangular +void blasfeo_ref_dtrtr_u(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj); + +// dia +// diag(A) += alpha +void blasfeo_ref_ddiare(int kmax, double alpha, struct blasfeo_dmat *sA, int ai, int aj); +// diag(A) <= alpha*x +void blasfeo_ref_ddiain(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj); +// diag(A)[idx] <= alpha*x +void blasfeo_ref_ddiain_sp(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dmat *sD, int di, int dj); +// x <= diag(A) +void blasfeo_ref_ddiaex(int kmax, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi); +// x <= diag(A)[idx] +void blasfeo_ref_ddiaex_sp(int kmax, double alpha, int *idx, struct blasfeo_dmat *sD, int di, int dj, struct blasfeo_dvec *sx, int xi); +// diag(A) += alpha*x +void blasfeo_ref_ddiaad(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj); +// diag(A)[idx] += alpha*x +void blasfeo_ref_ddiaad_sp(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dmat *sD, int di, int dj); +// diag(A)[idx] = y + alpha*x +void blasfeo_ref_ddiaadin_sp(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi, int *idx, struct blasfeo_dmat *sD, int di, int dj); + +// row +void blasfeo_ref_drowin(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj); +void blasfeo_ref_drowex(int kmax, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi); +void blasfeo_ref_drowad(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj); +void blasfeo_ref_drowad_sp(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dmat *sD, int di, int dj); +void blasfeo_ref_drowsw(int kmax, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sC, int ci, int cj); +void blasfeo_ref_drowpe(int kmax, int *ipiv, struct blasfeo_dmat *sA); +void blasfeo_ref_drowpei(int kmax, int *ipiv, struct blasfeo_dmat *sA); + +// col +void blasfeo_ref_dcolex(int kmax, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi); +void blasfeo_ref_dcolin(int kmax, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj); +void blasfeo_ref_dcolad(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj); +void blasfeo_ref_dcolsc(int kmax, double alpha, struct blasfeo_dmat *sA, int ai, int aj); +void blasfeo_ref_dcolsw(int kmax, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sC, int ci, int cj); +void blasfeo_ref_dcolpe(int kmax, int *ipiv, struct blasfeo_dmat *sA); +void blasfeo_ref_dcolpei(int kmax, int *ipiv, struct blasfeo_dmat *sA); + +// vec +// a <= alpha +void blasfeo_ref_dvecse(int m, double alpha, struct blasfeo_dvec *sx, int xi); +// sx[xi] <= a +void blasfeo_ref_dvecin1(double a, struct blasfeo_dvec *sx, int xi); +// <= sx[xi] +double blasfeo_ref_dvecex1(struct blasfeo_dvec *sx, int xi); +// y <= x +void blasfeo_ref_dveccp(int m, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi); +// x <= alpha*x +void blasfeo_ref_dvecsc(int m, double alpha, struct blasfeo_dvec *sx, int xi); +// y <= alpha*x +void blasfeo_ref_dveccpsc(int m, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi); +void blasfeo_ref_dvecad_sp(int m, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dvec *sz, int zi); +void blasfeo_ref_dvecin_sp(int m, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dvec *sz, int zi); +void blasfeo_ref_dvecex_sp(int m, double alpha, int *idx, struct blasfeo_dvec *sx, int x, struct blasfeo_dvec *sz, int zi); + +void blasfeo_ref_dveccl(int m, + struct blasfeo_dvec *sxm, int xim, struct blasfeo_dvec *sx, int xi, + struct blasfeo_dvec *sxp, int xip, struct blasfeo_dvec *sz, int zi); + +void blasfeo_ref_dveccl_mask(int m, + struct blasfeo_dvec *sxm, int xim, struct blasfeo_dvec *sx, int xi, + struct blasfeo_dvec *sxp, int xip, struct blasfeo_dvec *sz, int zi, + struct blasfeo_dvec *sm, int mi); + +void blasfeo_ref_dvecze(int m, struct blasfeo_dvec *sm, int mi, struct blasfeo_dvec *sv, int vi, struct blasfeo_dvec *se, int ei); +void blasfeo_ref_dvecnrm_inf(int m, struct blasfeo_dvec *sx, int xi, double *ptr_norm); +void blasfeo_ref_dvecpe(int kmax, int *ipiv, struct blasfeo_dvec *sx, int xi); +void blasfeo_ref_dvecpei(int kmax, int *ipiv, struct blasfeo_dvec *sx, int xi); + + + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_D_AUX_REF_H_ diff --git a/phonelibs/acados/include/blasfeo/include/blasfeo_d_aux_test.h b/phonelibs/acados/include/blasfeo/include/blasfeo_d_aux_test.h new file mode 100644 index 0000000000..1c61635f3f --- /dev/null +++ b/phonelibs/acados/include/blasfeo/include/blasfeo_d_aux_test.h @@ -0,0 +1,226 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +/* + * auxiliary algebra operations header + * + * include/blasfeo_aux_lib*.h + * + */ + +#ifndef BLASFEO_D_AUX_TEST_H_ +#define BLASFEO_D_AUX_TEST_H_ + +#include "blasfeo_common.h" + + +#ifdef __cplusplus +extern "C" { +#endif + +// --- memory calculations +int test_blasfeo_memsize_dmat(int m, int n); +int test_blasfeo_memsize_diag_dmat(int m, int n); +int test_blasfeo_memsize_dvec(int m); + +// --- creation +void test_blasfeo_create_dmat(int m, int n, struct blasfeo_dmat *sA, void *memory); +void test_blasfeo_create_dvec(int m, struct blasfeo_dvec *sA, void *memory); + +// --- conversion +void test_blasfeo_pack_dmat(int m, int n, double *A, int lda, struct blasfeo_dmat *sA, int ai, int aj); +void test_blasfeo_pack_dvec(int m, double *x, int xi, struct blasfeo_dvec *sa, int ai); +void test_blasfeo_pack_tran_dmat(int m, int n, double *A, int lda, struct blasfeo_dmat *sA, int ai, int aj); +void test_blasfeo_unpack_dmat(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, double *A, int lda); +void test_blasfeo_unpack_dvec(int m, struct blasfeo_dvec *sa, int ai, double *x, int xi); +void test_blasfeo_unpack_tran_dmat(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, double *A, int lda); + +// --- cast +void test_d_cast_mat2strmat(double *A, struct blasfeo_dmat *sA); +void test_d_cast_diag_mat2strmat(double *dA, struct blasfeo_dmat *sA); +void test_d_cast_vec2vecmat(double *a, struct blasfeo_dvec *sa); + +// ------ copy / scale + +// B <= A +void test_blasfeo_dgecp(int m, int n, + struct blasfeo_dmat *sA, int ai, int aj, + struct blasfeo_dmat *sB, int bi, int bj); + +// A <= alpha*A +void test_blasfeo_dgesc(int m, int n, + double alpha, + struct blasfeo_dmat *sA, int ai, int aj); + +// B <= alpha*A +void test_blasfeo_dgecpsc(int m, int n, + double alpha, + struct blasfeo_dmat *sA, int ai, int aj, + struct blasfeo_dmat *sB, int bi, int bj); + +// // --- insert/extract +// // +// // <= sA[ai, aj] +// void test_blasfeo_dgein1(double a, struct blasfeo_dmat *sA, int ai, int aj); +// // <= sA[ai, aj] +// double blasfeo_dgeex1(struct blasfeo_dmat *sA, int ai, int aj); +// // sx[xi] <= a +// void test_blasfeo_dvecin1(double a, struct blasfeo_dvec *sx, int xi); +// // <= sx[xi] +// double blasfeo_dvecex1(struct blasfeo_dvec *sx, int xi); +// // A <= alpha + +// // --- set +// void test_blasfeo_dgese(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj); +// // a <= alpha +// void test_blasfeo_dvecse(int m, double alpha, struct blasfeo_dvec *sx, int xi); +// // B <= A + + +// // --- vector +// // y <= x +// void test_blasfeo_dveccp(int m, struct blasfeo_dvec *sa, int ai, struct blasfeo_dvec *sc, int ci); +// // x <= alpha*x +// void test_blasfeo_dvecsc(int m, double alpha, struct blasfeo_dvec *sa, int ai); +// // TODO +// // x <= alpha*x +// void test_blasfeo_dveccpsc(int m, double alpha, struct blasfeo_dvec *sa, int ai, struct blasfeo_dvec *sc, int ci); + + +// // B <= A, A lower triangular +// void test_blasfeo_dtrcp_l(int m, +// struct blasfeo_dmat *sA, int ai, int aj, +// struct blasfeo_dmat *sB, int bi, int bj); + +// void test_blasfeo_dtrcpsc_l(int m, double alpha, +// struct blasfeo_dmat *sA, int ai, int aj, +// struct blasfeo_dmat *sB, int bi, int bj); + +// void test_blasfeo_dtrsc_l(int m, double alpha, +// struct blasfeo_dmat *sA, int ai, int aj); + + +// // B <= B + alpha*A +// void test_blasfeo_dgead(int m, int n, double alpha, +// struct blasfeo_dmat *sA, int ai, int aj, +// struct blasfeo_dmat *sC, int ci, int cj); + +// // y <= y + alpha*x +// void test_blasfeo_dvecad(int m, double alpha, +// struct blasfeo_dvec *sa, int ai, +// struct blasfeo_dvec *sc, int ci); + +// // --- traspositions +// void test_dgetr_lib(int m, int n, double alpha, int offsetA, double *pA, int sda, int offsetC, double *pC, int sdc); +// void test_blasfeo_dgetr(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sC, int ci, int cj); +// void test_dtrtr_l_lib(int m, double alpha, int offsetA, double *pA, int sda, int offsetC, double *pC, int sdc); +// void test_blasfeo_dtrtr_l(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sC, int ci, int cj); +// void test_dtrtr_u_lib(int m, double alpha, int offsetA, double *pA, int sda, int offsetC, double *pC, int sdc); +// void test_blasfeo_dtrtr_u(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sC, int ci, int cj); +// void test_blasfeo_ddiare(int kmax, double alpha, struct blasfeo_dmat *sA, int ai, int aj); +// void test_blasfeo_ddiain(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj); +// void test_ddiaex_lib(int kmax, double alpha, int offset, double *pD, int sdd, double *x); +// void test_ddiaad_lib(int kmax, double alpha, double *x, int offset, double *pD, int sdd); +// void test_ddiain_libsp(int kmax, int *idx, double alpha, double *x, double *pD, int sdd); +// void test_blasfeo_ddiain_sp(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dmat *sD, int di, int dj); +// void test_ddiaex_libsp(int kmax, int *idx, double alpha, double *pD, int sdd, double *x); +// void test_blasfeo_ddiaex(int kmax, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi); +// void test_blasfeo_ddiaex_sp(int kmax, double alpha, int *idx, struct blasfeo_dmat *sD, int di, int dj, struct blasfeo_dvec *sx, int xi); +// void test_blasfeo_ddiaad(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj); +// void test_ddiaad_libsp(int kmax, int *idx, double alpha, double *x, double *pD, int sdd); +// void test_blasfeo_ddiaad_sp(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dmat *sD, int di, int dj); +// void test_ddiaadin_libsp(int kmax, int *idx, double alpha, double *x, double *y, double *pD, int sdd); +// void test_blasfeo_ddiaadin_sp(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi, int *idx, struct blasfeo_dmat *sD, int di, int dj); +// void test_drowin_lib(int kmax, double alpha, double *x, double *pD); +// void test_blasfeo_drowin(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj); +// void test_drowex_lib(int kmax, double alpha, double *pD, double *x); +// void test_blasfeo_drowex(int kmax, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi); +// void test_drowad_lib(int kmax, double alpha, double *x, double *pD); +// void test_blasfeo_drowad(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj); +// void test_drowin_libsp(int kmax, double alpha, int *idx, double *x, double *pD); +// void test_drowad_libsp(int kmax, int *idx, double alpha, double *x, double *pD); +// void test_blasfeo_drowad_sp(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dmat *sD, int di, int dj); +// void test_drowadin_libsp(int kmax, int *idx, double alpha, double *x, double *y, double *pD); +// void test_drowsw_lib(int kmax, double *pA, double *pC); +// void test_blasfeo_drowsw(int kmax, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sC, int ci, int cj); +// void test_blasfeo_drowpe(int kmax, int *ipiv, struct blasfeo_dmat *sA); +// void test_blasfeo_dcolex(int kmax, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi); +// void test_dcolin_lib(int kmax, double *x, int offset, double *pD, int sdd); +// void test_blasfeo_dcolin(int kmax, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj); +// void test_dcolad_lib(int kmax, double alpha, double *x, int offset, double *pD, int sdd); +// void test_dcolin_libsp(int kmax, int *idx, double *x, double *pD, int sdd); +// void test_dcolad_libsp(int kmax, double alpha, int *idx, double *x, double *pD, int sdd); +// void test_dcolsw_lib(int kmax, int offsetA, double *pA, int sda, int offsetC, double *pC, int sdc); +// void test_blasfeo_dcolsw(int kmax, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sC, int ci, int cj); +// void test_blasfeo_dcolpe(int kmax, int *ipiv, struct blasfeo_dmat *sA); +// void test_dvecin_libsp(int kmax, int *idx, double *x, double *y); +// void test_dvecad_libsp(int kmax, int *idx, double alpha, double *x, double *y); +// void test_blasfeo_dvecad_sp(int m, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dvec *sz, int zi); +// void test_blasfeo_dvecin_sp(int m, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dvec *sz, int zi); +// void test_blasfeo_dvecex_sp(int m, double alpha, int *idx, struct blasfeo_dvec *sx, int x, struct blasfeo_dvec *sz, int zi); +// void test_blasfeo_dveccl(int m, struct blasfeo_dvec *sxm, int xim, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sxp, int xip, struct blasfeo_dvec *sz, int zi); +// void test_blasfeo_dveccl_mask(int m, struct blasfeo_dvec *sxm, int xim, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sxp, int xip, struct blasfeo_dvec *sz, int zi, struct blasfeo_dvec *sm, int mi); +// void test_blasfeo_dvecze(int m, struct blasfeo_dvec *sm, int mi, struct blasfeo_dvec *sv, int vi, struct blasfeo_dvec *se, int ei); +// void test_blasfeo_dvecnrm_inf(int m, struct blasfeo_dvec *sx, int xi, double *ptr_norm); +// void test_blasfeo_dvecpe(int kmax, int *ipiv, struct blasfeo_dvec *sx, int xi); +// void test_blasfeo_dvecpei(int kmax, int *ipiv, struct blasfeo_dvec *sx, int xi); + +// ext_dep + +void test_blasfeo_allocate_dmat(int m, int n, struct blasfeo_dmat *sA); +void test_blasfeo_allocate_dvec(int m, struct blasfeo_dvec *sa); + +void test_blasfeo_free_dmat(struct blasfeo_dmat *sA); +void test_blasfeo_free_dvec(struct blasfeo_dvec *sa); + +void test_blasfeo_print_dmat(int m, int n, struct blasfeo_dmat *sA, int ai, int aj); +void test_blasfeo_print_dvec(int m, struct blasfeo_dvec *sa, int ai); +void test_blasfeo_print_tran_dvec(int m, struct blasfeo_dvec *sa, int ai); + +void test_blasfeo_print_to_file_dmat(FILE *file, int m, int n, struct blasfeo_dmat *sA, int ai, int aj); +void test_blasfeo_print_to_file_dvec(FILE *file, int m, struct blasfeo_dvec *sa, int ai); +void test_blasfeo_print_to_file_tran_dvec(FILE *file, int m, struct blasfeo_dvec *sa, int ai); + +void test_blasfeo_print_exp_dmat(int m, int n, struct blasfeo_dmat *sA, int ai, int aj); +void test_blasfeo_print_exp_dvec(int m, struct blasfeo_dvec *sa, int ai); +void test_blasfeo_print_exp_tran_dvec(int m, struct blasfeo_dvec *sa, int ai); + + + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_D_AUX_TEST_H_ diff --git a/phonelibs/acados/include/blasfeo/include/blasfeo_d_blas.h b/phonelibs/acados/include/blasfeo/include/blasfeo_d_blas.h new file mode 100644 index 0000000000..ea8d006731 --- /dev/null +++ b/phonelibs/acados/include/blasfeo/include/blasfeo_d_blas.h @@ -0,0 +1,46 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_D_BLAS_H_ +#define BLASFEO_D_BLAS_H_ + + + +#include "blasfeo_d_blasfeo_api.h" +#include "blasfeo_d_blas_api.h" + + + +#endif // BLASFEO_D_BLAS_H_ diff --git a/phonelibs/acados/include/blasfeo/include/blasfeo_d_blas_api.h b/phonelibs/acados/include/blasfeo/include/blasfeo_d_blas_api.h new file mode 100644 index 0000000000..64dd43f677 --- /dev/null +++ b/phonelibs/acados/include/blasfeo/include/blasfeo_d_blas_api.h @@ -0,0 +1,161 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef BLASFEO_D_BLAS_API_H_ +#define BLASFEO_D_BLAS_API_H_ + + + +#include "blasfeo_target.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +#ifdef BLAS_API + + + +#ifdef FORTRAN_BLAS_API + + + +// BLAS 1 +// +void daxpy_(int *n, double *alpha, double *x, int *incx, double *y, int *incy); +// +void dcopy_(int *n, double *x, int *incx, double *y, int *incy); +// +double ddot_(int *n, double *x, int *incx, double *y, int *incy); + +// BLAS 3 +// +void dgemm_(char *ta, char *tb, int *m, int *n, int *k, double *alpha, double *A, int *lda, double *B, int *ldb, double *beta, double *C, int *ldc); +// +void dsyrk_(char *uplo, char *ta, int *m, int *k, double *alpha, double *A, int *lda, double *beta, double *C, int *ldc); +// +void dtrmm_(char *side, char *uplo, char *transa, char *diag, int *m, int *n, double *alpha, double *A, int *lda, double *B, int *ldb); +// +void dtrsm_(char *side, char *uplo, char *transa, char *diag, int *m, int *n, double *alpha, double *A, int *lda, double *B, int *ldb); + + + +// LAPACK +// +void dgesv_(int *m, int *n, double *A, int *lda, int *ipiv, double *B, int *ldb, int *info); +// +void dgetrf_(int *m, int *n, double *A, int *lda, int *ipiv, int *info); +// +void dgetrf_np_(int *m, int *n, double *A, int *lda, int *info); +// +void dgetrs_(char *trans, int *m, int *n, double *A, int *lda, int *ipiv, double *B, int *ldb, int *info); +// +void dlaswp_(int *n, double *A, int *lda, int *k1, int *k2, int *ipiv, int *incx); +// +void dposv_(char *uplo, int *m, int *n, double *A, int *lda, double *B, int *ldb, int *info); +// +void dpotrf_(char *uplo, int *m, double *A, int *lda, int *info); +// +void dpotrs_(char *uplo, int *m, int *n, double *A, int *lda, double *B, int *ldb, int *info); +// +void dtrtrs_(char *uplo, char *trans, char *diag, int *m, int *n, double *A, int *lda, double *B, int *ldb, int *info); + + + +#else // BLASFEO_API + + + +// BLAS 1 +// +void blas_daxpy(int *n, double *alpha, double *x, int *incx, double *y, int *incy); +// +double blas_ddot(int *n, double *x, int *incx, double *y, int *incy); +// +void blas_dcopy(int *n, double *x, int *incx, double *y, int *incy); + +// BLAS 3 +// +void blas_dgemm(char *ta, char *tb, int *m, int *n, int *k, double *alpha, double *A, int *lda, double *B, int *ldb, double *beta, double *C, int *ldc); +// +void blas_dsyrk(char *uplo, char *ta, int *m, int *k, double *alpha, double *A, int *lda, double *beta, double *C, int *ldc); +// +void blas_dtrmm(char *side, char *uplo, char *transa, char *diag, int *m, int *n, double *alpha, double *A, int *lda, double *B, int *ldb); +// +void blas_dtrsm(char *side, char *uplo, char *transa, char *diag, int *m, int *n, double *alpha, double *A, int *lda, double *B, int *ldb); + + + +// LAPACK +// +void blas_dgesv(int *m, int *n, double *A, int *lda, int *ipiv, double *B, int *ldb, int *info); +// +void blas_dgetrf(int *m, int *n, double *A, int *lda, int *ipiv, int *info); +// +void blas_dgetrf_np(int *m, int *n, double *A, int *lda, int *info); +// +void blas_dgetrs(char *trans, int *m, int *n, double *A, int *lda, int *ipiv, double *B, int *ldb, int *info); +// +void blas_dlaswp(int *n, double *A, int *lda, int *k1, int *k2, int *ipiv, int *incx); +// +void blas_dposv(char *uplo, int *m, int *n, double *A, int *lda, double *B, int *ldb, int *info); +// +void blas_dpotrf(char *uplo, int *m, double *A, int *lda, int *info); +// +void blas_dpotrs(char *uplo, int *m, int *n, double *A, int *lda, double *B, int *ldb, int *info); +// +void blas_dtrtrs(char *uplo, char *trans, char *diag, int *m, int *n, double *A, int *lda, double *B, int *ldb, int *info); + + + +#endif // BLASFEO_API + + + +#endif // BLAS_API + + + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_D_BLAS_API_H_ diff --git a/phonelibs/acados/include/blasfeo/include/blasfeo_d_blasfeo_api.h b/phonelibs/acados/include/blasfeo/include/blasfeo_d_blasfeo_api.h new file mode 100644 index 0000000000..f27264e133 --- /dev/null +++ b/phonelibs/acados/include/blasfeo/include/blasfeo_d_blasfeo_api.h @@ -0,0 +1,342 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_D_BLASFEO_API_H_ +#define BLASFEO_D_BLASFEO_API_H_ + + + +#include "blasfeo_common.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +// +// level 1 BLAS +// + +// z = y + alpha*x +// z[zi:zi+n] = alpha*x[xi:xi+n] + y[yi:yi+n] +// NB: Different arguments semantic compare to equivalent standard BLAS routine +void blasfeo_daxpy(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi); +// z = beta*y + alpha*x +void blasfeo_daxpby(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, double beta, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi); +// z = x .* y +void blasfeo_dvecmul(int m, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi); +// z += x .* y +void blasfeo_dvecmulacc(int m, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi); +// z = x .* y, return sum(z) = x^T * y +double blasfeo_dvecmuldot(int m, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi); +// return x^T * y +double blasfeo_ddot(int m, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi); +// construct givens plane rotation +void blasfeo_drotg(double a, double b, double *c, double *s); +// apply plane rotation [a b] [c -s; s; c] to the aj0 and aj1 columns of A at row index ai +void blasfeo_dcolrot(int m, struct blasfeo_dmat *sA, int ai, int aj0, int aj1, double c, double s); +// apply plane rotation [c s; -s c] [a; b] to the ai0 and ai1 rows of A at column index aj +void blasfeo_drowrot(int m, struct blasfeo_dmat *sA, int ai0, int ai1, int aj, double c, double s); + + + +// +// level 2 BLAS +// + +// dense + +// z <= beta * y + alpha * A * x +void blasfeo_dgemv_n(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, double beta, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi); +// z <= beta * y + alpha * A^T * x +void blasfeo_dgemv_t(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, double beta, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi); +// z <= inv( A ) * x, A (m)x(n) +void blasfeo_dtrsv_lnn_mn(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= inv( A^T ) * x, A (m)x(n) +void blasfeo_dtrsv_ltn_mn(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= inv( A ) * x, A (m)x(m) lower, not_transposed, not_unit +void blasfeo_dtrsv_lnn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= inv( A ) * x, A (m)x(m) lower, not_transposed, unit +void blasfeo_dtrsv_lnu(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= inv( A^T ) * x, A (m)x(m) lower, transposed, not_unit +void blasfeo_dtrsv_ltn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= inv( A^T ) * x, A (m)x(m) lower, transposed, unit +void blasfeo_dtrsv_ltu(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= inv( A^T ) * x, A (m)x(m) upper, not_transposed, not_unit +void blasfeo_dtrsv_unn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= inv( A^T ) * x, A (m)x(m) upper, transposed, not_unit +void blasfeo_dtrsv_utn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= beta * y + alpha * A * x ; A upper triangular +void blasfeo_dtrmv_unn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= A^T * x ; A upper triangular +void blasfeo_dtrmv_utn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= A * x ; A lower triangular +void blasfeo_dtrmv_lnn(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= A^T * x ; A lower triangular +void blasfeo_dtrmv_ltn(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= A * x ; A lower triangular, unit diagonal +void blasfeo_dtrmv_lnu(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= A^T * x ; A lower triangular, unit diagonal +void blasfeo_dtrmv_ltu(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z_n <= beta_n * y_n + alpha_n * A * x_n +// z_t <= beta_t * y_t + alpha_t * A^T * x_t +void blasfeo_dgemv_nt(int m, int n, double alpha_n, double alpha_t, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx_n, int xi_n, struct blasfeo_dvec *sx_t, int xi_t, double beta_n, double beta_t, struct blasfeo_dvec *sy_n, int yi_n, struct blasfeo_dvec *sy_t, int yi_t, struct blasfeo_dvec *sz_n, int zi_n, struct blasfeo_dvec *sz_t, int zi_t); +// z <= beta * y + alpha * A * x, where A is symmetric and only the lower triangular patr of A is accessed +void blasfeo_dsymv_l(int m, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, double beta, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi); +void blasfeo_dsymv_l_mn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, double beta, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi); + +// diagonal + +// z <= beta * y + alpha * A * x, A diagonal +void blasfeo_dgemv_d(int m, double alpha, struct blasfeo_dvec *sA, int ai, struct blasfeo_dvec *sx, int xi, double beta, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi); + + + +// +// level 3 BLAS +// + +// dense + +// D <= beta * C + alpha * A * B +void blasfeo_dgemm_nn(int m, int n, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= beta * C + alpha * A * B^T +void blasfeo_dgemm_nt(int m, int n, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= beta * C + alpha * A^T * B +void blasfeo_dgemm_tn(int m, int n, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= beta * C + alpha * A^T * B^T +void blasfeo_dgemm_tt(int m, int n, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= beta * C + alpha * A * B^T ; C, D lower triangular +void blasfeo_dsyrk_ln(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +void blasfeo_dsyrk_ln_mn(int m, int n, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +#if ( defined(LA_HIGH_PERFORMANCE) & defined(MF_COLMAJ) ) +void blasfeo_dsyrk3_ln(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +#endif +// D <= beta * C + alpha * A^T * B ; C, D lower triangular +void blasfeo_dsyrk_lt(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +#if ( defined(LA_HIGH_PERFORMANCE) & defined(MF_COLMAJ) ) +void blasfeo_dsyrk3_lt(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +#endif +// D <= beta * C + alpha * A * B^T ; C, D upper triangular +void blasfeo_dsyrk_un(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +#if ( defined(LA_HIGH_PERFORMANCE) & defined(MF_COLMAJ) ) +void blasfeo_dsyrk3_un(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +#endif +// D <= beta * C + alpha * A^T * B ; C, D upper triangular +void blasfeo_dsyrk_ut(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +#if ( defined(LA_HIGH_PERFORMANCE) & defined(MF_COLMAJ) ) +void blasfeo_dsyrk3_ut(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +#endif +// D <= alpha * A * B ; A lower triangular +void blasfeo_dtrmm_llnn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A * B ; A lower triangular +void blasfeo_dtrmm_llnu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^T * B ; A lower triangular +void blasfeo_dtrmm_lltn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^T * B ; A lower triangular +void blasfeo_dtrmm_lltu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A * B ; A upper triangular +void blasfeo_dtrmm_lunn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A * B ; A upper triangular +void blasfeo_dtrmm_lunu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^T * B ; A upper triangular +void blasfeo_dtrmm_lutn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^T * B ; A upper triangular +void blasfeo_dtrmm_lutu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A ; A lower triangular +void blasfeo_dtrmm_rlnn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A ; A lower triangular +void blasfeo_dtrmm_rlnu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^T ; A lower triangular +void blasfeo_dtrmm_rltn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^T ; A lower triangular +void blasfeo_dtrmm_rltu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A ; A upper triangular +void blasfeo_dtrmm_runn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A ; A upper triangular +void blasfeo_dtrmm_runu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^T ; A upper triangular +void blasfeo_dtrmm_rutn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^T ; A upper triangular +void blasfeo_dtrmm_rutu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^{-1} * B , with A lower triangular employint explicit inverse of diagonal +// D <= alpha * A^{-1} * B , with A lower triangular employing explicit inverse of diagonal +void blasfeo_dtrsm_llnn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^{-1} * B , with A lower triangular with unit diagonal +void blasfeo_dtrsm_llnu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^{-T} * B , with A lower triangular employing explicit inverse of diagonal +void blasfeo_dtrsm_lltn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^{-T} * B , with A lower triangular with unit diagonal +void blasfeo_dtrsm_lltu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^{-1} * B , with A upper triangular employing explicit inverse of diagonal +void blasfeo_dtrsm_lunn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^{-1} * B , with A upper triangular with unit diagonal +void blasfeo_dtrsm_lunu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^{-T} * B , with A upper triangular employing explicit inverse of diagonal +void blasfeo_dtrsm_lutn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^{-T} * B , with A upper triangular with unit diagonal +void blasfeo_dtrsm_lutu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^{-1} , with A lower triangular employing explicit inverse of diagonal +void blasfeo_dtrsm_rlnn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^{-1} , with A lower triangular with unit diagonal +void blasfeo_dtrsm_rlnu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^{-T} , with A lower triangular employing explicit inverse of diagonal +void blasfeo_dtrsm_rltn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^{-T} , with A lower triangular with unit diagonal +void blasfeo_dtrsm_rltu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^{-1} , with A upper triangular employing explicit inverse of diagonal +void blasfeo_dtrsm_runn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^{-1} , with A upper triangular with unit diagonal +void blasfeo_dtrsm_runu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^{-T} , with A upper triangular employing explicit inverse of diagonal +void blasfeo_dtrsm_rutn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^{-T} , with A upper triangular with unit diagonal +void blasfeo_dtrsm_rutu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); + +// diagonal + +// D <= alpha * A * B + beta * C, with A diagonal (stored as strvec) +void dgemm_diag_left_lib(int m, int n, double alpha, double *dA, double *pB, int sdb, double beta, double *pC, int sdc, double *pD, int sdd); +void blasfeo_dgemm_dn(int m, int n, double alpha, struct blasfeo_dvec *sA, int ai, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A * B + beta * C, with B diagonal (stored as strvec) +void blasfeo_dgemm_nd(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sB, int bi, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); + + + +// +// LAPACK +// + +// D <= chol( C ) ; C, D lower triangular +void blasfeo_dpotrf_l(int m, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +void blasfeo_dpotrf_l_mn(int m, int n, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= chol( C ) ; C, D upper triangular +void blasfeo_dpotrf_u(int m, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= chol( C + A * B' ) ; C, D lower triangular +// D <= chol( C + A * B^T ) ; C, D lower triangular +void blasfeo_dsyrk_dpotrf_ln(int m, int k, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +void blasfeo_dsyrk_dpotrf_ln_mn(int m, int n, int k, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= lu( C ) ; no pivoting +void blasfeo_dgetrf_np(int m, int n, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= lu( C ) ; row pivoting +void blasfeo_dgetrf_rp(int m, int n, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj, int *ipiv); +// D <= qr( C ) +int blasfeo_dgeqrf_worksize(int m, int n); // in bytes +void blasfeo_dgeqrf(int m, int n, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj, void *work); +// D <= Q factor, where C is the output of the LQ factorization +int blasfeo_dorglq_worksize(int m, int n, int k); // in bytes +void blasfeo_dorglq(int m, int n, int k, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj, void *work); +// D <= lq( C ) +void blasfeo_dgelqf(int m, int n, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj, void *work); +int blasfeo_dgelqf_worksize(int m, int n); // in bytes +// D <= lq( C ), positive diagonal elements +void blasfeo_dgelqf_pd(int m, int n, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj, void *work); +// [L, A] <= lq( [L, A] ), positive diagonal elements, array of matrices, with +// L lower triangular, of size (m)x(m) +// A full, of size (m)x(n1) +void blasfeo_dgelqf_pd_la(int m, int n1, struct blasfeo_dmat *sL, int li, int lj, struct blasfeo_dmat *sA, int ai, int aj, void *work); +// [L, L, A] <= lq( [L, L, A] ), positive diagonal elements, array of matrices, with: +// L lower triangular, of size (m)x(m) +// A full, of size (m)x(n1) +void blasfeo_dgelqf_pd_lla(int m, int n1, struct blasfeo_dmat *sL0, int l0i, int l0j, struct blasfeo_dmat *sL1, int l1i, int l1j, struct blasfeo_dmat *sA, int ai, int aj, void *work); + + + +// +// BLAS API helper functions +// + +#if ( defined(BLAS_API) & defined(MF_PANELMAJ) ) +// BLAS 3 +void blasfeo_cm_dgemm_nn(int m, int n, int k, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, double beta, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dgemm_nt(int m, int n, int k, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, double beta, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dgemm_tn(int m, int n, int k, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, double beta, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dgemm_tt(int m, int n, int k, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, double beta, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dsyrk_ln(int m, int k, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, double beta, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dsyrk_lt(int m, int k, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, double beta, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dsyrk_un(int m, int k, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, double beta, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dsyrk_ut(int m, int k, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, double beta, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dsyrk3_ln(int m, int k, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, double beta, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dsyrk3_lt(int m, int k, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, double beta, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dsyrk3_un(int m, int k, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, double beta, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dsyrk3_ut(int m, int k, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, double beta, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrsm_llnn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrsm_llnu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrsm_lltn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrsm_lltu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrsm_lunn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrsm_lunu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrsm_lutn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrsm_lutu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrsm_rlnn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrsm_rlnu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrsm_rltn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrsm_rltu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrsm_runn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrsm_runu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrsm_rutn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrsm_rutu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrmm_llnn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrmm_llnu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrmm_lltn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrmm_lltu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrmm_lunn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrmm_lunu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrmm_lutn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrmm_lutu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrmm_rlnn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrmm_rlnu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrmm_rltn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrmm_rltu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrmm_runn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrmm_runu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrmm_rutn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrmm_rutu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +// LAPACK +void blasfeo_cm_dpotrf_l(int m, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dpotrf_u(int m, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dgetrf_rp(int m, int n, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj, int *ipiv); +#endif + + + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_D_BLASFEO_API_H_ diff --git a/phonelibs/acados/include/blasfeo/include/blasfeo_d_blasfeo_api_ref.h b/phonelibs/acados/include/blasfeo/include/blasfeo_d_blasfeo_api_ref.h new file mode 100644 index 0000000000..141a1f0f06 --- /dev/null +++ b/phonelibs/acados/include/blasfeo/include/blasfeo_d_blasfeo_api_ref.h @@ -0,0 +1,147 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_D_BLASFEO_API_REF_H_ +#define BLASFEO_D_BLASFEO_API_REF_H_ + +#include "blasfeo_common.h" + + +#ifdef __cplusplus +extern "C" { +#endif + + +// expose reference BLASFEO for testing + +// --- level 1 + +void blasfeo_daxpy_ref(int kmax, double alpha, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sy, int yi, struct blasfeo_dvec_ref *sz, int zi); +void blasfeo_daxpby_ref(int kmax, double alpha, struct blasfeo_dvec_ref *sx, int xi, double beta, struct blasfeo_dvec_ref *sy, int yi, struct blasfeo_dvec_ref *sz, int zi); +void blasfeo_dvecmul_ref(int m, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sy, int yi, struct blasfeo_dvec_ref *sz, int zi); +void blasfeo_dvecmulacc_ref(int m, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sy, int yi, struct blasfeo_dvec_ref *sz, int zi); +double blasfeo_dvecmuldot_ref(int m, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sy, int yi, struct blasfeo_dvec_ref *sz, int zi); +double blasfeo_ddot_ref(int m, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sy, int yi); +void blasfeo_drotg_ref(double a, double b, double *c, double *s); +void blasfeo_dcolrot_ref(int m, struct blasfeo_dmat_ref *sA, int ai, int aj0, int aj1, double c, double s); +void blasfeo_drowrot_ref(int m, struct blasfeo_dmat_ref *sA, int ai0, int ai1, int aj, double c, double s); + + +// --- level 2 + +// dense +void blasfeo_dgemv_n_ref(int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, double beta, struct blasfeo_dvec_ref *sy, int yi, struct blasfeo_dvec_ref *sz, int zi); +void blasfeo_dgemv_t_ref(int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, double beta, struct blasfeo_dvec_ref *sy, int yi, struct blasfeo_dvec_ref *sz, int zi); +void blasfeo_dtrsv_lnn_mn_ref(int m, int n, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sz, int zi); +void blasfeo_dtrsv_ltn_mn_ref(int m, int n, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sz, int zi); +void blasfeo_dtrsv_lnn_ref(int m, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sz, int zi); +void blasfeo_dtrsv_lnu_ref(int m, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sz, int zi); +void blasfeo_dtrsv_ltn_ref(int m, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sz, int zi); +void blasfeo_dtrsv_ltu_ref(int m, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sz, int zi); +void blasfeo_dtrsv_unn_ref(int m, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sz, int zi); +void blasfeo_dtrsv_utn_ref(int m, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sz, int zi); +void blasfeo_dtrmv_unn_ref(int m, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sz, int zi); +void blasfeo_dtrmv_utn_ref(int m, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sz, int zi); +void blasfeo_dtrmv_lnn_ref(int m, int n, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sz, int zi); +void blasfeo_dtrmv_ltn_ref(int m, int n, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sz, int zi); +void blasfeo_dtrmv_lnu_ref(int m, int n, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sz, int zi); +void blasfeo_dtrmv_ltu_ref(int m, int n, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sz, int zi); +void blasfeo_dgemv_nt_ref(int m, int n, double alpha_n, double alpha_t, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx_n, int xi_n, struct blasfeo_dvec_ref *sx_t, int xi_t, double beta_n, double beta_t, struct blasfeo_dvec_ref *sy_n, int yi_n, struct blasfeo_dvec_ref *sy_t, int yi_t, struct blasfeo_dvec_ref *sz_n, int zi_n, struct blasfeo_dvec_ref *sz_t, int zi_t); +void blasfeo_dsymv_l_ref(int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, double beta, struct blasfeo_dvec_ref *sy, int yi, struct blasfeo_dvec_ref *sz, int zi); + +// diagonal +void blasfeo_dgemv_d_ref(int m, double alpha, struct blasfeo_dvec_ref *sA, int ai, struct blasfeo_dvec_ref *sx, int xi, double beta, struct blasfeo_dvec_ref *sy, int yi, struct blasfeo_dvec_ref *sz, int zi); + + +// --- level 3 + +// dense +void blasfeo_dgemm_nn_ref( int m, int n, int k, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, double beta, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dgemm_nt_ref( int m, int n, int k, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, double beta, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dgemm_tn_ref(int m, int n, int k, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, double beta, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dgemm_tt_ref(int m, int n, int k, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, double beta, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj); + +void blasfeo_dsyrk_ln_mn_ref( int m, int n, int k, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, double beta, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dsyrk_ln_ref( int m, int k, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, double beta, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dsyrk_lt_ref( int m, int k, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, double beta, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dsyrk_un_ref( int m, int k, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, double beta, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dsyrk_ut_ref( int m, int k, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, double beta, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj); + +void blasfeo_dtrmm_rutn_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dtrmm_rlnn_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj); + +void blasfeo_dtrsm_lunu_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dtrsm_lunn_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dtrsm_lutu_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dtrsm_lutn_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dtrsm_llnu_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dtrsm_llnn_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dtrsm_lltu_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dtrsm_lltn_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dtrsm_runu_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dtrsm_runn_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dtrsm_rutu_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dtrsm_rutn_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dtrsm_rlnu_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dtrsm_rlnn_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dtrsm_rltu_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dtrsm_rltn_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj); + +// diagonal +void dgemm_diag_left_lib_ref(int m, int n, double alpha, double *dA, double *pB, int sdb, double beta, double *pC, int sdc, double *pD, int sdd); +void blasfeo_dgemm_dn_ref(int m, int n, double alpha, struct blasfeo_dvec_ref *sA, int ai, struct blasfeo_dmat_ref *sB, int bi, int bj, double beta, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dgemm_nd_ref(int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sB, int bi, double beta, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj); + +// --- lapack + +void blasfeo_dgetrf_nopivot_ref(int m, int n, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dgetrf_rowpivot_ref(int m, int n, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj, int *ipiv); +void blasfeo_dpotrf_l_ref(int m, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dpotrf_l_mn_ref(int m, int n, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dsyrk_dpotrf_ln_ref(int m, int k, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dsyrk_dpotrf_ln_mn_ref(int m, int n, int k, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dgetrf_nopivot_ref(int m, int n, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dgetrf_rowpivot_ref(int m, int n, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj, int *ipiv); +void blasfeo_dgeqrf_ref(int m, int n, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj, void *work); +void blasfeo_dgelqf_ref(int m, int n, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj, void *work); +void blasfeo_dgelqf_pd_ref(int m, int n, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj, void *work); +void blasfeo_dgelqf_pd_la_ref(int m, int n1, struct blasfeo_dmat_ref *sL, int li, int lj, struct blasfeo_dmat_ref *sA, int ai, int aj, void *work); +void blasfeo_dgelqf_pd_lla_ref(int m, int n1, struct blasfeo_dmat_ref *sL0, int l0i, int l0j, struct blasfeo_dmat_ref *sL1, int l1i, int l1j, struct blasfeo_dmat_ref *sA, int ai, int aj, void *work); + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_D_BLASFEO_API_REF_H_ diff --git a/phonelibs/acados/include/blasfeo/include/blasfeo_d_blasfeo_ref_api.h b/phonelibs/acados/include/blasfeo/include/blasfeo_d_blasfeo_ref_api.h new file mode 100644 index 0000000000..eaac365817 --- /dev/null +++ b/phonelibs/acados/include/blasfeo/include/blasfeo_d_blasfeo_ref_api.h @@ -0,0 +1,270 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_D_BLASFEO_REF_API_H_ +#define BLASFEO_D_BLASFEO_REF_API_H_ + + + +#include "blasfeo_common.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +// +// level 1 BLAS +// + +// z = y + alpha*x +// z[zi:zi+n] = alpha*x[xi:xi+n] + y[yi:yi+n] +// NB: Different arguments semantic compare to equivalent standard BLAS routine +void blasfeo_ref_daxpy(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi); +// z = beta*y + alpha*x +void blasfeo_ref_daxpby(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, double beta, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi); +// z = x .* y +void blasfeo_ref_dvecmul(int m, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi); +// z += x .* y +void blasfeo_ref_dvecmulacc(int m, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi); +// z = x .* y, return sum(z) = x^T * y +double blasfeo_ref_dvecmuldot(int m, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi); +// return x^T * y +double blasfeo_ref_ddot(int m, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi); +// construct givens plane rotation +void blasfeo_ref_drotg(double a, double b, double *c, double *s); +// apply plane rotation [a b] [c -s; s; c] to the aj0 and aj1 columns of A at row index ai +void blasfeo_ref_dcolrot(int m, struct blasfeo_dmat *sA, int ai, int aj0, int aj1, double c, double s); +// apply plane rotation [c s; -s c] [a; b] to the ai0 and ai1 rows of A at column index aj +void blasfeo_ref_drowrot(int m, struct blasfeo_dmat *sA, int ai0, int ai1, int aj, double c, double s); + + + +// +// level 2 BLAS +// + +// dense + +// z <= beta * y + alpha * A * x +void blasfeo_ref_dgemv_n(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, double beta, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi); +// z <= beta * y + alpha * A' * x +void blasfeo_ref_dgemv_t(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, double beta, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi); +// z <= inv( A ) * x, A (m)x(n) +void blasfeo_ref_dtrsv_lnn_mn(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= inv( A' ) * x, A (m)x(n) +void blasfeo_ref_dtrsv_ltn_mn(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= inv( A ) * x, A (m)x(m) lower, not_transposed, not_unit +void blasfeo_ref_dtrsv_lnn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= inv( A ) * x, A (m)x(m) lower, not_transposed, unit +void blasfeo_ref_dtrsv_lnu(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= inv( A' ) * x, A (m)x(m) lower, transposed, not_unit +void blasfeo_ref_dtrsv_ltn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= inv( A' ) * x, A (m)x(m) lower, transposed, unit +void blasfeo_ref_dtrsv_ltu(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= inv( A' ) * x, A (m)x(m) upper, not_transposed, not_unit +void blasfeo_ref_dtrsv_unn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= inv( A' ) * x, A (m)x(m) upper, transposed, not_unit +void blasfeo_ref_dtrsv_utn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= beta * y + alpha * A * x ; A upper triangular +void blasfeo_ref_dtrmv_unn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= A' * x ; A upper triangular +void blasfeo_ref_dtrmv_utn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= A * x ; A lower triangular +void blasfeo_ref_dtrmv_lnn(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= A' * x ; A lower triangular +void blasfeo_ref_dtrmv_ltn(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= A * x ; A lower triangular, unit diagonal +void blasfeo_ref_dtrmv_lnu(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= A' * x ; A lower triangular, unit diagonal +void blasfeo_ref_dtrmv_ltu(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z_n <= beta_n * y_n + alpha_n * A * x_n +// z_t <= beta_t * y_t + alpha_t * A' * x_t +void blasfeo_ref_dgemv_nt(int m, int n, double alpha_n, double alpha_t, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx_n, int xi_n, struct blasfeo_dvec *sx_t, int xi_t, double beta_n, double beta_t, struct blasfeo_dvec *sy_n, int yi_n, struct blasfeo_dvec *sy_t, int yi_t, struct blasfeo_dvec *sz_n, int zi_n, struct blasfeo_dvec *sz_t, int zi_t); +// z <= beta * y + alpha * A * x, where A is symmetric and only the lower triangular patr of A is accessed +void blasfeo_ref_dsymv_l(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, double beta, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi); + +// diagonal + +// z <= beta * y + alpha * A * x, A diagonal +void blasfeo_ref_dgemv_d(int m, double alpha, struct blasfeo_dvec *sA, int ai, struct blasfeo_dvec *sx, int xi, double beta, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi); + + + +// +// level 3 BLAS +// + +// dense + +// D <= beta * C + alpha * A * B +void blasfeo_ref_dgemm_nn(int m, int n, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= beta * C + alpha * A * B^T +void blasfeo_ref_dgemm_nt(int m, int n, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= beta * C + alpha * A^T * B +void blasfeo_ref_dgemm_tn(int m, int n, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= beta * C + alpha * A^T * B^T +void blasfeo_ref_dgemm_tt(int m, int n, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= beta * C + alpha * A * B^T ; C, D lower triangular +void blasfeo_ref_dsyrk_ln(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +void blasfeo_ref_dsyrk_ln_mn(int m, int n, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= beta * C + alpha * A^T * B ; C, D lower triangular +void blasfeo_ref_dsyrk_lt(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= beta * C + alpha * A * B^T ; C, D upper triangular +void blasfeo_ref_dsyrk_un(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= beta * C + alpha * A^T * B ; C, D upper triangular +void blasfeo_ref_dsyrk_ut(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A * B ; A lower triangular +void blasfeo_ref_dtrmm_llnn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A * B ; A lower triangular +void blasfeo_ref_dtrmm_llnu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^T * B ; A lower triangular +void blasfeo_ref_dtrmm_lltn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^T * B ; A lower triangular +void blasfeo_ref_dtrmm_lltu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A * B ; A upper triangular +void blasfeo_ref_dtrmm_lunn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A * B ; A upper triangular +void blasfeo_ref_dtrmm_lunu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^T * B ; A upper triangular +void blasfeo_ref_dtrmm_lutn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^T * B ; A upper triangular +void blasfeo_ref_dtrmm_lutu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A ; A lower triangular +void blasfeo_ref_dtrmm_rlnn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A ; A lower triangular +void blasfeo_ref_dtrmm_rlnu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^T ; A lower triangular +void blasfeo_ref_dtrmm_rltn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^T ; A lower triangular +void blasfeo_ref_dtrmm_rltu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A ; A upper triangular +void blasfeo_ref_dtrmm_runn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A ; A upper triangular +void blasfeo_ref_dtrmm_runu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^T ; A upper triangular +void blasfeo_ref_dtrmm_rutn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^T ; A upper triangular +void blasfeo_ref_dtrmm_rutu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^{-1} * B , with A lower triangular employint explicit inverse of diagonal +void blasfeo_ref_dtrsm_llnn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^{-1} * B , with A lower triangular with unit diagonal +void blasfeo_ref_dtrsm_llnu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^{-T} * B , with A lower triangular employint explicit inverse of diagonal +void blasfeo_ref_dtrsm_lltn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^{-T} * B , with A lower triangular with unit diagonal +void blasfeo_ref_dtrsm_lltu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^{-1} * B , with A upper triangular employing explicit inverse of diagonal +void blasfeo_ref_dtrsm_lunn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^{-1} * B , with A upper triangular withunit diagonal +void blasfeo_ref_dtrsm_lunu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^{-T} * B , with A upper triangular employing explicit inverse of diagonal +void blasfeo_ref_dtrsm_lutn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^{-T} * B , with A upper triangular withunit diagonal +void blasfeo_ref_dtrsm_lutu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^{-1} , with A lower triangular employing explicit inverse of diagonal +void blasfeo_ref_dtrsm_rlnn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^{-1} , with A lower triangular with unit diagonal +void blasfeo_ref_dtrsm_rlnu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^{-T} , with A lower triangular employing explicit inverse of diagonal +void blasfeo_ref_dtrsm_rltn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^{-T} , with A lower triangular with unit diagonal +void blasfeo_ref_dtrsm_rltu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^{-1} , with A upper triangular employing explicit inverse of diagonal +void blasfeo_ref_dtrsm_runn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^{-1} , with A upper triangular with unit diagonal +void blasfeo_ref_dtrsm_runu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^{-T} , with A upper triangular employing explicit inverse of diagonal +void blasfeo_ref_dtrsm_rutn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^{-T} , with A upper triangular with unit diagonal +void blasfeo_ref_dtrsm_rutu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); + +// diagonal + +// D <= alpha * A * B + beta * C, with A diagonal (stored as strvec) +void dgemm_diag_left_lib(int m, int n, double alpha, double *dA, double *pB, int sdb, double beta, double *pC, int sdc, double *pD, int sdd); +void blasfeo_ref_dgemm_dn(int m, int n, double alpha, struct blasfeo_dvec *sA, int ai, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A * B + beta * C, with B diagonal (stored as strvec) +void blasfeo_ref_dgemm_nd(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sB, int bi, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); + + + +// +// LAPACK +// + +// D <= chol( C ) ; C, D lower triangular +void blasfeo_ref_dpotrf_l(int m, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +void blasfeo_ref_dpotrf_l_mn(int m, int n, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= chol( C ) ; C, D upper triangular +void blasfeo_ref_dpotrf_u(int m, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= chol( C + A * B' ) ; C, D lower triangular +void blasfeo_ref_dsyrk_dpotrf_ln(int m, int k, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +void blasfeo_ref_dsyrk_dpotrf_ln_mn(int m, int n, int k, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= lu( C ) ; no pivoting +void blasfeo_ref_dgetrf_np(int m, int n, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= lu( C ) ; row pivoting +void blasfeo_ref_dgetrf_rp(int m, int n, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj, int *ipiv); +// D <= qr( C ) +int blasfeo_ref_dgeqrf_worksize(int m, int n); // in bytes +void blasfeo_ref_dgeqrf(int m, int n, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj, void *work); +// D <= Q factor, where C is the output of the LQ factorization +int blasfeo_ref_dorglq_worksize(int m, int n, int k); // in bytes +void blasfeo_ref_dorglq(int m, int n, int k, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj, void *work); +// D <= lq( C ) +void blasfeo_ref_dgelqf(int m, int n, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj, void *work); +int blasfeo_ref_dgelqf_worksize(int m, int n); // in bytes +// D <= lq( C ), positive diagonal elements +void blasfeo_ref_dgelqf_pd(int m, int n, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj, void *work); +// [L, A] <= lq( [L, A] ), positive diagonal elements, array of matrices, with +// L lower triangular, of size (m)x(m) +// A full, of size (m)x(n1) +void blasfeo_ref_dgelqf_pd_la(int m, int n1, struct blasfeo_dmat *sL, int li, int lj, struct blasfeo_dmat *sA, int ai, int aj, void *work); +// [L, L, A] <= lq( [L, L, A] ), positive diagonal elements, array of matrices, with: +// L lower triangular, of size (m)x(m) +// A full, of size (m)x(n1) +void blasfeo_ref_dgelqf_pd_lla(int m, int n1, struct blasfeo_dmat *sL0, int l0i, int l0j, struct blasfeo_dmat *sL1, int l1i, int l1j, struct blasfeo_dmat *sA, int ai, int aj, void *work); + + + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_D_BLASFEO_REF_API_H_ + diff --git a/phonelibs/acados/include/blasfeo/include/blasfeo_d_kernel.h b/phonelibs/acados/include/blasfeo/include/blasfeo_d_kernel.h new file mode 100644 index 0000000000..bf110c2bf8 --- /dev/null +++ b/phonelibs/acados/include/blasfeo/include/blasfeo_d_kernel.h @@ -0,0 +1,1048 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_D_KERNEL_H_ +#define BLASFEO_D_KERNEL_H_ + + + +#include "blasfeo_target.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +// utils +void blasfeo_align_2MB(void *ptr, void **ptr_align); +void blasfeo_align_4096_byte(void *ptr, void **ptr_align); +void blasfeo_align_64_byte(void *ptr, void **ptr_align); + + + +// level 2 BLAS +// 12 +void kernel_dgemv_n_12_lib4(int k, double *alpha, double *A, int sda, double *x, double *beta, double *y, double *z); +void kernel_dgemv_t_12_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *x, double *beta, double *y, double *z); +// 8 +void kernel_dgemv_n_8_lib4(int k, double *alpha, double *A, int sda, double *x, double *beta, double *y, double *z); +void kernel_dgemv_t_8_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *x, double *beta, double *y, double *z); +void kernel_dtrmv_un_8_lib4(int k, double *A, int sda, double *x, double *z); +// 4 +void kernel_dgemv_n_4_lib4(int k, double *alpha, double *A, double *x, double *beta, double *y, double *z); +void kernel_dgemv_n_4_vs_lib4(int k, double *alpha, double *A, double *x, double *beta, double *y, double *z, int k1); +void kernel_dgemv_n_4_gen_lib4(int kmax, double *alpha, double *A, double *x, double *beta, double *y, double *z, int k0, int k1); +void kernel_dgemv_t_4_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *x, double *beta, double *y, double *z); +void kernel_dgemv_t_4_vs_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *x, double *beta, double *y, double *z, int k1); +void kernel_dtrsv_ln_inv_4_lib4(int k, double *A, double *inv_diag_A, double *x, double *y, double *z); +void kernel_dtrsv_ln_inv_4_vs_lib4(int k, double *A, double *inv_diag_A, double *x, double *y, double *z, int km, int kn); +void kernel_dtrsv_lt_inv_4_lib4(int k, double *A, int sda, double *inv_diag_A, double *x, double *y, double *z); +void kernel_dtrsv_lt_inv_3_lib4(int k, double *A, int sda, double *inv_diag_A, double *x, double *y, double *z); +void kernel_dtrsv_lt_inv_2_lib4(int k, double *A, int sda, double *inv_diag_A, double *x, double *y, double *z); +void kernel_dtrsv_lt_inv_1_lib4(int k, double *A, int sda, double *inv_diag_A, double *x, double *y, double *z); +void kernel_dtrsv_lt_one_4_lib4(int k, double *A, int sda, double *x, double *y, double *z); +void kernel_dtrsv_lt_one_3_lib4(int k, double *A, int sda, double *x, double *y, double *z); +void kernel_dtrsv_lt_one_2_lib4(int k, double *A, int sda, double *x, double *y, double *z); +void kernel_dtrsv_lt_one_1_lib4(int k, double *A, int sda, double *x, double *y, double *z); +void kernel_dtrsv_ln_one_4_vs_lib4(int kmax, double *A, double *x, double *y, double *z, int km, int kn); +void kernel_dtrsv_ln_one_4_lib4(int kmax, double *A, double *x, double *y, double *z); +void kernel_dtrsv_un_inv_4_lib4(int kmax, double *A, double *inv_diag_A, double *x, double *y, double *z); +void kernel_dtrsv_ut_inv_4_lib4(int kmax, double *A, int sda, double *inv_diag_A, double *x, double *y, double *z); +void kernel_dtrsv_ut_inv_4_vs_lib4(int kmax, double *A, int sda, double *inv_diag_A, double *x, double *y, double *z, int m1, int n1); +void kernel_dtrmv_un_4_lib4(int k, double *A, double *x, double *z); +void kernel_dtrmv_ut_4_lib4(int k, double *A, int sda, double *x, double *z); +void kernel_dtrmv_ut_4_vs_lib4(int k, double *A, int sda, double *x, double *z, int km); +void kernel_dgemv_nt_6_lib4(int kmax, double *alpha_n, double *alpha_t, double *A, int sda, double *x_n, double *x_t, double *beta_t, double *y_t, double *z_n, double *z_t); +void kernel_dgemv_nt_4_lib4(int kmax, double *alpha_n, double *alpha_t, double *A, int sda, double *x_n, double *x_t, double *beta_t, double *y_t, double *z_n, double *z_t); +void kernel_dgemv_nt_4_vs_lib4(int kmax, double *alpha_n, double *alpha_t, double *A, int sda, double *x_n, double *x_t, double *beta_t, double *y_t, double *z_n, double *z_t, int km); +void kernel_dsymv_l_4_lib4(int kmax, double *alpha, double *A, int sda, double *x, double *z); +void kernel_dsymv_l_4_gen_lib4(int kmax, double *alpha, int offA, double *A, int sda, double *x, double *z, int km); + + + +// level 3 BLAS +// 12x4 +void kernel_dgemm_nt_12x4_lib4(int k, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd); // +void kernel_dgemm_nt_12x4_vs_lib4(int k, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, int km, int kn); // +void kernel_dgemm_nt_12x4_gen_lib4(int k, double *alpha, double *A, int sda, double *B, double *beta, int offsetC, double *C, int sdc, int offsetD, double *D, int sdd, int m0, int m1, int k0, int k1); +void kernel_dgemm_nn_12x4_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); // +void kernel_dgemm_nn_12x4_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int km, int kn); // +void kernel_dgemm_nn_12x4_gen_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, int offsetC, double *C, int sdc, int offsetD, double *D, int sdd, int m0, int m1, int n0, int n1); // +void kernel_dgemm_tt_12x4_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd); // +void kernel_dgemm_tt_12x4_vs_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); // +void kernel_dsyrk_nn_u_12x4_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); // +void kernel_dsyrk_nn_u_12x4_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); // +void kernel_dsyrk_nt_l_12x4_lib4(int k, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd); // +void kernel_dsyrk_nt_l_12x4_vs_lib4(int k, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, int km, int kn); // +void kernel_dtrmm_nt_ru_12x4_lib4(int k, double *alpha, double *A, int sda, double *B, double *D, int sdd); // +void kernel_dtrmm_nt_ru_12x4_vs_lib4(int k, double *alpha, double *A, int sda, double *B, double *D, int sdd, int km, int kn); // +void kernel_dtrmm_nn_rl_12x4_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *D, int sdd); +void kernel_dtrmm_nn_rl_12x4_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *D, int sdd, int km, int kn); +void kernel_dtrsm_nt_rl_inv_12x4_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E); +void kernel_dtrsm_nt_rl_inv_12x4_vs_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E, int km, int kn); +void kernel_dtrsm_nt_rl_one_12x4_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E); +void kernel_dtrsm_nt_rl_one_12x4_vs_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, int km, int kn); +void kernel_dtrsm_nt_ru_inv_12x4_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E); +void kernel_dtrsm_nt_ru_inv_12x4_vs_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E, int km, int kn); +void kernel_dtrsm_nt_ru_one_12x4_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E); +void kernel_dtrsm_nt_ru_one_12x4_vs_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, int km, int kn); +void kernel_dtrsm_nn_ru_inv_12x4_lib4(int k, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E); +void kernel_dtrsm_nn_ru_inv_12x4_vs_lib4(int k, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E, int km, int kn); +void kernel_dtrsm_nn_ll_inv_12x4_lib4(int k, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int sde, double *inv_diag_E); +void kernel_dtrsm_nn_ll_inv_12x4_vs_lib4(int k, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int sde, double *inv_diag_E, int km, int kn); +void kernel_dtrsm_nn_ll_one_12x4_lib4(int k, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int sde); +void kernel_dtrsm_nn_ll_one_12x4_vs_lib4(int k, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int sde, int km, int kn); +void kernel_dtrsm_nn_lu_inv_12x4_lib4(int kmax, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *E, int sde, double *inv_diag_E); +void kernel_dtrsm_nn_lu_inv_12x4_vs_lib4(int kmax, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *E, int sde, double *inv_diag_E, int km, int kn); +// 4x12 +void kernel_dgemm_nt_4x12_lib4(int k, double *alpha, double *A, double *B, int sdb, double *beta, double *C, double *D); // +void kernel_dgemm_nt_4x12_vs_lib4(int k, double *alpha, double *A, double *B, int sdb, double *beta, double *C, double *D, int km, int kn); // +void kernel_dgemm_nn_4x12_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D); // +void kernel_dgemm_nn_4x12_vs_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D, int m1, int n1); // +void kernel_dgemm_tt_4x12_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *B, int sdb, double *beta, double *C, double *D); // +void kernel_dgemm_tt_4x12_vs_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *B, int sdb, double *beta, double *C, double *D, int m1, int n1); // +void kernel_dgemm_tt_4x12_gen_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *B, int sdb, double *beta, int offsetC, double *C, int sdc, int offsetD, double *D, int sdd, int m0, int m1, int n0, int n1); // +void kernel_dtrsm_nt_rl_inv_4x12_lib4(int k, double *A, double *B, int sdb, double *C, double *D, double *E, int sed, double *inv_diag_E); +void kernel_dtrsm_nt_rl_inv_4x12_vs_lib4(int k, double *A, double *B, int sdb, double *C, double *D, double *E, int sed, double *inv_diag_E, int km, int kn); +// 8x8 +void kernel_dgemm_nt_8x8l_lib4(int k, double *alpha, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); // computes [A00 *; A10 A11] +void kernel_dgemm_nt_8x8u_lib4(int k, double *alpha, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); // computes [A00 *; A10 A11] +void kernel_dgemm_nt_8x8l_vs_lib4(int k, double *alpha, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int km, int kn); // computes [A00 *; A10 A11] +void kernel_dgemm_nt_8x8u_vs_lib4(int k, double *alpha, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int km, int kn); // computes [A00 *; A10 A11] +void kernel_dsyrk_nn_u_8x8_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); // +void kernel_dsyrk_nn_u_8x8_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); // +void kernel_dsyrk_nt_l_8x8_lib4(int k, double *alpha, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); // computes [L00 *; A10 L11] +void kernel_dsyrk_nt_l_8x8_vs_lib4(int k, double *alpha, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int km, int kn); // computes [L00 *; A10 L11] +void kernel_dtrsm_nt_rl_inv_8x8l_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *E, int sed, double *inv_diag_E); +void kernel_dtrsm_nt_rl_inv_8x8l_vs_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *E, int sed, double *inv_diag_E, int km, int kn); +void kernel_dtrsm_nt_rl_inv_8x8u_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *E, int sed, double *inv_diag_E); +void kernel_dtrsm_nt_rl_inv_8x8u_vs_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *E, int sed, double *inv_diag_E, int km, int kn); +// 8x4 +void kernel_dgemm_nt_8x4_lib4(int k, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd); // +void kernel_dgemm_nt_8x4_vs_lib4(int k, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, int km, int kn); // +void kernel_dgemm_nt_8x4_gen_lib4(int k, double *alpha, double *A, int sda, double *B, double *beta, int offsetC, double *C, int sdc, int offsetD, double *D, int sdd, int m0, int m1, int k0, int k1); +void kernel_dgemm_nn_8x4_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); // +void kernel_dgemm_nn_8x4_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); // +void kernel_dgemm_nn_8x4_gen_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, int offsetC, double *C, int sdc, int offsetD, double *D, int sdd, int m0, int m1, int n0, int n1); // +void kernel_dgemm_tt_8x4_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd); // +void kernel_dgemm_tt_8x4_vs_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); // +void kernel_dsyrk_nn_u_8x4_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); // +void kernel_dsyrk_nn_u_8x4_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); // +void kernel_dsyrk_nt_l_8x4_lib4(int k, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd); // +void kernel_dsyrk_nt_l_8x4_vs_lib4(int k, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, int km, int kn); // +void kernel_dsyrk_nt_l_8x4_gen_lib4(int k, double *alpha, double *A, int sda, double *B, double *beta, int offsetC, double *C, int sdc, int offsetD, double *D, int sdd, int m0, int m1, int k0, int k1); +void kernel_dtrmm_nt_ru_8x4_lib4(int k, double *alpha, double *A, int sda, double *B, double *D, int sdd); // +void kernel_dtrmm_nt_ru_8x4_vs_lib4(int k, double *alpha, double *A, int sda, double *B, double *D, int sdd, int km, int kn); // +void kernel_dtrmm_nn_rl_8x4_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *D, int sdd); +void kernel_dtrmm_nn_rl_8x4_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *D, int sdd, int km, int kn); +void kernel_dtrmm_nn_rl_8x4_gen_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, int offsetD, double *D, int sdd, int m0, int m1, int n0, int n1); +void kernel_dtrsm_nt_rl_inv_8x4_vs_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E, int km, int kn); +void kernel_dtrsm_nt_rl_inv_8x4_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E); +void kernel_dtrsm_nt_rl_one_8x4_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E); +void kernel_dtrsm_nt_rl_one_8x4_vs_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, int km, int kn); +void kernel_dtrsm_nt_ru_inv_8x4_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E); +void kernel_dtrsm_nt_ru_inv_8x4_vs_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E, int km, int kn); +void kernel_dtrsm_nt_ru_one_8x4_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E); +void kernel_dtrsm_nt_ru_one_8x4_vs_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, int km, int kn); +void kernel_dtrsm_nn_ru_inv_8x4_lib4(int k, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E); +void kernel_dtrsm_nn_ru_inv_8x4_vs_lib4(int k, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E, int km, int kn); +void kernel_dtrsm_nn_ll_inv_8x4_lib4(int k, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int sde, double *inv_diag_E); +void kernel_dtrsm_nn_ll_inv_8x4_vs_lib4(int k, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int sde, double *inv_diag_E, int km, int kn); +void kernel_dtrsm_nn_ll_one_8x4_lib4(int k, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int sde); +void kernel_dtrsm_nn_ll_one_8x4_vs_lib4(int k, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int sde, int km, int kn); +void kernel_dtrsm_nn_lu_inv_8x4_lib4(int kmax, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *E, int sde, double *inv_diag_E); +void kernel_dtrsm_nn_lu_inv_8x4_vs_lib4(int kmax, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *E, int sde, double *inv_diag_E, int km, int kn); +// 4x8 +void kernel_dgemm_nt_4x8_lib4(int k, double *alpha, double *A, double *B, int sdb, double *beta, double *C, double *D); // +void kernel_dgemm_nt_4x8_vs_lib4(int k, double *alpha, double *A, double *B, int sdb, double *beta, double *C, double *D, int km, int kn); // +void kernel_dgemm_nn_4x8_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D); // +void kernel_dgemm_nn_4x8_vs_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D, int m1, int n1); // +void kernel_dgemm_tt_4x8_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *B, int sdb, double *beta, double *C, double *D); // +void kernel_dgemm_tt_4x8_vs_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *B, int sdb, double *beta, double *C, double *D, int m1, int n1); // +void kernel_dgemm_tt_4x8_gen_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *B, int sdb, double *beta, int offsetC, double *C, int sdc, int offsetD, double *D, int sdd, int m0, int m1, int n0, int n1); // +void kernel_dtrsm_nt_rl_inv_4x8_lib4(int k, double *A, double *B, int sdb, double *C, double *D, double *E, int sed, double *inv_diag_E); +void kernel_dtrsm_nt_rl_inv_4x8_vs_lib4(int k, double *A, double *B, int sdb, double *C, double *D, double *E, int sed, double *inv_diag_E, int km, int kn); +// 8x2 +void kernel_dgemm_nn_8x2_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); // +void kernel_dgemm_nn_8x2_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); // +// 2x8 +void kernel_dgemm_nn_2x8_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D); // +void kernel_dgemm_nn_2x8_vs_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D, int m1, int n1); // +// 10xX +void kernel_dgemm_nn_10x4_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); // +void kernel_dgemm_nn_10x4_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); // +void kernel_dgemm_nn_10x2_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); // +void kernel_dgemm_nn_10x2_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); // +// 6xX +void kernel_dgemm_nn_8x6_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); // +void kernel_dgemm_nn_8x6_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); // +void kernel_dgemm_nn_6x8_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); // +void kernel_dgemm_nn_6x8_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); // +void kernel_dgemm_nn_6x6_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); // +void kernel_dgemm_nn_6x6_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); // +void kernel_dgemm_nn_6x4_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); // +void kernel_dgemm_nn_6x4_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); // +void kernel_dgemm_nn_6x2_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); // +void kernel_dgemm_nn_6x2_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); // +// 4x4 +void kernel_dgemm_nt_4x4_lib4(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D); // +void kernel_dgemm_nt_4x4_vs_lib4(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D, int km, int kn); // +void kernel_dgemm_nt_4x4_gen_lib4(int k, double *alpha, double *A, double *B, double *beta, int offsetC, double *C, int sdc, int offsetD, double *D, int sdd, int m0, int m1, int n0, int n1); +void kernel_dgemm_nn_4x4_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D); // +void kernel_dgemm_nn_4x4_vs_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D, int m1, int n1); // +void kernel_dgemm_nn_4x4_gen_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, int offsetC, double *C, int sdc, int offsetD, double *D, int sdd, int m0, int m1, int n0, int n1); // +void kernel_dgemm_tt_4x4_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *B, double *beta, double *C, double *D); // +void kernel_dgemm_tt_4x4_vs_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *B, double *beta, double *C, double *D, int m1, int n1); // +void kernel_dgemm_tt_4x4_gen_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *B, double *beta, int offsetC, double *C, int sdc, int offsetD, double *D, int sdd, int m0, int m1, int n0, int n1); // +void kernel_dsyrk_nn_u_4x4_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D); // +void kernel_dsyrk_nn_u_4x4_vs_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D, int m1, int n1); // +void kernel_dsyrk_nt_l_4x4_lib4(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D); // +void kernel_dsyrk_nt_l_4x4_vs_lib4(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D, int km, int kn); // +void kernel_dsyrk_nt_l_4x4_gen_lib4(int k, double *alpha, double *A, double *B, double *beta, int offsetC, double *C, int sdc, int offsetD, double *D, int sdd, int m0, int m1, int n0, int n1); +void kernel_dsyrk_nt_u_4x4_lib4(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D); // +void kernel_dsyrk_nt_u_4x4_vs_lib4(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D, int km, int kn); // +void kernel_dsyrk_nt_u_4x4_gen_lib4(int k, double *alpha, double *A, double *B, double *beta, int offsetC, double *C, int sdc, int offsetD, double *D, int sdd, int m0, int m1, int n0, int n1); +void kernel_dtrmm_nt_ru_4x4_lib4(int k, double *alpha, double *A, double *B, double *D); // +void kernel_dtrmm_nt_ru_4x4_vs_lib4(int k, double *alpha, double *A, double *B, double *D, int km, int kn); // +void kernel_dtrmm_nn_rl_4x4_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *D); +void kernel_dtrmm_nn_rl_4x4_vs_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *D, int m1, int n1); +void kernel_dtrmm_nn_rl_4x4_gen_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, int offsetD, double *D, int sdd, int m0, int m1, int n0, int n1); +void kernel_dtrsm_nt_rl_inv_4x4_lib4(int k, double *A, double *B, double *beta, double *C, double *D, double *E, double *inv_diag_E); +void kernel_dtrsm_nt_rl_inv_4x4_vs_lib4(int k, double *A, double *B, double *beta, double *C, double *D, double *E, double *inv_diag_E, int km, int kn); +void kernel_dtrsm_nt_rl_one_4x4_lib4(int k, double *A, double *B, double *beta, double *C, double *D, double *E); +void kernel_dtrsm_nt_rl_one_4x4_vs_lib4(int k, double *A, double *B, double *beta, double *C, double *D, double *E, int km, int kn); +void kernel_dtrsm_nt_ru_inv_4x4_lib4(int k, double *A, double *B, double *beta, double *C, double *D, double *E, double *inv_diag_E); +void kernel_dtrsm_nt_ru_inv_4x4_vs_lib4(int k, double *A, double *B, double *beta, double *C, double *D, double *E, double *inv_diag_E, int km, int kn); +void kernel_dtrsm_nt_ru_one_4x4_lib4(int k, double *A, double *B, double *beta, double *C, double *D, double *E); +void kernel_dtrsm_nt_ru_one_4x4_vs_lib4(int k, double *A, double *B, double *beta, double *C, double *D, double *E, int km, int kn); +void kernel_dtrsm_nn_ru_inv_4x4_lib4(int k, double *A, double *B, int sdb, double *beta, double *C, double *D, double *E, double *inv_diag_E); +void kernel_dtrsm_nn_ru_inv_4x4_vs_lib4(int k, double *A, double *B, int sdb, double *beta, double *C, double *D, double *E, double *inv_diag_E, int km, int kn); +void kernel_dtrsm_nn_ll_inv_4x4_lib4(int k, double *A, double *B, int sdb, double *beta, double *C, double *D, double *E, double *inv_diag_E); +void kernel_dtrsm_nn_ll_inv_4x4_vs_lib4(int k, double *A, double *B, int sdb, double *beta, double *C, double *D, double *E, double *inv_diag_E, int km, int kn); +void kernel_dtrsm_nn_ll_one_4x4_lib4(int k, double *A, double *B, int sdb, double *beta, double *C, double *D, double *E); +void kernel_dtrsm_nn_ll_one_4x4_vs_lib4(int k, double *A, double *B, int sdb, double *beta, double *C, double *D, double *E, int km, int kn); +void kernel_dtrsm_nn_lu_inv_4x4_lib4(int kmax, double *A, double *B, int sdb, double *C, double *D, double *E, double *inv_diag_E); +void kernel_dtrsm_nn_lu_inv_4x4_vs_lib4(int kmax, double *A, double *B, int sdb, double *C, double *D, double *E, double *inv_diag_E, int km, int kn); +void kernel_dtrsm_nn_lu_one_4x4_lib4(int kmax, double *A, double *B, int sdb, double *C, double *D, double *E); +void kernel_dtrsm_nn_lu_one_4x4_vs_lib4(int kmax, double *A, double *B, int sdb, double *C, double *D, double *E, int km, int kn); +// 4x2 +void kernel_dgemm_nn_4x2_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D); // +void kernel_dgemm_nn_4x2_vs_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D, int m1, int n1); // +void kernel_dgemm_nt_4x2_lib4(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D); +void kernel_dgemm_nt_4x2_vs_lib4(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D, int m1, int n1); +void kernel_dsyrk_nt_l_4x2_lib4(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D); // +void kernel_dsyrk_nt_l_4x2_vs_lib4(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D, int km, int kn); // +void kernel_dtrmm_nn_rl_4x2_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *D); +void kernel_dtrmm_nn_rl_4x2_vs_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *D, int m1, int n1); +void kernel_dtrsm_nt_rl_inv_4x2_lib4(int k, double *A, double *B, double *beta, double *C, double *D, double *E, double *inv_diag_E); +void kernel_dtrsm_nt_rl_inv_4x2_vs_lib4(int k, double *A, double *B, double *beta, double *C, double *D, double *E, double *inv_diag_E, int km, int kn); +// 2x4 +void kernel_dgemm_nn_2x4_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D); // +void kernel_dgemm_nn_2x4_vs_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D, int m1, int n1); // +// 2x2 +void kernel_dgemm_nn_2x2_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D); // +void kernel_dsyrk_nt_l_2x2_lib4(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D); // +void kernel_dsyrk_nt_l_2x2_vs_lib4(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D, int km, int kn); // +// diag +void kernel_dgemm_diag_right_4_a0_lib4(int kmax, double *alpha, double *A, int sda, double *B, double *D, int sdd); +void kernel_dgemm_diag_right_4_lib4(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd); +void kernel_dgemm_diag_right_3_lib4(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd); +void kernel_dgemm_diag_right_2_lib4(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd); +void kernel_dgemm_diag_right_1_lib4(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd); +void kernel_dgemm_diag_left_4_a0_lib4(int kmax, double *alpha, double *A, double *B, double *D); +void kernel_dgemm_diag_left_4_lib4(int kmax, double *alpha, double *A, double *B, double *beta, double *C, double *D); +void kernel_dgemm_diag_left_3_lib4(int kmax, double *alpha, double *A, double *B, double *beta, double *C, double *D); +void kernel_dgemm_diag_left_2_lib4(int kmax, double *alpha, double *A, double *B, double *beta, double *C, double *D); +void kernel_dgemm_diag_left_1_lib4(int kmax, double *alpha, double *A, double *B, double *beta, double *C, double *D); +// low rank update +void kernel_dger4_sub_12r_lib4(int k, double *A, int sda, double *B, double *C, int sdc); +void kernel_dger4_sub_12r_vs_lib4(int k, double *A, int sda, double *B, double *C, int sdc, int km); +void kernel_dger4_sub_8r_lib4(int k, double *A, int sda, double *B, double *C, int sdc); +void kernel_dger4_sub_8r_vs_lib4(int k, double *A, int sda, double *B, double *C, int sdc, int km); +void kernel_dger4_sub_4r_lib4(int n, double *A, double *B, double *C); +void kernel_dger4_sub_4r_vs_lib4(int n, double *A, double *B, double *C, int km); + + + +// LAPACK +// 12x4 +void kernel_dpotrf_nt_l_12x4_lib4(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D); +void kernel_dpotrf_nt_l_12x4_vs_lib4(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn); +void kernel_dgetrf_nn_l_12x4_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *inv_diag_D); +void kernel_dgetrf_nn_l_12x4_vs_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn); +void kernel_dgetrf_nn_m_12x4_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *inv_diag_D); +void kernel_dgetrf_nn_m_12x4_vs_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn); +void kernel_dgetrf_nn_r_12x4_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *inv_diag_D); +void kernel_dgetrf_nn_r_12x4_vs_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn); +void kernel_dgetrf_nt_l_12x4_lib4(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D); +void kernel_dgetrf_nt_l_12x4_vs_lib4(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn); +void kernel_dgetrf_nt_m_12x4_lib4(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D); +void kernel_dgetrf_nt_m_12x4_vs_lib4(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn); +void kernel_dgetrf_nt_r_12x4_lib4(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D); +void kernel_dgetrf_nt_r_12x4_vs_lib4(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn); +// 8x8 +void kernel_dpotrf_nt_l_8x8_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *inv_diag_D); +void kernel_dpotrf_nt_l_8x8_vs_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn); +// 8x4 +void kernel_dpotrf_nt_l_8x4_lib4(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D); +void kernel_dpotrf_nt_l_8x4_vs_lib4(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn); +void kernel_dgetrf_nn_l_8x4_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *inv_diag_D); +void kernel_dgetrf_nn_l_8x4_vs_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn); +void kernel_dgetrf_nn_r_8x4_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *inv_diag_D); +void kernel_dgetrf_nn_r_8x4_vs_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn); +void kernel_dgetrf_nt_l_8x4_lib4(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D); +void kernel_dgetrf_nt_l_8x4_vs_lib4(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn); +void kernel_dgetrf_nt_r_8x4_lib4(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D); +void kernel_dgetrf_nt_r_8x4_vs_lib4(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn); +// 4x4 +void kernel_dpotrf_nt_l_4x4_lib4(int k, double *A, double *B, double *C, double *D, double *inv_diag_D); +void kernel_dpotrf_nt_l_4x4_vs_lib4(int k, double *A, double *B, double *C, double *D, double *inv_diag_D, int km, int kn); +#if defined(TARGET_X64_INTEL_SANDY_BRIDGE) +void kernel_dlauum_nt_4x4_lib4(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D); // +void kernel_dlauum_nt_4x4_vs_lib4(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D, int km, int kn); // +#endif +void kernel_dgetrf_nn_4x4_lib4(int k, double *A, double *B, int sdb, double *C, double *D, double *inv_diag_D); +void kernel_dgetrf_nn_4x4_vs_lib4(int k, double *A, double *B, int sdb, double *C, double *D, double *inv_diag_D, int km, int kn); +void kernel_dgetrf_nt_4x4_lib4(int k, double *A, double *B, double *C, double *D, double *inv_diag_D); +void kernel_dgetrf_nt_4x4_vs_lib4(int k, double *A, double *B, double *C, double *D, double *inv_diag_D, int km, int kn); +void kernel_dgeqrf_4_lib4(int m, double *pD, int sdd, double *dD); +void kernel_dgeqrf_vs_lib4(int m, int n, int k, int offD, double *pD, int sdd, double *dD); +void kernel_dlarf_4_lib4(int m, int n, double *pV, int sdv, double *tau, double *pC, int sdc); // rank-4 reflector +void kernel_dlarf_t_4_lib4(int m, int n, double *pD, int sdd, double *pVt, double *dD, double *pC0, int sdc, double *pW); +void kernel_dgelqf_4_lib4(int n, double *pD, double *dD); +void kernel_dgelqf_vs_lib4(int m, int n, int k, int offD, double *pD, int sdd, double *dD); +void kernel_dlarft_4_lib4(int kmax, double *pD, double *dD, double *pT); +void kernel_dlarft_3_lib4(int kmax, double *pD, double *dD, double *pT); +void kernel_dlarft_2_lib4(int kmax, double *pD, double *dD, double *pT); +void kernel_dlarft_1_lib4(int kmax, double *pD, double *dD, double *pT); +void kernel_dgelqf_dlarft12_12_lib4(int n, double *pD, int sdd, double *dD, double *pT); +void kernel_dgelqf_dlarft4_12_lib4(int n, double *pD, int sdd, double *dD, double *pT); +void kernel_dgelqf_dlarft4_8_lib4(int n, double *pD, int sdd, double *dD, double *pT); +void kernel_dgelqf_dlarft4_4_lib4(int n, double *pD, double *dD, double *pT); +void kernel_dlarfb12_rn_12_lib4(int kmax, double *pV, int sdd, double *pT, double *pD, double *pK); +void kernel_dlarfb12_rn_4_lib4(int kmax, double *pV, int sdd, double *pT, double *pD, double *pK, int km); +void kernel_dlarfb4_rn_12_lib4(int kmax, double *pV, double *pT, double *pD, int sdd); +void kernel_dlarfb4_rn_8_lib4(int kmax, double *pV, double *pT, double *pD, int sdd); +void kernel_dlarfb4_rn_4_lib4(int kmax, double *pV, double *pT, double *pD); +void kernel_dlarfb3_rn_4_lib4(int kmax, double *pV, double *pT, double *pD); +void kernel_dlarfb2_rn_4_lib4(int kmax, double *pV, double *pT, double *pD); +void kernel_dlarfb1_rn_4_lib4(int kmax, double *pV, double *pT, double *pD); +void kernel_dlarfb4_rn_1_lib4(int kmax, double *pV, double *pT, double *pD); +void kernel_dlarfb3_rn_1_lib4(int kmax, double *pV, double *pT, double *pD); +void kernel_dlarfb2_rn_1_lib4(int kmax, double *pV, double *pT, double *pD); +void kernel_dlarfb1_rn_1_lib4(int kmax, double *pV, double *pT, double *pD); +void kernel_dlarfb4_rt_4_lib4(int kmax, double *pV, double *pT, double *pD); +void kernel_dlarfb3_rt_4_lib4(int kmax, double *pV, double *pT, double *pD); +void kernel_dlarfb2_rt_4_lib4(int kmax, double *pV, double *pT, double *pD); +void kernel_dlarfb1_rt_4_lib4(int kmax, double *pV, double *pT, double *pD); +void kernel_dlarfb4_rt_1_lib4(int kmax, double *pV, double *pT, double *pD); +void kernel_dlarfb3_rt_1_lib4(int kmax, double *pV, double *pT, double *pD); +void kernel_dlarfb2_rt_1_lib4(int kmax, double *pV, double *pT, double *pD); +void kernel_dlarfb1_rt_1_lib4(int kmax, double *pV, double *pT, double *pD); +void kernel_dgelqf_pd_dlarft12_12_lib4(int n, double *pD, int sdd, double *dD, double *pT); +void kernel_dgelqf_pd_dlarft4_8_lib4(int n, double *pD, int sdd, double *dD, double *pT); +void kernel_dgelqf_pd_dlarft4_4_lib4(int n, double *pD, double *dD, double *pT); +void kernel_dgelqf_pd_4_lib4(int n, double *pD, double *dD); +void kernel_dgelqf_pd_vs_lib4(int m, int n, int k, int offD, double *pD, int sdd, double *dD); +void kernel_dgelqf_pd_la_vs_lib4(int m, int n1, int k, int offD, double *pD, int sdd, double *dD, int offA, double *pA, int sda); +void kernel_dlarft_4_la_lib4(int n1, double *dD, double *pA, double *pT); +void kernel_dlarfb4_rn_12_la_lib4(int n1, double *pVA, double *pT, double *pD, int sdd, double *pA, int sda); +void kernel_dlarfb4_rn_8_la_lib4(int n1, double *pVA, double *pT, double *pD, int sdd, double *pA, int sda); +void kernel_dlarfb4_rn_4_la_lib4(int n1, double *pVA, double *pT, double *pD, double *pA); +void kernel_dlarfb4_rn_1_la_lib4(int n1, double *pVA, double *pT, double *pD, double *pA); +void kernel_dgelqf_pd_lla_vs_lib4(int m, int n0, int n1, int k, int offD, double *pD, int sdd, double *dD, int offL, double *pL, int sdl, int offA, double *pA, int sda); +void kernel_dlarft_4_lla_lib4(int n0, int n1, double *dD, double *pL, double *pA, double *pT); +void kernel_dlarfb4_rn_12_lla_lib4(int n0, int n1, double *pVL, double *pVA, double *pT, double *pD, int sdd, double *pL, int sdl, double *pA, int sda); +void kernel_dlarfb4_rn_8_lla_lib4(int n0, int n1, double *pVL, double *pVA, double *pT, double *pD, int sdd, double *pL, int sdl, double *pA, int sda); +void kernel_dlarfb4_rn_4_lla_lib4(int n0, int n1, double *pVL, double *pVA, double *pT, double *pD, double *pL, double *pA); +void kernel_dlarfb4_rn_1_lla_lib4(int n0, int n1, double *pVL, double *pVA, double *pT, double *pD, double *pL, double *pA); +// 4x2 +void kernel_dpotrf_nt_l_4x2_lib4(int k, double *A, double *B, double *C, double *D, double *inv_diag_D); +void kernel_dpotrf_nt_l_4x2_vs_lib4(int k, double *A, double *B, double *C, double *D, double *inv_diag_D, int km, int kn); +// 2x2 +void kernel_dpotrf_nt_l_2x2_lib4(int k, double *A, double *B, double *C, double *D, double *inv_diag_D); +void kernel_dpotrf_nt_l_2x2_vs_lib4(int k, double *A, double *B, double *C, double *D, double *inv_diag_D, int km, int kn); +// 12 +void kernel_dgetrf_pivot_12_lib4(int m, double *pA, int sda, double *inv_diag_A, int* ipiv); +void kernel_dgetrf_pivot_12_vs_lib4(int m, double *pA, int sda, double *inv_diag_A, int* ipiv, int n); +// 8 +void kernel_dgetrf_pivot_8_lib4(int m, double *pA, int sda, double *inv_diag_A, int* ipiv); +void kernel_dgetrf_pivot_8_vs_lib4(int m, double *pA, int sda, double *inv_diag_A, int* ipiv, int n); +// 4 +void kernel_dgetrf_pivot_4_lib4(int m, double *pA, int sda, double *inv_diag_A, int* ipiv); +void kernel_dgetrf_pivot_4_vs_lib4(int m, double *pA, int sda, double *inv_diag_A, int* ipiv, int n1); +// vector +void kernel_drowsw_lib4(int kmax, double *pA, double *pC); + + + +// merged routines +// 12x4 +void kernel_dgemm_dtrsm_nt_rl_inv_12x4_vs_lib4(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E, int km, int kn); +void kernel_dgemm_dtrsm_nt_rl_inv_12x4_lib4(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E); +void kernel_dsyrk_dpotrf_nt_l_12x4_vs_lib4(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn); +void kernel_dsyrk_dpotrf_nt_l_12x4_lib4(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *inv_diag_D); +// 4x12 +void kernel_dgemm_dtrsm_nt_rl_inv_4x12_vs_lib4(int kp, double *Ap, double *Bp, int sdbp, int km_, double *Am, double *Bm, int sdbm, double *C, double *D, double *E, int sde, double *inv_diag_E, int km, int kn); +// 8x8 +void kernel_dsyrk_dpotrf_nt_l_8x8_lib4(int kp, double *Ap, int sdap, double *Bp, int sdbp, int km_, double *Am, int sdam, double *Bm, int sdbm, double *C, int sdc, double *D, int sdd, double *inv_diag_D); +void kernel_dsyrk_dpotrf_nt_l_8x8_vs_lib4(int kp, double *Ap, int sdap, double *Bp, int sdbp, int km_, double *Am, int sdam, double *Bm, int sdbm, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn); +void kernel_dgemm_dtrsm_nt_rl_inv_8x8l_vs_lib4(int kp, double *Ap, int sdap, double *Bp, int sdb, int km_, double *Am, int sdam, double *Bm, int sdbm, double *C, int sdc, double *D, int sdd, double *E, int sde, double *inv_diag_E, int km, int kn); +void kernel_dgemm_dtrsm_nt_rl_inv_8x8u_vs_lib4(int kp, double *Ap, int sdap, double *Bp, int sdb, int km_, double *Am, int sdam, double *Bm, int sdbm, double *C, int sdc, double *D, int sdd, double *E, int sde, double *inv_diag_E, int km, int kn); +// 8x4 +void kernel_dgemm_dtrsm_nt_rl_inv_8x4_vs_lib4(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E, int km, int kn); +void kernel_dgemm_dtrsm_nt_rl_inv_8x4_lib4(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E); +void kernel_dsyrk_dpotrf_nt_l_8x4_lib4(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *inv_diag_D); +void kernel_dsyrk_dpotrf_nt_l_8x4_vs_lib4(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn); +// 4x8 +void kernel_dgemm_dtrsm_nt_rl_inv_4x8_vs_lib4(int kp, double *Ap, double *Bp, int sdbp, int km_, double *Am, double *Bm, int sdbm, double *C, double *D, double *E, int sde, double *inv_diag_E, int km, int kn); +// 4x4 +void kernel_dgemm_dtrsm_nt_rl_inv_4x4_lib4(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *E, double *inv_diag_E); +void kernel_dgemm_dtrsm_nt_rl_inv_4x4_vs_lib4(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *E, double *inv_diag_E, int km, int kn); +void kernel_dsyrk_dpotrf_nt_l_4x4_vs_lib4(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *inv_diag_D, int km, int kn); +void kernel_dsyrk_dpotrf_nt_l_4x4_lib4(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *inv_diag_D); +// 4x2 +void kernel_dgemm_dtrsm_nt_rl_inv_4x2_lib4(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *E, double *inv_diag_E); +void kernel_dgemm_dtrsm_nt_rl_inv_4x2_vs_lib4(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *E, double *inv_diag_E, int km, int kn); +void kernel_dsyrk_dpotrf_nt_l_4x2_vs_lib4(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *inv_diag_D, int km, int kn); +void kernel_dsyrk_dpotrf_nt_l_4x2_lib4(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *inv_diag_D); +// 2x2 +void kernel_dsyrk_dpotrf_nt_l_2x2_vs_lib4(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *inv_diag_D, int km, int kn); +void kernel_dsyrk_dpotrf_nt_l_2x2_lib4(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *inv_diag_D); + +/* + * + * Auxiliary routines + * + * cpsc copy and scale, scale + * cp copy + * add + * set and scale + * transpose and scale + * set and scale + * + */ + +// copy and scale +void kernel_dgecpsc_8_0_lib4(int tri, int kmax, double alpha, double *A0, int sda, double *B, int sdb); +void kernel_dgecpsc_8_1_lib4(int tri, int kmax, double alpha, double *A0, int sda, double *B, int sdb); +void kernel_dgecpsc_8_2_lib4(int tri, int kmax, double alpha, double *A0, int sda, double *B, int sdb); +void kernel_dgecpsc_8_3_lib4(int tri, int kmax, double alpha, double *A0, int sda, double *B, int sdb); + +void kernel_dgecpsc_4_0_lib4(int tri, int kmax, double alpha, double *A, double *B); +void kernel_dgecpsc_4_1_lib4(int tri, int kmax, double alpha, double *A0, int sda, double *B); +void kernel_dgecpsc_4_2_lib4(int tri, int kmax, double alpha, double *A0, int sda, double *B); +void kernel_dgecpsc_4_3_lib4(int tri, int kmax, double alpha, double *A0, int sda, double *B); + +void kernel_dgecpsc_3_0_lib4(int tri, int kmax, double alpha, double *A, double *B); +void kernel_dgecpsc_3_2_lib4(int tri, int kmax, double alpha, double *A0, int sda, double *B); +void kernel_dgecpsc_3_3_lib4(int tri, int kmax, double alpha, double *A0, int sda, double *B); + +void kernel_dgecpsc_2_0_lib4(int tri, int kmax, double alpha, double *A, double *B); +void kernel_dgecpsc_2_3_lib4(int tri, int kmax, double alpha, double *A0, int sda, double *B); + +void kernel_dgecpsc_1_0_lib4(int tri, int kmax, double alpha, double *A, double *B); + +// copy only +void kernel_dgecp_8_0_lib4(int tri, int kmax, double *A, int sda, double *B, int sdb); + +void kernel_dgecp_4_0_lib4(int tri, int kmax, double *A, double *B); + +// add +void kernel_dgead_8_0_lib4(int kmax, double alpha, double *A0, int sda, double *B, int sdb); +void kernel_dgead_8_1_lib4(int kmax, double alpha, double *A0, int sda, double *B, int sdb); +void kernel_dgead_8_2_lib4(int kmax, double alpha, double *A0, int sda, double *B, int sdb); +void kernel_dgead_8_3_lib4(int kmax, double alpha, double *A0, int sda, double *B, int sdb); +void kernel_dgead_4_0_lib4(int kmax, double alpha, double *A, double *B); +void kernel_dgead_4_1_lib4(int kmax, double alpha, double *A0, int sda, double *B); +void kernel_dgead_4_2_lib4(int kmax, double alpha, double *A0, int sda, double *B); +void kernel_dgead_4_3_lib4(int kmax, double alpha, double *A0, int sda, double *B); +void kernel_dgead_3_0_lib4(int kmax, double alpha, double *A, double *B); +void kernel_dgead_3_2_lib4(int kmax, double alpha, double *A0, int sda, double *B); +void kernel_dgead_3_3_lib4(int kmax, double alpha, double *A0, int sda, double *B); +void kernel_dgead_2_0_lib4(int kmax, double alpha, double *A, double *B); +void kernel_dgead_2_3_lib4(int kmax, double alpha, double *A0, int sda, double *B); +void kernel_dgead_1_0_lib4(int kmax, double alpha, double *A, double *B); + +// set +void kernel_dgeset_4_lib4(int kmax, double alpha, double *A); +void kernel_dtrset_4_lib4(int kmax, double alpha, double *A); + +// traspose +void kernel_dgetr_8_lib4(int tri, int kmax, int kna, double alpha, double *A, int sda, double *C, int sdc); +void kernel_dgetr_4_lib4(int tri, int kmax, int kna, double alpha, double *A, double *C, int sdc); +void kernel_dgetr_3_lib4(int tri, int kmax, int kna, double alpha, double *A, double *C, int sdc); +void kernel_dgetr_2_lib4(int tri, int kmax, int kna, double alpha, double *A, double *C, int sdc); +void kernel_dgetr_1_lib4(int tri, int kmax, int kna, double alpha, double *A, double *C, int sdc); +void kernel_dgetr_4_0_lib4(int m, double *A, int sda, double *B); + + + +// pack +// 12 +void kernel_dpack_nn_12_lib4(int kmax, double *A, int lda, double *B, int sdb); +void kernel_dpack_nn_12_vs_lib4(int kmax, double *A, int lda, double *B, int sdb, int m1); +void kernel_dpack_tt_12_lib4(int kmax, double *A, int lda, double *B, int sdb); +// 8 +void kernel_dpack_nn_8_lib4(int kmax, double *A, int lda, double *B, int sdb); +void kernel_dpack_nn_8_vs_lib4(int kmax, double *A, int lda, double *B, int sdb, int m1); +void kernel_dpack_tt_8_lib4(int kmax, double *A, int lda, double *B, int sdb); +// 4 +void kernel_dpack_nn_4_lib4(int kmax, double *A, int lda, double *B); +void kernel_dpack_nn_4_vs_lib4(int kmax, double *A, int lda, double *B, int m1); +void kernel_dpack_tn_4_p0_lib4(int kmax, double *A, int lda, double *B); +void kernel_dpack_tn_4_lib4(int kmax, double *A, int lda, double *B); +void kernel_dpack_tn_4_vs_lib4(int kmax, double *A, int lda, double *B, int m1); +void kernel_dpack_tt_4_lib4(int kmax, double *A, int lda, double *B, int sdb); +void kernel_dpack_tt_4_vs_lib4(int kmax, double *A, int lda, double *B, int sdb, int m1); +// unpack +// 12 +void kernel_dunpack_nn_12_lib4(int kmax, double *A, int sda, double *B, int ldb); +void kernel_dunpack_nn_12_vs_lib4(int kmax, double *A, int sda, double *B, int ldb, int m1); +void kernel_dunpack_tt_12_lib4(int kmax, double *A, int sda, double *B, int ldb); +// 8 +void kernel_dunpack_nn_8_lib4(int kmax, double *A, int sda, double *B, int ldb); +void kernel_dunpack_nn_8_vs_lib4(int kmax, double *A, int sda, double *B, int ldb, int m1); +void kernel_dunpack_tt_8_lib4(int kmax, double *A, int sda, double *B, int ldb); +// 4 +void kernel_dunpack_nn_4_lib4(int kmax, double *A, double *B, int ldb); +void kernel_dunpack_nn_4_vs_lib4(int kmax, double *A, double *B, int ldb, int m1); +void kernel_dunpack_nt_4_lib4(int kmax, double *A, double *B, int ldb); +void kernel_dunpack_nt_4_vs_lib4(int kmax, double *A, double *B, int ldb, int m1); +void kernel_dunpack_tt_4_lib4(int kmax, double *A, int sda, double *B, int ldb); + +// panel copy +// 12 +void kernel_dpacp_nn_12_lib4(int kmax, int offsetA, double *A, int sda, double *B, int sdb); +void kernel_dpacp_nn_12_vs_lib4(int kmax, int offsetA, double *A, int sda, double *B, int sdb, int m1); +// 8 +void kernel_dpacp_nn_8_lib4(int kmax, int offsetA, double *A, int sda, double *B, int sdb); +void kernel_dpacp_nn_8_vs_lib4(int kmax, int offsetA, double *A, int sda, double *B, int sdb, int m1); +// 4 +void kernel_dpacp_nt_4_lib4(int kmax, double *A, int offsetB, double *B, int sdb); +void kernel_dpacp_tn_4_lib4(int kmax, int offsetA, double *A, int sda, double *B); +void kernel_dpacp_nn_4_lib4(int kmax, int offsetA, double *A, int sda, double *B); +void kernel_dpacp_nn_4_vs_lib4(int kmax, int offsetA, double *A, int sda, double *B, int m1); + + + +/************************************************ +* BLAS API kernels +************************************************/ + +//#if defined(BLAS_API) + +// A, B panel-major bs=4; C, D column-major +// 12x4 +void kernel_dgemm_nt_12x4_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_12x4_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_nt_12x4_p0_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *A_p, double *B_p); +void kernel_dsyrk_nt_l_12x4_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dsyrk_nt_l_12x4_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dsyrk_nt_u_12x4_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dsyrk_nt_u_12x4_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_12x4_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_rl_12x4_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_one_12x4_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_rl_one_12x4_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_12x4_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_ru_12x4_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_one_12x4_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_ru_one_12x4_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrsm_nt_rl_inv_12x4_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, double *dE); +void kernel_dtrsm_nt_rl_inv_12x4_vs_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, double *dE, int m1, int n1); +void kernel_dtrsm_nt_rl_inv_12x4_lib44ccc(int kmax, double *A, int sda, double *B, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE); +void kernel_dtrsm_nt_rl_inv_12x4_vs_lib44ccc(int kmax, double *A, int sda, double *B, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nt_rl_one_12x4_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E); +void kernel_dtrsm_nt_rl_one_12x4_vs_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, int m1, int n1); +void kernel_dtrsm_nt_ru_inv_12x4_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, double *dE); +void kernel_dtrsm_nt_ru_inv_12x4_vs_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, double *dE, int m1, int n1); +void kernel_dtrsm_nt_ru_one_12x4_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E); +void kernel_dtrsm_nt_ru_one_12x4_vs_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, int m1, int n1); +void kernel_dpotrf_nt_l_12x4_lib44cc(int kmax, double *A, int sda, double *B, double *C, int ldc, double *D, int ldd, double *dD); +void kernel_dpotrf_nt_l_12x4_vs_lib44cc(int kmax, double *A, int sda, double *B, double *C, int ldc, double *D, int ldd, double *dD, int m1, int n1); +// 4x12 +void kernel_dgemm_nt_4x12_lib44cc(int kmax, double *alpha, double *A, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_4x12_vs_lib44cc(int kmax, double *alpha, double *A, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_4x12_tran_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nt_rl_4x12_tran_vs_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_one_4x12_tran_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nt_rl_one_4x12_tran_vs_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_4x12_tran_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nt_ru_4x12_tran_vs_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_one_4x12_tran_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nt_ru_one_4x12_tran_vs_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +// 8x8 +void kernel_dsyrk_nt_l_8x8_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dsyrk_nt_l_8x8_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dsyrk_nt_u_8x8_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dsyrk_nt_u_8x8_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dpotrf_nt_l_8x8_lib44cc(int kmax, double *A, int sda, double *B, int sdb, double *C, int ldc, double *D, int ldd, double *dD); +void kernel_dpotrf_nt_l_8x8_vs_lib44cc(int kmax, double *A, int sda, double *B, int sdb, double *C, int ldc, double *D, int ldd, double *dD, int m1, int n1); +// 8x4 +void kernel_dgemm_nt_8x4_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_8x4_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_nt_8x4_p0_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *A_p, double *B_p); +void kernel_dgemm_nt_8x4_p_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *A_p); +void kernel_dsyrk_nt_l_8x4_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dsyrk_nt_l_8x4_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dsyrk_nt_u_8x4_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dsyrk_nt_u_8x4_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_8x4_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_rl_8x4_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_one_8x4_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_rl_one_8x4_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_8x4_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_ru_8x4_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_one_8x4_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_ru_one_8x4_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrsm_nt_rl_inv_8x4_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, double *dE); +void kernel_dtrsm_nt_rl_inv_8x4_vs_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, double *dE, int m1, int n1); +void kernel_dtrsm_nt_rl_inv_8x4_lib44ccc(int kmax, double *A, int sda, double *B, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE); +void kernel_dtrsm_nt_rl_inv_8x4_vs_lib44ccc(int kmax, double *A, int sda, double *B, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nt_rl_one_8x4_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E); +void kernel_dtrsm_nt_rl_one_8x4_vs_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, int m1, int n1); +void kernel_dtrsm_nt_ru_inv_8x4_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, double *dE); +void kernel_dtrsm_nt_ru_inv_8x4_vs_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, double *dE, int m1, int n1); +void kernel_dtrsm_nt_ru_one_8x4_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E); +void kernel_dtrsm_nt_ru_one_8x4_vs_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, int m1, int n1); +void kernel_dpotrf_nt_l_8x4_lib44cc(int kmax, double *A, int sda, double *B, double *C, int ldc, double *D, int ldd, double *dD); +void kernel_dpotrf_nt_l_8x4_vs_lib44cc(int kmax, double *A, int sda, double *B, double *C, int ldc, double *D, int ldd, double *dD, int m1, int n1); +// 4x8 +void kernel_dgemm_nt_4x8_lib44cc(int kmax, double *alpha, double *A, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_4x8_vs_lib44cc(int kmax, double *alpha, double *A, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_4x8_tran_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nt_rl_4x8_tran_vs_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_one_4x8_tran_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nt_rl_one_4x8_tran_vs_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_4x8_tran_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nt_ru_4x8_tran_vs_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_one_4x8_tran_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nt_ru_one_4x8_tran_vs_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +// 4x4 +void kernel_dgemm_nt_4x4_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_4x4_vs_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dsyrk_nt_l_4x4_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dsyrk_nt_l_4x4_vs_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dsyrk_nt_u_4x4_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dsyrk_nt_u_4x4_vs_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_4x4_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_rl_4x4_vs_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_4x4_tran_lib444c(int kmax, double *alpha, double *A, double *B, double *beta, double *C, double *D, int ldd); +void kernel_dtrmm_nt_rl_4x4_tran_vs_lib444c(int kmax, double *alpha, double *A, double *B, double *beta, double *C, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_one_4x4_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_rl_one_4x4_vs_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_one_4x4_tran_lib444c(int kmax, double *alpha, double *A, double *B, double *beta, double *C, double *D, int ldd); +void kernel_dtrmm_nt_rl_one_4x4_tran_vs_lib444c(int kmax, double *alpha, double *A, double *B, double *beta, double *C, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_4x4_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_ru_4x4_vs_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_4x4_tran_lib444c(int kmax, double *alpha, double *A, double *B, double *beta, double *C, double *D, int ldd); +void kernel_dtrmm_nt_ru_4x4_tran_vs_lib444c(int kmax, double *alpha, double *A, double *B, double *beta, double *C, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_one_4x4_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_ru_one_4x4_vs_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_one_4x4_tran_lib444c(int kmax, double *alpha, double *A, double *B, double *beta, double *C, double *D, int ldd); +void kernel_dtrmm_nt_ru_one_4x4_tran_vs_lib444c(int kmax, double *alpha, double *A, double *B, double *beta, double *C, double *D, int ldd, int m1, int n1); +void kernel_dtrsm_nt_rl_inv_4x4_lib44cc4(int kmax, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, double *dE); +void kernel_dtrsm_nt_rl_inv_4x4_vs_lib44cc4(int kmax, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, double *dE, int m1, int n1); +void kernel_dtrsm_nt_rl_inv_4x4_lib44ccc(int kmax, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE); +void kernel_dtrsm_nt_rl_inv_4x4_vs_lib44ccc(int kmax, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nt_rl_one_4x4_lib44cc4(int kmax, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E); +void kernel_dtrsm_nt_rl_one_4x4_vs_lib44cc4(int kmax, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, int m1, int n1); +void kernel_dtrsm_nt_ru_inv_4x4_lib44cc4(int kmax, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, double *dE); +void kernel_dtrsm_nt_ru_inv_4x4_vs_lib44cc4(int kmax, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, double *dE, int m1, int n1); +void kernel_dtrsm_nt_ru_one_4x4_lib44cc4(int kmax, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E); +void kernel_dtrsm_nt_ru_one_4x4_vs_lib44cc4(int kmax, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, int m1, int n1); +void kernel_dpotrf_nt_l_4x4_lib44cc(int kmax, double *A, double *B, double *C, int ldc, double *D, int ldd, double *dD); +void kernel_dpotrf_nt_l_4x4_vs_lib44cc(int kmax, double *A, double *B, double *C, int ldc, double *D, int ldd, double *dD, int m1, int n1); +// 4x2 +void kernel_dgemm_nt_4x2_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd); + +// A panel-major bs=4; B, C, D column-major +// 12x4 +void kernel_dgemm_nn_12x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nn_12x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_nt_12x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_12x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dsyrk_nt_l_12x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dsyrk_nt_l_12x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_rl_12x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nn_rl_12x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_rl_one_12x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nn_rl_one_12x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_ru_12x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nn_ru_12x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_ru_one_12x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nn_ru_one_12x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_12x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_rl_12x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_one_12x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_rl_one_12x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_12x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_ru_12x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_one_12x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_ru_one_12x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrsm_nn_ll_one_12x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde); +void kernel_dtrsm_nn_ll_one_12x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1); +void kernel_dtrsm_nn_rl_inv_12x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE); +void kernel_dtrsm_nn_rl_inv_12x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nn_rl_one_12x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde); +void kernel_dtrsm_nn_rl_one_12x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1); +void kernel_dtrsm_nt_rl_inv_12x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE); +void kernel_dtrsm_nt_rl_inv_12x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nt_rl_one_12x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde); +void kernel_dtrsm_nt_rl_one_12x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1); +void kernel_dtrsm_nn_ru_inv_12x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE); +void kernel_dtrsm_nn_ru_inv_12x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nn_ru_one_12x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde); +void kernel_dtrsm_nn_ru_one_12x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1); +void kernel_dtrsm_nt_ru_inv_12x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE); +void kernel_dtrsm_nt_ru_inv_12x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nt_ru_one_12x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde); +void kernel_dtrsm_nt_ru_one_12x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1); +void kernel_dgetrf_nn_l_12x4_lib4ccc(int kmax, double *A, int sda, double *B, int ldb, double *C, int ldc, double *D, int ldd, double *diag); +void kernel_dgetrf_nn_l_12x4_vs_lib4ccc(int kmax, double *A, int sda, double *B, int ldb, double *C, int ldc, double *D, int ldd, double *diag, int m1, int n1); +void kernel_dgetrf_nn_m_12x4_lib4ccc(int kmax, double *A, int sda, double *B, int ldb, double *C, int ldc, double *D, int ldd, double *diag); +void kernel_dgetrf_nn_m_12x4_vs_lib4ccc(int kmax, double *A, int sda, double *B, int ldb, double *C, int ldc, double *D, int ldd, double *diag, int m1, int n1); +void kernel_dgetrf_nn_r_12x4_lib4ccc(int kmax, double *A, int sda, double *B, int ldb, double *C, int ldc, double *D, int ldd, double *diag); +void kernel_dgetrf_nn_r_12x4_vs_lib4ccc(int kmax, double *A, int sda, double *B, int ldb, double *C, int ldc, double *D, int ldd, double *diag, int m1, int n1); +// 4x12 +void kernel_dgemm_nn_4x12_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nn_4x12_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_nt_4x12_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_4x12_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_rl_4x12_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nn_rl_4x12_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_rl_one_4x12_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nn_rl_one_4x12_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_ru_4x12_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nn_ru_4x12_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_ru_one_4x12_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nn_ru_one_4x12_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_4x12_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nt_rl_4x12_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_one_4x12_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nt_rl_one_4x12_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_4x12_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nt_ru_4x12_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_one_4x12_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nt_ru_one_4x12_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +// 8x4 +void kernel_dgemm_nn_8x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nn_8x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_nt_8x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_8x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dsyrk_nt_l_8x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dsyrk_nt_l_8x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_rl_8x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nn_rl_8x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_rl_one_8x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nn_rl_one_8x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_ru_8x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nn_ru_8x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_ru_one_8x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nn_ru_one_8x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_8x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_rl_8x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_one_8x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_rl_one_8x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_8x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_ru_8x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_one_8x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_ru_one_8x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrsm_nn_ll_one_8x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde); +void kernel_dtrsm_nn_ll_one_8x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1); +void kernel_dtrsm_nn_rl_inv_8x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE); +void kernel_dtrsm_nn_rl_inv_8x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nn_rl_one_8x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde); +void kernel_dtrsm_nn_rl_one_8x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1); +void kernel_dtrsm_nt_rl_inv_8x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE); +void kernel_dtrsm_nt_rl_inv_8x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nt_rl_one_8x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde); +void kernel_dtrsm_nt_rl_one_8x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1); +void kernel_dtrsm_nn_ru_inv_8x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE); +void kernel_dtrsm_nn_ru_inv_8x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nn_ru_one_8x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde); +void kernel_dtrsm_nn_ru_one_8x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1); +void kernel_dtrsm_nt_ru_inv_8x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE); +void kernel_dtrsm_nt_ru_inv_8x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nt_ru_one_8x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde); +void kernel_dtrsm_nt_ru_one_8x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1); +void kernel_dgetrf_nn_l_8x4_lib4ccc(int kmax, double *A, int sda, double *B, int ldb, double *C, int ldc, double *D, int ldd, double *diag); +void kernel_dgetrf_nn_l_8x4_vs_lib4ccc(int kmax, double *A, int sda, double *B, int ldb, double *C, int ldc, double *D, int ldd, double *diag, int m1, int n1); +void kernel_dgetrf_nn_r_8x4_lib4ccc(int kmax, double *A, int sda, double *B, int ldb, double *C, int ldc, double *D, int ldd, double *diag); +void kernel_dgetrf_nn_r_8x4_vs_lib4ccc(int kmax, double *A, int sda, double *B, int ldb, double *C, int ldc, double *D, int ldd, double *diag, int m1, int n1); +// 4x8 +void kernel_dgemm_nn_4x8_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nn_4x8_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_nt_4x8_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_4x8_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_rl_4x8_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nn_rl_4x8_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_rl_one_4x8_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nn_rl_one_4x8_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_ru_4x8_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nn_ru_4x8_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_ru_one_4x8_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nn_ru_one_4x8_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_4x8_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nt_rl_4x8_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_one_4x8_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nt_rl_one_4x8_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_4x8_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nt_ru_4x8_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_one_4x8_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nt_ru_one_4x8_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +// 4x4 +void kernel_dgemm_nn_4x4_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nn_4x4_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_nt_4x4_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_4x4_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dsyrk_nt_l_4x4_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dsyrk_nt_l_4x4_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_rl_4x4_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nn_rl_4x4_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_rl_4x4_tran_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd); +void kernel_dtrmm_nn_rl_4x4_tran_vs_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_rl_one_4x4_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nn_rl_one_4x4_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_rl_one_4x4_tran_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd); +void kernel_dtrmm_nn_rl_one_4x4_tran_vs_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_ru_4x4_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nn_ru_4x4_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_ru_4x4_tran_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd); +void kernel_dtrmm_nn_ru_4x4_tran_vs_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_ru_one_4x4_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nn_ru_one_4x4_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_ru_one_4x4_tran_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd); +void kernel_dtrmm_nn_ru_one_4x4_tran_vs_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_4x4_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_rl_4x4_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_4x4_tran_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd); +void kernel_dtrmm_nt_rl_4x4_tran_vs_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_one_4x4_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_rl_one_4x4_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_one_4x4_tran_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd); +void kernel_dtrmm_nt_rl_one_4x4_tran_vs_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_4x4_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_ru_4x4_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_4x4_tran_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd); +void kernel_dtrmm_nt_ru_4x4_tran_vs_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_one_4x4_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_ru_one_4x4_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_one_4x4_tran_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd); +void kernel_dtrmm_nt_ru_one_4x4_tran_vs_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd, int m1, int n1); +void kernel_dtrsm_nn_ll_one_4x4_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde); +void kernel_dtrsm_nn_ll_one_4x4_vs_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1); +void kernel_dtrsm_nn_rl_inv_4x4_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE); +void kernel_dtrsm_nn_rl_inv_4x4_vs_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nn_rl_one_4x4_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde); +void kernel_dtrsm_nn_rl_one_4x4_vs_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1); +void kernel_dtrsm_nt_rl_inv_4x4_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE); +void kernel_dtrsm_nt_rl_inv_4x4_vs_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nt_rl_one_4x4_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde); +void kernel_dtrsm_nt_rl_one_4x4_vs_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1); +void kernel_dtrsm_nn_ru_inv_4x4_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE); +void kernel_dtrsm_nn_ru_inv_4x4_vs_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nn_ru_one_4x4_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde); +void kernel_dtrsm_nn_ru_one_4x4_vs_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1); +void kernel_dtrsm_nt_ru_inv_4x4_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE); +void kernel_dtrsm_nt_ru_inv_4x4_vs_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nt_ru_one_4x4_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde); +void kernel_dtrsm_nt_ru_one_4x4_vs_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1); +void kernel_dgetrf_nn_4x4_lib4ccc(int kmax, double *A, double *B, int ldb, double *C, int ldc, double *D, int ldd, double *diag); +void kernel_dgetrf_nn_4x4_vs_lib4ccc(int kmax, double *A, double *B, int ldb, double *C, int ldc, double *D, int ldd, double *diag, int m1, int n1); + +// B panel-major bs=4; A, C, D column-major +// 12x4 +void kernel_dgemm_nt_12x4_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_12x4_vs_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_tt_12x4_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_tt_12x4_vs_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +// 4x12 +void kernel_dgemm_nt_4x12_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_4x12_vs_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_tt_4x12_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_tt_4x12_vs_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +// 8x4 +void kernel_dgemm_nt_8x4_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_8x4_vs_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_tt_8x4_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_tt_8x4_vs_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +// 4x8 +void kernel_dgemm_nt_4x8_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_4x8_vs_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_tt_4x8_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_tt_4x8_vs_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +// 4x4 +void kernel_dgemm_nt_4x4_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_4x4_vs_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_tt_4x4_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_tt_4x4_vs_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); + +// A, C, D panel-major; B, E column-major +// TODO merge with above +// 12x4 +void kernel_dtrsm_nn_rl_inv_12x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE); +void kernel_dtrsm_nn_rl_inv_12x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nn_rl_one_12x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde); +void kernel_dtrsm_nn_rl_one_12x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, int m1, int n1); +void kernel_dtrsm_nn_ru_inv_12x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE); +void kernel_dtrsm_nn_ru_inv_12x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nn_ru_one_12x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde); +void kernel_dtrsm_nn_ru_one_12x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, int m1, int n1); +void kernel_dtrsm_nt_rl_inv_12x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE); +void kernel_dtrsm_nt_rl_inv_12x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nt_rl_one_12x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde); +void kernel_dtrsm_nt_rl_one_12x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, int m1, int n1); +void kernel_dtrsm_nt_ru_inv_12x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE); +void kernel_dtrsm_nt_ru_inv_12x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nt_ru_one_12x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde); +void kernel_dtrsm_nt_ru_one_12x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, int m1, int n1); +// 8x4 +void kernel_dtrsm_nn_rl_inv_8x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE); +void kernel_dtrsm_nn_rl_inv_8x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nn_rl_one_8x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde); +void kernel_dtrsm_nn_rl_one_8x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, int m1, int n1); +void kernel_dtrsm_nn_ru_inv_8x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE); +void kernel_dtrsm_nn_ru_inv_8x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nn_ru_one_8x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde); +void kernel_dtrsm_nn_ru_one_8x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, int m1, int n1); +void kernel_dtrsm_nt_rl_inv_8x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE); +void kernel_dtrsm_nt_rl_inv_8x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nt_rl_one_8x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde); +void kernel_dtrsm_nt_rl_one_8x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, int m1, int n1); +void kernel_dtrsm_nt_ru_inv_8x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE); +void kernel_dtrsm_nt_ru_inv_8x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nt_ru_one_8x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde); +void kernel_dtrsm_nt_ru_one_8x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, int m1, int n1); +// 4x4 +void kernel_dtrsm_nn_rl_inv_4x4_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde, double *dE); +void kernel_dtrsm_nn_rl_inv_4x4_vs_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nn_rl_one_4x4_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde); +void kernel_dtrsm_nn_rl_one_4x4_vs_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde, int m1, int n1); +void kernel_dtrsm_nn_ru_inv_4x4_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde, double *dE); +void kernel_dtrsm_nn_ru_inv_4x4_vs_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nn_ru_one_4x4_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde); +void kernel_dtrsm_nn_ru_one_4x4_vs_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde, int m1, int n1); +void kernel_dtrsm_nt_rl_inv_4x4_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde, double *dE); +void kernel_dtrsm_nt_rl_inv_4x4_vs_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nt_rl_one_4x4_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde); +void kernel_dtrsm_nt_rl_one_4x4_vs_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde, int m1, int n1); +void kernel_dtrsm_nt_ru_inv_4x4_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde, double *dE); +void kernel_dtrsm_nt_ru_inv_4x4_vs_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nt_ru_one_4x4_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde); +void kernel_dtrsm_nt_ru_one_4x4_vs_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde, int m1, int n1); + +// A, B, C, D column-major +// 12x4 +void kernel_dgemm_nn_12x4_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nn_12x4_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_nt_12x4_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_12x4_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_tt_12x4_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_tt_12x4_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +// 4x12 +void kernel_dgemm_nn_4x12_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nn_4x12_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_nt_4x12_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_4x12_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_tt_4x12_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_tt_4x12_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +// 8x4 +void kernel_dgemm_nn_8x4_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nn_8x4_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_nt_8x4_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_8x4_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_tt_8x4_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_tt_8x4_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +// 4x8 +void kernel_dgemm_nn_4x8_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nn_4x8_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_nt_4x8_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_4x8_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_tt_4x8_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_tt_4x8_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +// 4x4 +void kernel_dgemm_nn_4x4_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nn_4x4_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_nt_4x4_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_4x4_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_tt_4x4_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_tt_4x4_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); + +// A column-major +// 12 +void kernel_dgetrf_pivot_12_lib(int m, double *pA, int lda, double *inv_diag_A, int* ipiv); +void kernel_dgetrf_pivot_12_vs_lib(int m, double *pA, int lda, double *inv_diag_A, int* ipiv, int n); +// 8 +void kernel_dgetrf_pivot_8_lib(int m, double *pA, int lda, double *inv_diag_A, int* ipiv); +void kernel_dgetrf_pivot_8_vs_lib(int m, double *pA, int lda, double *inv_diag_A, int* ipiv, int n); +// 4 +void kernel_dgetrf_pivot_4_lib(int m, double *pA, int lda, double *inv_diag_A, int* ipiv); +void kernel_dgetrf_pivot_4_vs_lib(int m, double *pA, int lda, double *inv_diag_A, int* ipiv, int n); + +// vector +void kernel_ddot_11_lib(int n, double *x, double *y, double *res); +void kernel_daxpy_11_lib(int n, double *alpha, double *x, double *y); +void kernel_drowsw_lib(int kmax, double *pA, int lda, double *pC, int ldc); + +//#endif // BLAS_API + + + +// larger kernels +// 12 +void kernel_dgemm_nt_12xn_p0_lib44cc(int n, int k, double *alpha, double *A, int sda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd, double *A_p, double *B_p); +void kernel_dgemm_nt_12xn_pl_lib44cc(int n, int k, double *alpha, double *A, int sda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd, double *A_p, double *B_p); +// 8 +void kernel_dgemm_nt_8xn_p0_lib44cc(int n, int k, double *alpha, double *A, int sda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd, double *A_p, double *B_p); + + + +// aux +void kernel_dvecld_inc1(int kmax, double *x); +void kernel_dveccp_inc1(int kmax, double *x, double *y); + + + + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_D_KERNEL_H_ diff --git a/phonelibs/acados/include/blasfeo/include/blasfeo_i_aux_ext_dep.h b/phonelibs/acados/include/blasfeo/include/blasfeo_i_aux_ext_dep.h new file mode 100644 index 0000000000..74c3fb5c0a --- /dev/null +++ b/phonelibs/acados/include/blasfeo/include/blasfeo_i_aux_ext_dep.h @@ -0,0 +1,69 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_I_AUX_EXT_DEP_H_ +#define BLASFEO_I_AUX_EXT_DEP_H_ + + + +#include "blasfeo_target.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +#ifdef EXT_DEP + +// i_aux_extern_depend_lib +void int_zeros(int **pA, int row, int col); +void int_zeros_align(int **pA, int row, int col); +void int_free(int *pA); +void int_free_align(int *pA); +void int_print_mat(int row, int col, int *A, int lda); +int int_print_to_string_mat(char **buf_out, int row, int col, int *A, int lda); + +#endif // EXT_DEP + + + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_I_AUX_EXT_DEP_H_ diff --git a/phonelibs/acados/include/blasfeo/include/blasfeo_m_aux.h b/phonelibs/acados/include/blasfeo/include/blasfeo_m_aux.h new file mode 100644 index 0000000000..6248853e29 --- /dev/null +++ b/phonelibs/acados/include/blasfeo/include/blasfeo_m_aux.h @@ -0,0 +1,57 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_M_AUX_H_ +#define BLASFEO_M_AUX_H_ + +#include "blasfeo_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + + + +void blasfeo_cvt_d2s_vec(int m, struct blasfeo_dvec *vd, int vdi, struct blasfeo_svec *vs, int vsi); +void blasfeo_cvt_s2d_vec(int m, struct blasfeo_svec *vs, int vsi, struct blasfeo_dvec *vd, int vdi); +void blasfeo_cvt_d2s_mat(int m, int n, struct blasfeo_dmat *Md, int mid, int nid, struct blasfeo_smat *Ms, int mis, int nis); +void blasfeo_cvt_s2d_mat(int m, int n, struct blasfeo_smat *Ms, int mis, int nis, struct blasfeo_dmat *Md, int mid, int nid); + + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_M_AUX_H_ diff --git a/phonelibs/acados/include/blasfeo/include/blasfeo_memory.h b/phonelibs/acados/include/blasfeo/include/blasfeo_memory.h new file mode 100644 index 0000000000..da4e7fa090 --- /dev/null +++ b/phonelibs/acados/include/blasfeo/include/blasfeo_memory.h @@ -0,0 +1,62 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2020 by Gianluca Frison. * +* All rights reserved. * +* * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + +#ifndef BLASFEO_MEMORY_H_ +#define BLASFEO_MEMORY_H_ + +#ifdef __cplusplus +extern "C" { +#endif + + + +// +int blasfeo_is_init(); +// +void blasfeo_init(); +// +void blasfeo_quit(); +// +void *blasfeo_get_buffer(); + + + + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_MEMORY_H_ diff --git a/phonelibs/acados/include/blasfeo/include/blasfeo_naming.h b/phonelibs/acados/include/blasfeo/include/blasfeo_naming.h new file mode 100644 index 0000000000..c289443b17 --- /dev/null +++ b/phonelibs/acados/include/blasfeo/include/blasfeo_naming.h @@ -0,0 +1,77 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + +/* + * ----------- Naming conventions + * + * (precision)(data) + * + * 1) d(double) + * s(single) + * + * 2) ge(general) + * tr(triangular) + * vec(vector) + * row(row) + * col(column) + * dia(diagonal) + * + * 3) se(set) + * cp(copy) + * sc(scale) + * ad(add) + * tr(transpose) + * in(insert) + * ex(extract) + * pe(premute) + * sw(swap) + * + * f(factorization) + * + * lqf(LQ factorization) + * qrf (factorization) + * trf (LU factorization using partial pivoting with row interchanges.) + * + * 4) _l(lower) / _u(upper) + * _lib8 (hp implementation, 8 rows kernel) + * _lib4 (hp implementation, 4 rows kernel) + * _lib0 (hp interface with reference implentation) + * _lib (reference implementation) + * _libref (reference implementation with dedicated namespace) + * + * 5) _sp(sparse) + * _exp(exponential format) + */ diff --git a/phonelibs/acados/include/blasfeo/include/blasfeo_processor_features.h b/phonelibs/acados/include/blasfeo/include/blasfeo_processor_features.h new file mode 100644 index 0000000000..65a0c7e80a --- /dev/null +++ b/phonelibs/acados/include/blasfeo/include/blasfeo_processor_features.h @@ -0,0 +1,88 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Ian McInerney * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_PROCESSOR_FEATURES_H_ +#define BLASFEO_PROCESSOR_FEATURES_H_ + +/** + * Flags to indicate the different processor features + */ +enum +{ + // x86-64 CPU features + BLASFEO_PROCESSOR_FEATURE_AVX = 0x0001, /// AVX instruction set + BLASFEO_PROCESSOR_FEATURE_AVX2 = 0x0002, /// AVX2 instruction set + BLASFEO_PROCESSOR_FEATURE_FMA = 0x0004, /// FMA instruction set + BLASFEO_PROCESSOR_FEATURE_SSE3 = 0x0008, /// SSE3 instruction set + + // ARM CPU features + BLASFEO_PROCESSOR_FEATURE_VFPv3 = 0x0100, /// VFPv3 instruction set + BLASFEO_PROCESSOR_FEATURE_NEON = 0x0100, /// NEON instruction set + BLASFEO_PROCESSOR_FEATURE_VFPv4 = 0x0100, /// VFPv4 instruction set + BLASFEO_PROCESSOR_FEATURE_NEONv2 = 0x0100, /// NEONv2 instruction set +} BLASFEO_PROCESSOR_FEATURES; + +/** + * Test the features that this processor provides against what the library was compiled with. + * + * @param features - Pointer to an integer to store the supported feature set (using the flags in the BLASFEO_PROCESSOR_FEATURES enum) + * @return 0 if current processor doesn't support all features required for this library, 1 otherwise + */ +int blasfeo_processor_cpu_features( int* features ); + +/** + * Test the features that this processor provides against what the library was compiled with. + * + * @param features - Pointer to an integer to store the supported feature set (using the flags in the BLASFEO_PROCESSOR_FEATURES enum) + * @return 0 if current processor doesn't support all features required for this library, 1 otherwise + */ +void blasfeo_processor_library_features( int* features ); + +/** + * Create a string listing the features the current processor supports. + * + * @param features - Flags from the BLASFEO_PROCESSOR_FEATURES enum indicating the features supported + * @param featureString - Character array to store the feature string in + */ +void blasfeo_processor_feature_string( int features, char* featureString ); + +/** + * Get a string listing the processor features that this library version needs to run. + * + * @param featureString - Character array to store the feature string in + */ +void blasfeo_processor_library_string( char* featureString ); + +#endif // BLASFEO_PROCESSOR_FEATURES_H_ diff --git a/phonelibs/acados/include/blasfeo/include/blasfeo_s_aux.h b/phonelibs/acados/include/blasfeo/include/blasfeo_s_aux.h new file mode 100644 index 0000000000..539268849c --- /dev/null +++ b/phonelibs/acados/include/blasfeo/include/blasfeo_s_aux.h @@ -0,0 +1,166 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_S_AUX_H_ +#define BLASFEO_S_AUX_H_ + + + +#include + +#include "blasfeo_s_aux_old.h" +#include "blasfeo_common.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + +/************************************************ +* d_aux_lib.c +************************************************/ + +// returns the memory size (in bytes) needed for a smat +size_t blasfeo_memsize_smat(int m, int n); +size_t blasfeo_memsize_smat_ps(int ps, int m, int n); +// returns the memory size (in bytes) needed for the diagonal of a smat +size_t blasfeo_memsize_diag_smat(int m, int n); +// returns the memory size (in bytes) needed for a svec +size_t blasfeo_memsize_svec(int m); +// create a strmat for a matrix of size m*n by using memory passed by a pointer (pointer is not updated) +void blasfeo_create_smat(int m, int n, struct blasfeo_smat *sA, void *memory); +void blasfeo_create_smat_ps(int ps, int m, int n, struct blasfeo_smat *sA, void *memory); +// create a strvec for a vector of size m by using memory passed by a pointer (pointer is not updated) +void blasfeo_create_svec(int m, struct blasfeo_svec *sA, void *memory); +void blasfeo_pack_smat(int m, int n, float *A, int lda, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_pack_l_smat(int m, int n, float *A, int lda, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_pack_u_smat(int m, int n, float *A, int lda, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_pack_tran_smat(int m, int n, float *A, int lda, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_pack_svec(int m, float *x, int xi, struct blasfeo_svec *sa, int ai); +void blasfeo_unpack_smat(int m, int n, struct blasfeo_smat *sA, int ai, int aj, float *A, int lda); +void blasfeo_unpack_tran_smat(int m, int n, struct blasfeo_smat *sA, int ai, int aj, float *A, int lda); +void blasfeo_unpack_svec(int m, struct blasfeo_svec *sa, int ai, float *x, int xi); +//void s_cast_mat2strmat(float *A, struct blasfeo_smat *sA); +//void s_cast_diag_mat2strmat(float *dA, struct blasfeo_smat *sA); +//void s_cast_vec2vecmat(float *a, struct blasfeo_svec *sa); + +// ge +void blasfeo_sgese(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_sgecpsc(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +void blasfeo_sgecp(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +void blasfeo_sgesc(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_sgein1(float a, struct blasfeo_smat *sA, int ai, int aj); +float blasfeo_sgeex1(struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_sgead(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +void blasfeo_sgetr(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +// tr +void blasfeo_strcp_l(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +void blasfeo_strtr_l(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +void blasfeo_strtr_u(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +// dia +void blasfeo_sdiare(int kmax, float alpha, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_sdiaex(int kmax, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi); +void blasfeo_sdiain(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_sdiain_sp(int kmax, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_smat *sD, int di, int dj); +void blasfeo_sdiaex_sp(int kmax, float alpha, int *idx, struct blasfeo_smat *sD, int di, int dj, struct blasfeo_svec *sx, int xi); +void blasfeo_sdiaad(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_sdiaad_sp(int kmax, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_smat *sD, int di, int dj); +void blasfeo_sdiaadin_sp(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sy, int yi, int *idx, struct blasfeo_smat *sD, int di, int dj); +// row +void blasfeo_srowin(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_srowex(int kmax, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi); +void blasfeo_srowad(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_srowad_sp(int kmax, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_smat *sD, int di, int dj); +void blasfeo_srowsw(int kmax, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +void blasfeo_srowpe(int kmax, int *ipiv, struct blasfeo_smat *sA); +void blasfeo_srowpei(int kmax, int *ipiv, struct blasfeo_smat *sA); +// col +void blasfeo_scolex(int kmax, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi); +void blasfeo_scolin(int kmax, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_scolad(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_scolsc(int kmax, float alpha, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_scolsw(int kmax, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +void blasfeo_scolpe(int kmax, int *ipiv, struct blasfeo_smat *sA); +void blasfeo_scolpei(int kmax, int *ipiv, struct blasfeo_smat *sA); +// vec +void blasfeo_svecse(int m, float alpha, struct blasfeo_svec *sx, int xi); +void blasfeo_sveccp(int m, struct blasfeo_svec *sa, int ai, struct blasfeo_svec *sc, int ci); +void blasfeo_svecsc(int m, float alpha, struct blasfeo_svec *sa, int ai); +void blasfeo_sveccpsc(int m, float alpha, struct blasfeo_svec *sa, int ai, struct blasfeo_svec *sc, int ci); +void blasfeo_svecad(int m, float alpha, struct blasfeo_svec *sa, int ai, struct blasfeo_svec *sc, int ci); +void blasfeo_svecin1(float a, struct blasfeo_svec *sx, int xi); +float blasfeo_svecex1(struct blasfeo_svec *sx, int xi); +void blasfeo_svecad_sp(int m, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_svec *sz, int zi); +void blasfeo_svecin_sp(int m, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_svec *sz, int zi); +void blasfeo_svecex_sp(int m, float alpha, int *idx, struct blasfeo_svec *sx, int x, struct blasfeo_svec *sz, int zi); +void blasfeo_sveccl(int m, struct blasfeo_svec *sxm, int xim, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sxp, int xip, struct blasfeo_svec *sz, int zi); +void blasfeo_sveccl_mask(int m, struct blasfeo_svec *sxm, int xim, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sxp, int xip, struct blasfeo_svec *sz, int zi, struct blasfeo_svec *sm, int mi); +void blasfeo_svecze(int m, struct blasfeo_svec *sm, int mi, struct blasfeo_svec *sv, int vi, struct blasfeo_svec *se, int ei); +void blasfeo_svecnrm_inf(int m, struct blasfeo_svec *sx, int xi, float *ptr_norm); +void blasfeo_svecnrm_2(int m, struct blasfeo_svec *sx, int xi, float *ptr_norm); +void blasfeo_svecpe(int kmax, int *ipiv, struct blasfeo_svec *sx, int xi); +void blasfeo_svecpei(int kmax, int *ipiv, struct blasfeo_svec *sx, int xi); + + + +/* +* Explicitly panel-major matrix format +*/ + +// returns the memory size (in bytes) needed for a dmat +size_t blasfeo_pm_memsize_smat(int ps, int m, int n); +// create a strmat for a matrix of size m*n by using memory passed by a pointer (pointer is not updated) +void blasfeo_pm_create_smat(int ps, int m, int n, struct blasfeo_pm_smat *sA, void *memory); + + + +/* +* Explicitly column-major matrix format +*/ + +// returns the memory size (in bytes) needed for a dmat +size_t blasfeo_cm_memsize_smat(int m, int n); +// create a strmat for a matrix of size m*n by using memory passed by a pointer (pointer is not updated) +void blasfeo_cm_create_smat(int m, int n, struct blasfeo_pm_smat *sA, void *memory); + + + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_S_AUX_H_ diff --git a/phonelibs/acados/include/blasfeo/include/blasfeo_s_aux_ext_dep.h b/phonelibs/acados/include/blasfeo/include/blasfeo_s_aux_ext_dep.h new file mode 100644 index 0000000000..5209d20d37 --- /dev/null +++ b/phonelibs/acados/include/blasfeo/include/blasfeo_s_aux_ext_dep.h @@ -0,0 +1,141 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_S_AUX_EXT_DEP_H_ +#define BLASFEO_S_AUX_EXT_DEP_H_ + + + +#include + + + +#include "blasfeo_common.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +#ifdef EXT_DEP + +/************************************************ +* s_aux_extern_depend_lib.c +************************************************/ + +/* column-major matrices */ + +// dynamically allocate row*col floats of memory and set accordingly a pointer to float; set allocated memory to zero +void s_zeros(float **pA, int row, int col); +// dynamically allocate row*col floats of memory aligned to 64-byte boundaries and set accordingly a pointer to float; set allocated memory to zero +void s_zeros_align(float **pA, int row, int col); +// dynamically allocate size bytes of memory aligned to 64-byte boundaries and set accordingly a pointer to float; set allocated memory to zero +void s_zeros_align_bytes(float **pA, int size); +// free the memory allocated by d_zeros +void s_free(float *pA); +// free the memory allocated by d_zeros_align or d_zeros_align_bytes +void s_free_align(float *pA); +// print a column-major matrix +void s_print_mat(int m, int n, float *A, int lda); +// print the transposed of a column-major matrix +void s_print_tran_mat(int row, int col, float *A, int lda); +// print to file a column-major matrix +void s_print_to_file_mat(FILE *file, int row, int col, float *A, int lda); +// print to file a column-major matrix in exponential format +void s_print_to_file_exp_mat(FILE *file, int row, int col, float *A, int lda); +// print to string a column-major matrix +void s_print_to_string_mat(char **buf_out, int row, int col, float *A, int lda); +// print to file the transposed of a column-major matrix +void s_print_tran_to_file_mat(FILE *file, int row, int col, float *A, int lda); +// print to file the transposed of a column-major matrix in exponential format +void s_print_tran_to_file_exp_mat(FILE *file, int row, int col, float *A, int lda); +// print in exponential notation a column-major matrix +void s_print_exp_mat(int m, int n, float *A, int lda); +// print in exponential notation the transposed of a column-major matrix +void s_print_exp_tran_mat(int row, int col, float *A, int lda); + +/* strmat and strvec */ + +// create a strmat for a matrix of size m*n by dynamically allocating memory +void blasfeo_allocate_smat(int m, int n, struct blasfeo_smat *sA); +// create a strvec for a vector of size m by dynamically allocating memory +void blasfeo_allocate_svec(int m, struct blasfeo_svec *sa); +// free the memory allocated by blasfeo_allocate_dmat +void blasfeo_free_smat(struct blasfeo_smat *sA); +// free the memory allocated by blasfeo_allocate_dvec +void blasfeo_free_svec(struct blasfeo_svec *sa); +// print a strmat +void blasfeo_print_smat(int m, int n, struct blasfeo_smat *sA, int ai, int aj); +// print in exponential notation a strmat +void blasfeo_print_exp_smat(int m, int n, struct blasfeo_smat *sA, int ai, int aj); +// print to file a strmat +void blasfeo_print_to_file_smat(FILE *file, int m, int n, struct blasfeo_smat *sA, int ai, int aj); +// print to file a strmat in exponential format +void blasfeo_print_to_file_exp_smat(FILE *file, int m, int n, struct blasfeo_smat *sA, int ai, int aj); +// print to string a strmat +void blasfeo_print_to_string_smat(char **buf_out, int m, int n, struct blasfeo_smat *sA, int ai, int aj); +// print the transpose of a strmat +void blasfeo_print_tran_smat(int m, int n, struct blasfeo_smat *sA, int ai, int aj); +// print a strvec +void blasfeo_print_svec(int m, struct blasfeo_svec *sa, int ai); +// print in exponential notation a strvec +void blasfeo_print_exp_svec(int m, struct blasfeo_svec *sa, int ai); +// print to file a strvec +void blasfeo_print_to_file_svec(FILE *file, int m, struct blasfeo_svec *sa, int ai); +// print to string a strvec +void blasfeo_print_to_string_svec(char **buf_out, int m, struct blasfeo_svec *sa, int ai); +// print the transposed of a strvec +void blasfeo_print_tran_svec(int m, struct blasfeo_svec *sa, int ai); +// print in exponential notation the transposed of a strvec +void blasfeo_print_exp_tran_svec(int m, struct blasfeo_svec *sa, int ai); +// print to file the transposed of a strvec +void blasfeo_print_to_file_tran_svec(FILE *file, int m, struct blasfeo_svec *sa, int ai); +// print to string the transposed of a strvec +void blasfeo_print_to_string_tran_svec(char **buf_out, int m, struct blasfeo_svec *sa, int ai); + +#endif // EXT_DEP + + + +#ifdef __cplusplus +} +#endif + + + +#endif // BLASFEO_S_AUX_EXT_DEP_H_ diff --git a/phonelibs/acados/include/blasfeo/include/blasfeo_s_aux_ext_dep_ref.h b/phonelibs/acados/include/blasfeo/include/blasfeo_s_aux_ext_dep_ref.h new file mode 100644 index 0000000000..6640e20a40 --- /dev/null +++ b/phonelibs/acados/include/blasfeo/include/blasfeo_s_aux_ext_dep_ref.h @@ -0,0 +1,82 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_S_AUX_EXT_DEP_REF_H_ +#define BLASFEO_S_AUX_EXT_DEP_REF_H_ + +#if defined(EXT_DEP) + + + +#include + +#include "blasfeo_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +// expose reference BLASFEO for testing +// see blasfeo_s_aux_exp_dep.h for help + +void blasfeo_print_smat_ref(int m, int n, struct blasfeo_smat_ref *sA, int ai, int aj); +void blasfeo_allocate_smat_ref(int m, int n, struct blasfeo_smat_ref *sA); +void blasfeo_allocate_svec_ref(int m, struct blasfeo_svec_ref *sa); +void blasfeo_free_smat_ref(struct blasfeo_smat_ref *sA); +void blasfeo_free_svec_ref(struct blasfeo_svec_ref *sa); +void blasfeo_print_smat_ref(int m, int n, struct blasfeo_smat_ref *sA, int ai, int aj); +void blasfeo_print_exp_smat_ref(int m, int n, struct blasfeo_smat_ref *sA, int ai, int aj); +void blasfeo_print_to_file_smat_ref(FILE *file, int m, int n, struct blasfeo_smat_ref *sA, int ai, int aj); +void blasfeo_print_to_file_exp_smat_ref(FILE *file, int m, int n, struct blasfeo_smat_ref *sA, int ai, int aj); +void blasfeo_print_to_string_smat_ref(char **buf_out, int m, int n, struct blasfeo_smat_ref *sA, int ai, int aj); +void blasfeo_print_svec(int m, struct blasfeo_svec *sa, int ai); +void blasfeo_print_exp_svec(int m, struct blasfeo_svec *sa, int ai); +void blasfeo_print_to_file_svec(FILE *file, int m, struct blasfeo_svec *sa, int ai); +void blasfeo_print_to_string_svec(char **buf_out, int m, struct blasfeo_svec *sa, int ai); +void blasfeo_print_tran_svec(int m, struct blasfeo_svec *sa, int ai); +void blasfeo_print_exp_tran_svec(int m, struct blasfeo_svec *sa, int ai); +void blasfeo_print_to_file_tran_svec(FILE *file, int m, struct blasfeo_svec *sa, int ai); +void blasfeo_print_to_string_tran_svec(char **buf_out, int m, struct blasfeo_svec *sa, int ai); + + +#ifdef __cplusplus +} +#endif + + + +#endif // EXT_DEP + +#endif // BLASFEO_S_AUX_EXT_DEP_REF_H_ diff --git a/phonelibs/acados/include/blasfeo/include/blasfeo_s_aux_old.h b/phonelibs/acados/include/blasfeo/include/blasfeo_s_aux_old.h new file mode 100644 index 0000000000..5c6db37bab --- /dev/null +++ b/phonelibs/acados/include/blasfeo/include/blasfeo_s_aux_old.h @@ -0,0 +1,64 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +// TODO remove +// +void strcp_l_lib(int m, float alpha, int offsetA, float *A, int sda, int offsetB, float *B, int sdb); +void sgead_lib(int m, int n, float alpha, int offsetA, float *A, int sda, int offsetB, float *B, int sdb); +void sgetr_lib(int m, int n, float alpha, int offsetA, float *pA, int sda, int offsetC, float *pC, int sdc); +void strtr_l_lib(int m, float alpha, int offsetA, float *pA, int sda, int offsetC, float *pC, int sdc); +void strtr_u_lib(int m, float alpha, int offsetA, float *pA, int sda, int offsetC, float *pC, int sdc); +void sdiareg_lib(int kmax, float reg, int offset, float *pD, int sdd); +void sdiain_sqrt_lib(int kmax, float *x, int offset, float *pD, int sdd); +void sdiaex_lib(int kmax, float alpha, int offset, float *pD, int sdd, float *x); +void sdiaad_lib(int kmax, float alpha, float *x, int offset, float *pD, int sdd); +void sdiain_libsp(int kmax, int *idx, float alpha, float *x, float *pD, int sdd); +void sdiaex_libsp(int kmax, int *idx, float alpha, float *pD, int sdd, float *x); +void sdiaad_libsp(int kmax, int *idx, float alpha, float *x, float *pD, int sdd); +void sdiaadin_libsp(int kmax, int *idx, float alpha, float *x, float *y, float *pD, int sdd); +void srowin_lib(int kmax, float alpha, float *x, float *pD); +void srowex_lib(int kmax, float alpha, float *pD, float *x); +void srowad_lib(int kmax, float alpha, float *x, float *pD); +void srowin_libsp(int kmax, float alpha, int *idx, float *x, float *pD); +void srowad_libsp(int kmax, int *idx, float alpha, float *x, float *pD); +void srowadin_libsp(int kmax, int *idx, float alpha, float *x, float *y, float *pD); +void srowsw_lib(int kmax, float *pA, float *pC); +void scolin_lib(int kmax, float *x, int offset, float *pD, int sdd); +void scolad_lib(int kmax, float alpha, float *x, int offset, float *pD, int sdd); +void scolin_libsp(int kmax, int *idx, float *x, float *pD, int sdd); +void scolad_libsp(int kmax, float alpha, int *idx, float *x, float *pD, int sdd); +void scolsw_lib(int kmax, int offsetA, float *pA, int sda, int offsetC, float *pC, int sdc); +void svecin_libsp(int kmax, int *idx, float *x, float *y); +void svecad_libsp(int kmax, int *idx, float alpha, float *x, float *y); diff --git a/phonelibs/acados/include/blasfeo/include/blasfeo_s_aux_ref.h b/phonelibs/acados/include/blasfeo/include/blasfeo_s_aux_ref.h new file mode 100644 index 0000000000..f6e16a9577 --- /dev/null +++ b/phonelibs/acados/include/blasfeo/include/blasfeo_s_aux_ref.h @@ -0,0 +1,145 @@ + +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_S_AUX_REF_H_ +#define BLASFEO_S_AUX_REF_H_ + + + +#include + +#include "blasfeo_s_aux_old.h" +#include "blasfeo_common.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + +/************************************************ +* d_aux_lib.c +************************************************/ + +// returns the memory size (in bytes) needed for a smat +size_t blasfeo_ref_memsize_smat(int m, int n); +size_t blasfeo_ref_memsize_smat_ps(int ps, int m, int n); +// returns the memory size (in bytes) needed for the diagonal of a smat +size_t blasfeo_ref_memsize_diag_smat(int m, int n); +// returns the memory size (in bytes) needed for a svec +size_t blasfeo_ref_memsize_svec(int m); +// create a strmat for a matrix of size m*n by using memory passed by a pointer (pointer is not updated) +void blasfeo_ref_create_smat(int m, int n, struct blasfeo_smat *sA, void *memory); +void blasfeo_ref_create_smat_ps(int ps, int m, int n, struct blasfeo_smat *sA, void *memory); +// create a strvec for a vector of size m by using memory passed by a pointer (pointer is not updated) +void blasfeo_ref_create_svec(int m, struct blasfeo_svec *sA, void *memory); +void blasfeo_ref_pack_smat(int m, int n, float *A, int lda, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_ref_pack_l_smat(int m, int n, float *A, int lda, struct blasfeo_smat *sB, int bi, int bj); +void blasfeo_ref_pack_l_smat(int m, int n, float *A, int lda, struct blasfeo_smat *sB, int bi, int bj); +void blasfeo_ref_pack_tran_smat(int m, int n, float *A, int lda, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_ref_pack_svec(int m, float *x, int xi, struct blasfeo_svec *sa, int ai); +void blasfeo_ref_unpack_smat(int m, int n, struct blasfeo_smat *sA, int ai, int aj, float *A, int lda); +void blasfeo_ref_unpack_tran_smat(int m, int n, struct blasfeo_smat *sA, int ai, int aj, float *A, int lda); +void blasfeo_ref_unpack_svec(int m, struct blasfeo_svec *sa, int ai, float *x, int xi); +void ref_s_cast_mat2strmat(float *A, struct blasfeo_smat *sA); +void ref_s_cast_diag_mat2strmat(float *dA, struct blasfeo_smat *sA); +void ref_s_cast_vec2vecmat(float *a, struct blasfeo_svec *sa); + +// ge +void blasfeo_ref_sgese(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_ref_sgecpsc(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +void blasfeo_ref_sgecp(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +void blasfeo_ref_sgesc(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_ref_sgein1(float a, struct blasfeo_smat *sA, int ai, int aj); +float blasfeo_ref_sgeex1(struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_ref_sgead(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +void blasfeo_ref_sgetr(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +// tr +void blasfeo_ref_strcp_l(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +void blasfeo_ref_strtr_l(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +void blasfeo_ref_strtr_u(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +// dia +void blasfeo_ref_sdiare(int kmax, float alpha, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_ref_sdiaex(int kmax, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi); +void blasfeo_ref_sdiain(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_ref_sdiain_sp(int kmax, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_smat *sD, int di, int dj); +void blasfeo_ref_sdiaex_sp(int kmax, float alpha, int *idx, struct blasfeo_smat *sD, int di, int dj, struct blasfeo_svec *sx, int xi); +void blasfeo_ref_sdiaad(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_ref_sdiaad_sp(int kmax, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_smat *sD, int di, int dj); +void blasfeo_ref_sdiaadin_sp(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sy, int yi, int *idx, struct blasfeo_smat *sD, int di, int dj); +// row +void blasfeo_ref_srowin(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_ref_srowex(int kmax, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi); +void blasfeo_ref_srowad(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_ref_srowad_sp(int kmax, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_smat *sD, int di, int dj); +void blasfeo_ref_srowsw(int kmax, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +void blasfeo_ref_srowpe(int kmax, int *ipiv, struct blasfeo_smat *sA); +void blasfeo_ref_srowpei(int kmax, int *ipiv, struct blasfeo_smat *sA); +// col +void blasfeo_ref_scolex(int kmax, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi); +void blasfeo_ref_scolin(int kmax, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_ref_scolad(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_ref_scolsc(int kmax, float alpha, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_ref_scolsw(int kmax, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +void blasfeo_ref_scolpe(int kmax, int *ipiv, struct blasfeo_smat *sA); +void blasfeo_ref_scolpei(int kmax, int *ipiv, struct blasfeo_smat *sA); +// vec +void blasfeo_ref_svecse(int m, float alpha, struct blasfeo_svec *sx, int xi); +void blasfeo_ref_sveccp(int m, struct blasfeo_svec *sa, int ai, struct blasfeo_svec *sc, int ci); +void blasfeo_ref_svecsc(int m, float alpha, struct blasfeo_svec *sa, int ai); +void blasfeo_ref_sveccpsc(int m, float alpha, struct blasfeo_svec *sa, int ai, struct blasfeo_svec *sc, int ci); +void blasfeo_ref_svecad(int m, float alpha, struct blasfeo_svec *sa, int ai, struct blasfeo_svec *sc, int ci); +void blasfeo_ref_svecin1(float a, struct blasfeo_svec *sx, int xi); +float blasfeo_ref_svecex1(struct blasfeo_svec *sx, int xi); +void blasfeo_ref_svecad_sp(int m, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_svec *sz, int zi); +void blasfeo_ref_svecin_sp(int m, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_svec *sz, int zi); +void blasfeo_ref_svecex_sp(int m, float alpha, int *idx, struct blasfeo_svec *sx, int x, struct blasfeo_svec *sz, int zi); +void blasfeo_ref_sveccl(int m, struct blasfeo_svec *sxm, int xim, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sxp, int xip, struct blasfeo_svec *sz, int zi); +void blasfeo_ref_sveccl_mask(int m, struct blasfeo_svec *sxm, int xim, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sxp, int xip, struct blasfeo_svec *sz, int zi, struct blasfeo_svec *sm, int mi); +void blasfeo_ref_svecze(int m, struct blasfeo_svec *sm, int mi, struct blasfeo_svec *sv, int vi, struct blasfeo_svec *se, int ei); +void blasfeo_ref_svecnrm_inf(int m, struct blasfeo_svec *sx, int xi, float *ptr_norm); +void blasfeo_ref_svecpe(int kmax, int *ipiv, struct blasfeo_svec *sx, int xi); +void blasfeo_ref_svecpei(int kmax, int *ipiv, struct blasfeo_svec *sx, int xi); + + + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_S_AUX_REF_H_ + diff --git a/phonelibs/acados/include/blasfeo/include/blasfeo_s_aux_test.h b/phonelibs/acados/include/blasfeo/include/blasfeo_s_aux_test.h new file mode 100644 index 0000000000..08d9a14a6a --- /dev/null +++ b/phonelibs/acados/include/blasfeo/include/blasfeo_s_aux_test.h @@ -0,0 +1,177 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_S_AUX_TEST_H_ +#define BLASFEO_S_AUX_TEST_H_ + +#include + +#include "blasfeo_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + + + +/************************************************ +* d_aux_lib.c +************************************************/ + +int test_blasfeo_memsize_smat(int m, int n); +int test_blasfeo_memsize_diag_smat(int m, int n); +int test_blasfeo_memsize_svec(int m); + +void test_blasfeo_create_smat(int m, int n, struct blasfeo_smat *sA, void *memory); +void test_blasfeo_create_svec(int m, struct blasfeo_svec *sA, void *memory); + +void test_blasfeo_pack_smat(int m, int n, float *A, int lda, struct blasfeo_smat *sA, int ai, int aj); +void test_blasfeo_pack_svec(int m, float *x, int xi, struct blasfeo_svec *sa, int ai); +void test_blasfeo_pack_tran_smat(int m, int n, float *A, int lda, struct blasfeo_smat *sA, int ai, int aj); +void test_blasfeo_unpack_smat(int m, int n, struct blasfeo_smat *sA, int ai, int aj, float *A, int lda); +void test_blasfeo_unpack_svec(int m, struct blasfeo_svec *sa, int ai, float *x, int xi); +void test_blasfeo_unpack_tran_smat(int m, int n, struct blasfeo_smat *sA, int ai, int aj, float *A, int lda); + +void test_s_cast_mat2strmat(float *A, struct blasfeo_smat *sA); +void test_s_cast_diag_mat2strmat(float *dA, struct blasfeo_smat *sA); +void test_s_cast_vec2vecmat(float *a, struct blasfeo_svec *sa); +// copy and scale +void test_blasfeo_sgecpsc(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +void test_blasfeo_sgecp(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +void test_blasfeo_sgesc(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj); + +// void test_blasfeo_sgein1(float a, struct blasfeo_smat *sA, int ai, int aj); +// float test_blasfeo_sgeex1(struct blasfeo_smat *sA, int ai, int aj); +// void test_blasfeo_svecin1(float a, struct blasfeo_svec *sx, int xi); +// float test_blasfeo_svecex1(struct blasfeo_svec *sx, int xi); + +// // A <= alpha +// void test_blasfeo_sgese(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj); +// // a <= alpha +// void test_blasfeo_svecse(int m, float alpha, struct blasfeo_svec *sx, int xi); + + +// void test_blasfeo_sveccp(int m, struct blasfeo_svec *sa, int ai, struct blasfeo_svec *sc, int ci); +// void test_blasfeo_svecsc(int m, float alpha, struct blasfeo_svec *sa, int ai); + +// void test_strcp_l_lib(int m, float alpha, int offsetA, float *A, int sda, int offsetB, float *B, int sdb); +// void test_blasfeo_strcp_l(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); + +// void test_sgead_lib(int m, int n, float alpha, int offsetA, float *A, int sda, int offsetB, float *B, int sdb); +// void test_blasfeo_sgead(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +// void test_blasfeo_svecad(int m, float alpha, struct blasfeo_svec *sa, int ai, struct blasfeo_svec *sc, int ci); + +// void test_sgetr_lib(int m, int n, float alpha, int offsetA, float *pA, int sda, int offsetC, float *pC, int sdc); +// void test_blasfeo_sgetr(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); + +// void test_strtr_l_lib(int m, float alpha, int offsetA, float *pA, int sda, int offsetC, float *pC, int sdc); +// void test_blasfeo_strtr_l(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +// void test_strtr_u_lib(int m, float alpha, int offsetA, float *pA, int sda, int offsetC, float *pC, int sdc); +// void test_blasfeo_strtr_u(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); + +// void test_sdiareg_lib(int kmax, float reg, int offset, float *pD, int sdd); +// void test_blasfeo_sdiaex(int kmax, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi); +// void test_blasfeo_sdiain(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj); +// void test_sdiain_sqrt_lib(int kmax, float *x, int offset, float *pD, int sdd); +// void test_sdiaex_lib(int kmax, float alpha, int offset, float *pD, int sdd, float *x); +// void test_sdiaad_lib(int kmax, float alpha, float *x, int offset, float *pD, int sdd); +// void test_sdiain_libsp(int kmax, int *idx, float alpha, float *x, float *pD, int sdd); +// void test_blasfeo_sdiain_sp(int kmax, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_smat *sD, int di, int dj); +// void test_sdiaex_libsp(int kmax, int *idx, float alpha, float *pD, int sdd, float *x); +// void test_blasfeo_sdiaex_sp(int kmax, float alpha, int *idx, struct blasfeo_smat *sD, int di, int dj, struct blasfeo_svec *sx, int xi); +// void test_blasfeo_sdiaad(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj); +// void test_sdiaad_libsp(int kmax, int *idx, float alpha, float *x, float *pD, int sdd); +// void test_blasfeo_sdiaad_sp(int kmax, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_smat *sD, int di, int dj); +// void test_sdiaadin_libsp(int kmax, int *idx, float alpha, float *x, float *y, float *pD, int sdd); +// void test_blasfeo_sdiaadin_sp(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sy, int yi, int *idx, struct blasfeo_smat *sD, int di, int dj); +// void test_srowin_lib(int kmax, float alpha, float *x, float *pD); +// void test_blasfeo_srowin(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj); +// void test_srowex_lib(int kmax, float alpha, float *pD, float *x); +// void test_blasfeo_srowex(int kmax, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi); +// void test_srowad_lib(int kmax, float alpha, float *x, float *pD); +// void test_blasfeo_srowad(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj); +// void test_srowin_libsp(int kmax, float alpha, int *idx, float *x, float *pD); +// void test_srowad_libsp(int kmax, int *idx, float alpha, float *x, float *pD); +// void test_blasfeo_srowad_sp(int kmax, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_smat *sD, int di, int dj); +// void test_srowadin_libsp(int kmax, int *idx, float alpha, float *x, float *y, float *pD); +// void test_srowsw_lib(int kmax, float *pA, float *pC); +// void test_blasfeo_srowsw(int kmax, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +// void test_blasfeo_srowpe(int kmax, int *ipiv, struct blasfeo_smat *sA); +// void test_scolin_lib(int kmax, float *x, int offset, float *pD, int sdd); +// void test_blasfeo_scolin(int kmax, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj); +// void test_scolad_lib(int kmax, float alpha, float *x, int offset, float *pD, int sdd); +// void test_scolin_libsp(int kmax, int *idx, float *x, float *pD, int sdd); +// void test_scolad_libsp(int kmax, float alpha, int *idx, float *x, float *pD, int sdd); +// void test_scolsw_lib(int kmax, int offsetA, float *pA, int sda, int offsetC, float *pC, int sdc); +// void test_blasfeo_scolsw(int kmax, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +// void test_blasfeo_scolpe(int kmax, int *ipiv, struct blasfeo_smat *sA); +// void test_svecin_libsp(int kmax, int *idx, float *x, float *y); +// void test_svecad_libsp(int kmax, int *idx, float alpha, float *x, float *y); +// void test_blasfeo_svecad_sp(int m, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_svec *sz, int zi); +// void test_blasfeo_svecin_sp(int m, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_svec *sz, int zi); +// void test_blasfeo_svecex_sp(int m, float alpha, int *idx, struct blasfeo_svec *sx, int x, struct blasfeo_svec *sz, int zi); +// void test_blasfeo_sveccl(int m, struct blasfeo_svec *sxm, int xim, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sxp, int xip, struct blasfeo_svec *sz, int zi); +// void test_blasfeo_sveccl_mask(int m, struct blasfeo_svec *sxm, int xim, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sxp, int xip, struct blasfeo_svec *sz, int zi, struct blasfeo_svec *sm, int mi); +// void test_blasfeo_svecze(int m, struct blasfeo_svec *sm, int mi, struct blasfeo_svec *sv, int vi, struct blasfeo_svec *se, int ei); +// void test_blasfeo_svecnrm_inf(int m, struct blasfeo_svec *sx, int xi, float *ptr_norm); +// void test_blasfeo_svecpe(int kmax, int *ipiv, struct blasfeo_svec *sx, int xi); + + +// ext_dep + +void test_blasfeo_allocate_smat(int m, int n, struct blasfeo_smat *sA); +void test_blasfeo_allocate_svec(int m, struct blasfeo_svec *sa); + +void test_blasfeo_free_smat(struct blasfeo_smat *sA); +void test_blasfeo_free_svec(struct blasfeo_svec *sa); + +void test_blasfeo_print_smat(int m, int n, struct blasfeo_smat *sA, int ai, int aj); +void test_blasfeo_print_svec(int m, struct blasfeo_svec *sa, int ai); +void test_blasfeo_print_tran_svec(int m, struct blasfeo_svec *sa, int ai); + +void test_blasfeo_print_to_file_smat(FILE *file, int m, int n, struct blasfeo_smat *sA, int ai, int aj); +void test_blasfeo_print_to_file_svec(FILE *file, int m, struct blasfeo_svec *sa, int ai); +void test_blasfeo_print_to_file_tran_svec(FILE *file, int m, struct blasfeo_svec *sa, int ai); + +void test_blasfeo_print_exp_smat(int m, int n, struct blasfeo_smat *sA, int ai, int aj); +void test_blasfeo_print_exp_svec(int m, struct blasfeo_svec *sa, int ai); +void test_blasfeo_print_exp_tran_svec(int m, struct blasfeo_svec *sa, int ai); + + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_S_AUX_TEST_H_ diff --git a/phonelibs/acados/include/blasfeo/include/blasfeo_s_blas.h b/phonelibs/acados/include/blasfeo/include/blasfeo_s_blas.h new file mode 100644 index 0000000000..200f1f51b1 --- /dev/null +++ b/phonelibs/acados/include/blasfeo/include/blasfeo_s_blas.h @@ -0,0 +1,46 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_S_BLAS_H_ +#define BLASFEO_S_BLAS_H_ + + + +#include "blasfeo_s_blasfeo_api.h" +#include "blasfeo_s_blas_api.h" + + + +#endif // BLASFEO_S_BLAS_H_ diff --git a/phonelibs/acados/include/blasfeo/include/blasfeo_s_blas_api.h b/phonelibs/acados/include/blasfeo/include/blasfeo_s_blas_api.h new file mode 100644 index 0000000000..d0fa2245c5 --- /dev/null +++ b/phonelibs/acados/include/blasfeo/include/blasfeo_s_blas_api.h @@ -0,0 +1,117 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef BLASFEO_S_BLAS_API_H_ +#define BLASFEO_S_BLAS_API_H_ + + + +#include "blasfeo_target.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +#ifdef BLAS_API + + + +#ifdef FORTRAN_BLAS_API + + + +// BLAS 1 +// +void saxpy_(int *n, float *alpha, float *x, int *incx, float *y, int *incy); +// +float sdot_(int *n, float *x, int *incx, float *y, int *incy); + +// BLAS 3 +// +void sgemm_(char *ta, char *tb, int *m, int *n, int *k, float *alpha, float *A, int *lda, float *B, int *ldb, float *beta, float *C, int *ldc); +// +void strsm_(char *side, char *uplo, char *transa, char *diag, int *m, int *n, float *alpha, float *A, int *lda, float *B, int *ldb); + + + +// LAPACK +// +void spotrf_(char *uplo, int *m, float *A, int *lda, int *info); + + + +#else // BLASFEO_API + + + +// BLAS 1 +// +void blas_saxpy(int *n, float *alpha, float *x, int *incx, float *y, int *incy); +// +float blas_sdot(int *n, float *x, int *incx, float *y, int *incy); + +// BLAS 3 +// +void blas_sgemm(char *ta, char *tb, int *m, int *n, int *k, float *alpha, float *A, int *lda, float *B, int *ldb, float *beta, float *C, int *ldc); +// +void blas_strsm(char *side, char *uplo, char *transa, char *diag, int *m, int *n, float *alpha, float *A, int *lda, float *B, int *ldb); + + + +// LAPACK +// +void blas_spotrf(char *uplo, int *m, float *A, int *lda, int *info); + + + +#endif // BLASFEO_API + + + +#endif // BLAS_API + + + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_S_BLAS_API_H_ diff --git a/phonelibs/acados/include/blasfeo/include/blasfeo_s_blasfeo_api.h b/phonelibs/acados/include/blasfeo/include/blasfeo_s_blasfeo_api.h new file mode 100644 index 0000000000..72c19dcdc0 --- /dev/null +++ b/phonelibs/acados/include/blasfeo/include/blasfeo_s_blasfeo_api.h @@ -0,0 +1,268 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_S_BLASFEO_API_H_ +#define BLASFEO_S_BLASFEO_API_H_ + + + +#include "blasfeo_common.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + +// +// level 1 BLAS +// + +// z = y + alpha*x +void blasfeo_saxpy(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi); +// z = beta*y + alpha*x +void blasfeo_saxpby(int kmax, float alpha, struct blasfeo_svec *sx, int xi, float beta, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi); +// z = x .* y +void blasfeo_svecmul(int m, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi); +// z += x .* y +void blasfeo_svecmulacc(int m, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi); +// z = x .* y, return sum(z) = x^T * y +float blasfeo_svecmuldot(int m, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi); +// return x^T * y +float blasfeo_sdot(int m, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sy, int yi); +// construct givens plane rotation +void blasfeo_srotg(float a, float b, float *c, float *s); +// apply plane rotation [a b] [c -s; s; c] to the aj0 and aj1 columns of A at row index ai +void blasfeo_scolrot(int m, struct blasfeo_smat *sA, int ai, int aj0, int aj1, float c, float s); +// apply plane rotation [c s; -s c] [a; b] to the ai0 and ai1 rows of A at column index aj +void blasfeo_srowrot(int m, struct blasfeo_smat *sA, int ai0, int ai1, int aj, float c, float s); + + + +// +// level 2 BLAS +// + +// dense + +// z <= beta * y + alpha * A * x +void blasfeo_sgemv_n(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, float beta, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi); +// z <= beta * y + alpha * A' * x +void blasfeo_sgemv_t(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, float beta, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi); +// z <= inv( A ) * x, A (m)x(n) +void blasfeo_strsv_lnn_mn(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= inv( A' ) * x, A (m)x(n) +void blasfeo_strsv_ltn_mn(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= inv( A ) * x, A (m)x(m) lower, not_transposed, not_unit +void blasfeo_strsv_lnn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= inv( A ) * x, A (m)x(m) lower, not_transposed, unit +void blasfeo_strsv_lnu(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= inv( A' ) * x, A (m)x(m) lower, transposed, not_unit +void blasfeo_strsv_ltn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= inv( A' ) * x, A (m)x(m) lower, transposed, unit +void blasfeo_strsv_ltu(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= inv( A' ) * x, A (m)x(m) upper, not_transposed, not_unit +void blasfeo_strsv_unn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= inv( A' ) * x, A (m)x(m) upper, transposed, not_unit +void blasfeo_strsv_utn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= beta * y + alpha * A * x ; A upper triangular +void blasfeo_strmv_unn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= A' * x ; A upper triangular +void blasfeo_strmv_utn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= A * x ; A lower triangular +void blasfeo_strmv_lnn(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= A' * x ; A lower triangular +void blasfeo_strmv_ltn(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z_n <= beta_n * y_n + alpha_n * A * x_n +// z_t <= beta_t * y_t + alpha_t * A' * x_t +void blasfeo_sgemv_nt(int m, int n, float alpha_n, float alpha_t, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx_n, int xi_n, struct blasfeo_svec *sx_t, int xi_t, float beta_n, float beta_t, struct blasfeo_svec *sy_n, int yi_n, struct blasfeo_svec *sy_t, int yi_t, struct blasfeo_svec *sz_n, int zi_n, struct blasfeo_svec *sz_t, int zi_t); +// z <= beta * y + alpha * A * x, where A is symmetric and only the lower triangular patr of A is accessed +void blasfeo_ssymv_l(int m, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, float beta, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi); +void blasfeo_ssymv_l_mn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, float beta, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi); + +// diagonal + +// z <= beta * y + alpha * A * x, A diagonal +void blasfeo_sgemv_d(int m, float alpha, struct blasfeo_svec *sA, int ai, struct blasfeo_svec *sx, int xi, float beta, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi); + + + +// +// level 3 BLAS +// + +// dense + +// D <= beta * C + alpha * A * B +void blasfeo_sgemm_nn(int m, int n, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= beta * C + alpha * A * B^T +void blasfeo_sgemm_nt(int m, int n, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= beta * C + alpha * A^T * B +void blasfeo_sgemm_tn(int m, int n, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= beta * C + alpha * A^T * B +void blasfeo_sgemm_tt(int m, int n, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= beta * C + alpha * A * B^T ; C, D lower triangular +void blasfeo_ssyrk_ln(int m, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +void blasfeo_ssyrk_ln_mn(int m, int n, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= beta * C + alpha * A^T * B ; C, D lower triangular +void blasfeo_ssyrk_lt(int m, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= beta * C + alpha * A * B^T ; C, D upper triangular +void blasfeo_ssyrk_un(int m, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= beta * C + alpha * A^T * B ; C, D upper triangular +void blasfeo_ssyrk_ut(int m, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * B * A^T ; B upper triangular +void blasfeo_strmm_rutn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * B * A ; A lower triangular +void blasfeo_strmm_rlnn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * A^{-1} * B , with A lower triangular employint explicit inverse of diagonal +void blasfeo_strsm_llnn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * A^{-1} * B , with A lower triangular with unit diagonal +void blasfeo_strsm_llnu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * A^{-T} * B , with A lower triangular employint explicit inverse of diagonal +void blasfeo_strsm_lltn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * A^{-T} * B , with A lower triangular with unit diagonal +void blasfeo_strsm_lltu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * A^{-1} * B , with A upper triangular employing explicit inverse of diagonal +void blasfeo_strsm_lunn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * A^{-1} * B , with A upper triangular withunit diagonal +void blasfeo_strsm_lunu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * A^{-T} * B , with A upper triangular employing explicit inverse of diagonal +void blasfeo_strsm_lutn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * A^{-T} * B , with A upper triangular withunit diagonal +void blasfeo_strsm_lutu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * B * A^{-1} , with A lower triangular employing explicit inverse of diagonal +void blasfeo_strsm_rlnn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * B * A^{-1} , with A lower triangular with unit diagonal +void blasfeo_strsm_rlnu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * B * A^{-T} , with A lower triangular employing explicit inverse of diagonal +void blasfeo_strsm_rltn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * B * A^{-T} , with A lower triangular with unit diagonal +void blasfeo_strsm_rltu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * B * A^{-1} , with A upper triangular employing explicit inverse of diagonal +void blasfeo_strsm_runn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * B * A^{-1} , with A upper triangular with unit diagonal +void blasfeo_strsm_runu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * B * A^{-T} , with A upper triangular employing explicit inverse of diagonal +void blasfeo_strsm_rutn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * B * A^{-T} , with A upper triangular with unit diagonal +void blasfeo_strsm_rutu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); + +// diagonal + +// D <= alpha * A * B + beta * C, with A diagonal (stored as strvec) +void sgemm_diag_left_ib(int m, int n, float alpha, float *dA, float *pB, int sdb, float beta, float *pC, int sdc, float *pD, int sdd); +void blasfeo_sgemm_dn(int m, int n, float alpha, struct blasfeo_svec *sA, int ai, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * A * B + beta * C, with B diagonal (stored as strvec) +void blasfeo_sgemm_nd(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sB, int bi, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); + + + +// +// LAPACK +// + +// D <= chol( C ) ; C, D lower triangular +void blasfeo_spotrf_l(int m, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +void blasfeo_spotrf_l_mn(int m, int n, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= chol( C ) ; C, D upper triangular +void blasfeo_spotrf_u(int m, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= chol( C + A * B' ) ; C, D lower triangular +void blasfeo_ssyrk_spotrf_ln(int m, int k, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +void blasfeo_ssyrk_spotrf_ln_mn(int m, int n, int k, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= lu( C ) ; no pivoting +void blasfeo_sgetrf_np(int m, int n, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= lu( C ) ; row pivoting +void blasfeo_sgetrf_rp(int m, int n, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj, int *ipiv); +// D <= qr( C ) +void blasfeo_sgeqrf(int m, int n, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj, void *work); +int blasfeo_sgeqrf_worksize(int m, int n); // in bytes +// D <= Q factor, where C is the output of the LQ factorization +int blasfeo_sorglq_worksize(int m, int n, int k); // in bytes +void blasfeo_sorglq(int m, int n, int k, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj, void *work); +// D <= lq( C ) +void blasfeo_sgelqf(int m, int n, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj, void *work); +int blasfeo_sgelqf_worksize(int m, int n); // in bytes +// D <= lq( C ), positive diagonal elements +void blasfeo_sgelqf_pd(int m, int n, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj, void *work); +// [L, A] <= lq( [L, A] ), positive diagonal elements, array of matrices, with +// L lower triangular, of size (m)x(m) +// A full, of size (m)x(n1) +void blasfeo_sgelqf_pd_la(int m, int n1, struct blasfeo_smat *sL, int li, int lj, struct blasfeo_smat *sA, int ai, int aj, void *work); +// [L, L, A] <= lq( [L, L, A] ), positive diagonal elements, array of matrices, with: +// L lower triangular, of size (m)x(m) +// A full, of size (m)x(n1) +void blasfeo_sgelqf_pd_lla(int m, int n1, struct blasfeo_smat *sL0, int l0i, int l0j, struct blasfeo_smat *sL1, int l1i, int l1j, struct blasfeo_smat *sA, int ai, int aj, void *work); + + + + +// +// BLAS API helper functions +// + +#if ( defined(BLAS_API) & defined(MF_PANELMAJ) ) +// BLAS 3 +void blasfeo_cm_sgemm_nn(int m, int n, int k, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, float beta, struct blasfeo_cm_smat *sC, int ci, int cj, struct blasfeo_cm_smat *sD, int di, int dj); +void blasfeo_cm_sgemm_nt(int m, int n, int k, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, float beta, struct blasfeo_cm_smat *sC, int ci, int cj, struct blasfeo_cm_smat *sD, int di, int dj); +void blasfeo_cm_sgemm_tn(int m, int n, int k, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, float beta, struct blasfeo_cm_smat *sC, int ci, int cj, struct blasfeo_cm_smat *sD, int di, int dj); +void blasfeo_cm_sgemm_tt(int m, int n, int k, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, float beta, struct blasfeo_cm_smat *sC, int ci, int cj, struct blasfeo_cm_smat *sD, int di, int dj); +void blasfeo_cm_strsm_llnn(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj); +void blasfeo_cm_strsm_llnu(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj); +void blasfeo_cm_strsm_lltn(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj); +void blasfeo_cm_strsm_lltu(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj); +void blasfeo_cm_strsm_lunn(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj); +void blasfeo_cm_strsm_lunu(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj); +void blasfeo_cm_strsm_lutn(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj); +void blasfeo_cm_strsm_lutu(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj); +void blasfeo_cm_strsm_rlnn(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj); +void blasfeo_cm_strsm_rlnu(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj); +void blasfeo_cm_strsm_rltn(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj); +void blasfeo_cm_strsm_rltu(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj); +void blasfeo_cm_strsm_runn(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj); +void blasfeo_cm_strsm_runu(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj); +void blasfeo_cm_strsm_rutn(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj); +void blasfeo_cm_strsm_rutu(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj); +// LAPACK +void blasfeo_cm_spotrf_l(int m, struct blasfeo_cm_smat *sC, int ci, int cj, struct blasfeo_cm_smat *sD, int di, int dj); +void blasfeo_cm_spotrf_u(int m, struct blasfeo_cm_smat *sC, int ci, int cj, struct blasfeo_cm_smat *sD, int di, int dj); +#endif + + + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_S_BLASFEO_API_H_ diff --git a/phonelibs/acados/include/blasfeo/include/blasfeo_s_blasfeo_api_ref.h b/phonelibs/acados/include/blasfeo/include/blasfeo_s_blasfeo_api_ref.h new file mode 100644 index 0000000000..f429a79dc3 --- /dev/null +++ b/phonelibs/acados/include/blasfeo/include/blasfeo_s_blasfeo_api_ref.h @@ -0,0 +1,135 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_S_BLASFEO_API_REF_H_ +#define BLASFEO_S_BLASFEO_API_REF_H_ + +#include "blasfeo_common.h" + + +#ifdef __cplusplus +extern "C" { +#endif + + +// expose reference BLASFEO for testing + +// --- level 1 + +void blasfeo_saxpy_ref(int kmax, float alpha, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sy, int yi, struct blasfeo_svec_ref *sz, int zi); +void blasfeo_saxpby_ref(int kmax, float alpha, struct blasfeo_svec_ref *sx, int xi, float beta, struct blasfeo_svec_ref *sy, int yi, struct blasfeo_svec_ref *sz, int zi); +void blasfeo_svecmul_ref(int m, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sy, int yi, struct blasfeo_svec_ref *sz, int zi); +void blasfeo_svecmulacc_ref(int m, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sy, int yi, struct blasfeo_svec_ref *sz, int zi); +float blasfeo_svecmuldot_ref(int m, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sy, int yi, struct blasfeo_svec_ref *sz, int zi); +float blasfeo_sdot_ref(int m, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sy, int yi); +void blasfeo_srotg_ref(float a, float b, float *c, float *s); +void blasfeo_scolrot_ref(int m, struct blasfeo_smat_ref *sA, int ai, int aj0, int aj1, float c, float s); +void blasfeo_srowrot_ref(int m, struct blasfeo_smat_ref *sA, int ai0, int ai1, int aj, float c, float s); + + +// --- level 2 + +// dense +void blasfeo_sgemv_n_ref(int m, int n, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, float beta, struct blasfeo_svec_ref *sy, int yi, struct blasfeo_svec_ref *sz, int zi); +void blasfeo_sgemv_t_ref(int m, int n, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, float beta, struct blasfeo_svec_ref *sy, int yi, struct blasfeo_svec_ref *sz, int zi); +void blasfeo_strsv_lnn_mn_ref(int m, int n, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sz, int zi); +void blasfeo_strsv_ltn_mn_ref(int m, int n, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sz, int zi); +void blasfeo_strsv_lnn_ref(int m, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sz, int zi); +void blasfeo_strsv_lnu_ref(int m, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sz, int zi); +void blasfeo_strsv_ltn_ref(int m, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sz, int zi); +void blasfeo_strsv_ltu_ref(int m, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sz, int zi); +void blasfeo_strsv_unn_ref(int m, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sz, int zi); +void blasfeo_strsv_utn_ref(int m, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sz, int zi); +void blasfeo_strmv_unn_ref(int m, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sz, int zi); +void blasfeo_strmv_utn_ref(int m, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sz, int zi); +void blasfeo_strmv_lnn_ref(int m, int n, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sz, int zi); +void blasfeo_strmv_ltn_ref(int m, int n, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sz, int zi); +void blasfeo_strmv_lnu_ref(int m, int n, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sz, int zi); +void blasfeo_strmv_ltu_ref(int m, int n, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sz, int zi); +void blasfeo_sgemv_nt_ref(int m, int n, float alpha_n, float alpha_t, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx_n, int xi_n, struct blasfeo_svec_ref *sx_t, int xi_t, float beta_n, float beta_t, struct blasfeo_svec_ref *sy_n, int yi_n, struct blasfeo_svec_ref *sy_t, int yi_t, struct blasfeo_svec_ref *sz_n, int zi_n, struct blasfeo_svec_ref *sz_t, int zi_t); +void blasfeo_ssymv_l_ref(int m, int n, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, float beta, struct blasfeo_svec_ref *sy, int yi, struct blasfeo_svec_ref *sz, int zi); + +// diagonal +void blasfeo_sgemv_d_ref(int m, float alpha, struct blasfeo_svec_ref *sA, int ai, struct blasfeo_svec_ref *sx, int xi, float beta, struct blasfeo_svec_ref *sy, int yi, struct blasfeo_svec_ref *sz, int zi); + + +// --- level 3 + +// dense +void blasfeo_sgemm_nn_ref( int m, int n, int k, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, float beta, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj); +void blasfeo_sgemm_nt_ref( int m, int n, int k, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, float beta, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj); +void blasfeo_sgemm_tn_ref(int m, int n, int k, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, float beta, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj); +void blasfeo_sgemm_tt_ref(int m, int n, int k, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, float beta, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj); + +void blasfeo_ssyrk_ln_mn_ref( int m, int n, int k, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, float beta, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj); +void blasfeo_ssyrk_ln_ref( int m, int k, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, float beta, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj); +void blasfeo_ssyrk_lt_ref( int m, int k, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, float beta, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj); +void blasfeo_ssyrk_un_ref( int m, int k, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, float beta, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj); +void blasfeo_ssyrk_ut_ref( int m, int k, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, float beta, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj); + +void blasfeo_strmm_rutn_ref( int m, int n, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, struct blasfeo_smat_ref *sD, int di, int dj); +void blasfeo_strmm_rlnn_ref( int m, int n, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, struct blasfeo_smat_ref *sD, int di, int dj); +void blasfeo_strsm_rltn_ref( int m, int n, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, struct blasfeo_smat_ref *sD, int di, int dj); +void blasfeo_strsm_rltu_ref( int m, int n, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, struct blasfeo_smat_ref *sD, int di, int dj); +void blasfeo_strsm_rutn_ref( int m, int n, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, struct blasfeo_smat_ref *sD, int di, int dj); +void blasfeo_strsm_llnu_ref( int m, int n, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, struct blasfeo_smat_ref *sD, int di, int dj); +void blasfeo_strsm_lunn_ref( int m, int n, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, struct blasfeo_smat_ref *sD, int di, int dj); + +// diagonal +void dgemm_diag_left_lib_ref(int m, int n, float alpha, float *dA, float *pB, int sdb, float beta, float *pC, int sdc, float *pD, int sdd); +void blasfeo_sgemm_dn_ref(int m, int n, float alpha, struct blasfeo_svec_ref *sA, int ai, struct blasfeo_smat_ref *sB, int bi, int bj, float beta, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj); +void blasfeo_sgemm_nd_ref(int m, int n, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sB, int bi, float beta, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj); + +// --- lapack + +void blasfeo_sgetrf_nopivot_ref(int m, int n, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj); +void blasfeo_sgetrf_rowpivot_ref(int m, int n, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj, int *ipiv); +void blasfeo_spotrf_l_ref(int m, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj); +void blasfeo_spotrf_l_mn_ref(int m, int n, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj); +void blasfeo_ssyrk_dpotrf_ln_ref(int m, int k, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj); +void blasfeo_ssyrk_dpotrf_ln_mn_ref(int m, int n, int k, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj); +void blasfeo_sgetrf_nopivot_ref(int m, int n, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj); +void blasfeo_sgetrf_rowpivot_ref(int m, int n, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj, int *ipiv); +void blasfeo_sgeqrf_ref(int m, int n, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj, void *work); +void blasfeo_sgelqf_ref(int m, int n, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj, void *work); +void blasfeo_sgelqf_pd_ref(int m, int n, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj, void *work); +void blasfeo_sgelqf_pd_la_ref(int m, int n1, struct blasfeo_smat_ref *sL, int li, int lj, struct blasfeo_smat_ref *sA, int ai, int aj, void *work); +void blasfeo_sgelqf_pd_lla_ref(int m, int n1, struct blasfeo_smat_ref *sL0, int l0i, int l0j, struct blasfeo_smat_ref *sL1, int l1i, int l1j, struct blasfeo_smat_ref *sA, int ai, int aj, void *work); + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_S_BLASFEO_API_REF_H_ diff --git a/phonelibs/acados/include/blasfeo/include/blasfeo_s_blasfeo_ref_api.h b/phonelibs/acados/include/blasfeo/include/blasfeo_s_blasfeo_ref_api.h new file mode 100644 index 0000000000..db8b28d3b7 --- /dev/null +++ b/phonelibs/acados/include/blasfeo/include/blasfeo_s_blasfeo_ref_api.h @@ -0,0 +1,235 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_S_BLASFEO_REF_API_H_ +#define BLASFEO_S_BLASFEO_REF_API_H_ + + + +#include "blasfeo_common.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + +// +// level 1 BLAS +// + +// z = y + alpha*x +void blasfeo_ref_saxpy(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi); +// z = beta*y + alpha*x +void blasfeo_ref_saxpby(int kmax, float alpha, struct blasfeo_svec *sx, int xi, float beta, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi); +// z = x .* y +void blasfeo_ref_svecmul(int m, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi); +// z += x .* y +void blasfeo_ref_svecmulacc(int m, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi); +// z = x .* y, return sum(z) = x^T * y +float blasfeo_ref_svecmuldot(int m, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi); +// return x^T * y +float blasfeo_ref_sdot(int m, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sy, int yi); +// construct givens plane rotation +void blasfeo_ref_srotg(float a, float b, float *c, float *s); +// apply plane rotation [a b] [c -s; s; c] to the aj0 and aj1 columns of A at row index ai +void blasfeo_ref_scolrot(int m, struct blasfeo_smat *sA, int ai, int aj0, int aj1, float c, float s); +// apply plane rotation [c s; -s c] [a; b] to the ai0 and ai1 rows of A at column index aj +void blasfeo_ref_srowrot(int m, struct blasfeo_smat *sA, int ai0, int ai1, int aj, float c, float s); + + + +// +// level 2 BLAS +// + +// dense + +// z <= beta * y + alpha * A * x +void blasfeo_ref_sgemv_n(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, float beta, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi); +// z <= beta * y + alpha * A' * x +void blasfeo_ref_sgemv_t(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, float beta, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi); +// z <= inv( A ) * x, A (m)x(n) +void blasfeo_ref_strsv_lnn_mn(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= inv( A' ) * x, A (m)x(n) +void blasfeo_ref_strsv_ltn_mn(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= inv( A ) * x, A (m)x(m) lower, not_transposed, not_unit +void blasfeo_ref_strsv_lnn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= inv( A ) * x, A (m)x(m) lower, not_transposed, unit +void blasfeo_ref_strsv_lnu(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= inv( A' ) * x, A (m)x(m) lower, transposed, not_unit +void blasfeo_ref_strsv_ltn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= inv( A' ) * x, A (m)x(m) lower, transposed, unit +void blasfeo_ref_strsv_ltu(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= inv( A' ) * x, A (m)x(m) upper, not_transposed, not_unit +void blasfeo_ref_strsv_unn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= inv( A' ) * x, A (m)x(m) upper, transposed, not_unit +void blasfeo_ref_strsv_utn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= beta * y + alpha * A * x ; A upper triangular +void blasfeo_ref_strmv_unn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= A' * x ; A upper triangular +void blasfeo_ref_strmv_utn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= A * x ; A lower triangular +void blasfeo_ref_strmv_lnn(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= A' * x ; A lower triangular +void blasfeo_ref_strmv_ltn(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z_n <= beta_n * y_n + alpha_n * A * x_n +// z_t <= beta_t * y_t + alpha_t * A' * x_t +void blasfeo_ref_sgemv_nt(int m, int n, float alpha_n, float alpha_t, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx_n, int xi_n, struct blasfeo_svec *sx_t, int xi_t, float beta_n, float beta_t, struct blasfeo_svec *sy_n, int yi_n, struct blasfeo_svec *sy_t, int yi_t, struct blasfeo_svec *sz_n, int zi_n, struct blasfeo_svec *sz_t, int zi_t); +// z <= beta * y + alpha * A * x, where A is symmetric and only the lower triangular patr of A is accessed +void blasfeo_ref_ssymv_l(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, float beta, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi); + +// diagonal + +// z <= beta * y + alpha * A * x, A diagonal +void blasfeo_ref_sgemv_d(int m, float alpha, struct blasfeo_svec *sA, int ai, struct blasfeo_svec *sx, int xi, float beta, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi); + + + +// +// level 3 BLAS +// + +// dense + +// D <= beta * C + alpha * A * B +void blasfeo_ref_sgemm_nn(int m, int n, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= beta * C + alpha * A * B^T +void blasfeo_ref_sgemm_nt(int m, int n, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= beta * C + alpha * A^T * B +void blasfeo_ref_sgemm_tn(int m, int n, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= beta * C + alpha * A^T * B +void blasfeo_ref_sgemm_tt(int m, int n, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= beta * C + alpha * A * B^T ; C, D lower triangular +void blasfeo_ref_ssyrk_ln(int m, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +void blasfeo_ref_ssyrk_ln_mn(int m, int n, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= beta * C + alpha * A^T * B ; C, D lower triangular +void blasfeo_ref_ssyrk_lt(int m, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= beta * C + alpha * A * B^T ; C, D upper triangular +void blasfeo_ref_ssyrk_un(int m, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= beta * C + alpha * A^T * B ; C, D upper triangular +void blasfeo_ref_ssyrk_ut(int m, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * B * A^T ; B upper triangular +void blasfeo_ref_strmm_rutn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * B * A ; A lower triangular +void blasfeo_ref_strmm_rlnn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * A^{-1} * B , with A lower triangular employint explicit inverse of diagonal +void blasfeo_ref_strsm_llnn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * A^{-1} * B , with A lower triangular with unit diagonal +void blasfeo_ref_strsm_llnu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * A^{-T} * B , with A lower triangular employint explicit inverse of diagonal +void blasfeo_ref_strsm_lltn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * A^{-T} * B , with A lower triangular with unit diagonal +void blasfeo_ref_strsm_lltu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * A^{-1} * B , with A upper triangular employing explicit inverse of diagonal +void blasfeo_ref_strsm_lunn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * A^{-1} * B , with A upper triangular withunit diagonal +void blasfeo_ref_strsm_lunu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * A^{-T} * B , with A upper triangular employing explicit inverse of diagonal +void blasfeo_ref_strsm_lutn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * A^{-T} * B , with A upper triangular withunit diagonal +void blasfeo_ref_strsm_lutu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * B * A^{-1} , with A lower triangular employing explicit inverse of diagonal +void blasfeo_ref_strsm_rlnn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * B * A^{-1} , with A lower triangular with unit diagonal +void blasfeo_ref_strsm_rlnu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * B * A^{-T} , with A lower triangular employing explicit inverse of diagonal +void blasfeo_ref_strsm_rltn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * B * A^{-T} , with A lower triangular with unit diagonal +void blasfeo_ref_strsm_rltu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * B * A^{-1} , with A upper triangular employing explicit inverse of diagonal +void blasfeo_ref_strsm_runn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * B * A^{-1} , with A upper triangular with unit diagonal +void blasfeo_ref_strsm_runu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * B * A^{-T} , with A upper triangular employing explicit inverse of diagonal +void blasfeo_ref_strsm_rutn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * B * A^{-T} , with A upper triangular with unit diagonal +void blasfeo_ref_strsm_rutu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); + +// diagonal + +// D <= alpha * A * B + beta * C, with A diagonal (stored as strvec) +void sgemm_diag_left_ib(int m, int n, float alpha, float *dA, float *pB, int sdb, float beta, float *pC, int sdc, float *pD, int sdd); +void blasfeo_ref_sgemm_dn(int m, int n, float alpha, struct blasfeo_svec *sA, int ai, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * A * B + beta * C, with B diagonal (stored as strvec) +void blasfeo_ref_sgemm_nd(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sB, int bi, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); + + + +// +// LAPACK +// + +// D <= chol( C ) ; C, D lower triangular +void blasfeo_ref_spotrf_l(int m, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +void blasfeo_ref_spotrf_l_mn(int m, int n, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= chol( C ) ; C, D upper triangular +void blasfeo_ref_spotrf_u(int m, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= chol( C + A * B' ) ; C, D lower triangular +void blasfeo_ref_ssyrk_spotrf_ln(int m, int k, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +void blasfeo_ref_ssyrk_spotrf_ln_mn(int m, int n, int k, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= lu( C ) ; no pivoting +void blasfeo_ref_sgetrf_np(int m, int n, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= lu( C ) ; row pivoting +void blasfeo_ref_sgetrf_rp(int m, int n, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj, int *ipiv); +// D <= qr( C ) +void blasfeo_ref_sgeqrf(int m, int n, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj, void *work); +int blasfeo_ref_sgeqrf_worksize(int m, int n); // in bytes +// D <= Q factor, where C is the output of the LQ factorization +int blasfeo_ref_sorglq_worksize(int m, int n, int k); // in bytes +void blasfeo_ref_sorglq(int m, int n, int k, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj, void *work); +// D <= lq( C ) +void blasfeo_ref_sgelqf(int m, int n, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj, void *work); +int blasfeo_ref_sgelqf_worksize(int m, int n); // in bytes +// D <= lq( C ), positive diagonal elements +void blasfeo_ref_sgelqf_pd(int m, int n, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj, void *work); +// [L, A] <= lq( [L, A] ), positive diagonal elements, array of matrices, with +// L lower triangular, of size (m)x(m) +// A full, of size (m)x(n1) +void blasfeo_ref_sgelqf_pd_la(int m, int n1, struct blasfeo_smat *sL, int li, int lj, struct blasfeo_smat *sA, int ai, int aj, void *work); +// [L, L, A] <= lq( [L, L, A] ), positive diagonal elements, array of matrices, with: +// L lower triangular, of size (m)x(m) +// A full, of size (m)x(n1) +void blasfeo_ref_sgelqf_pd_lla(int m, int n1, struct blasfeo_smat *sL0, int l0i, int l0j, struct blasfeo_smat *sL1, int l1i, int l1j, struct blasfeo_smat *sA, int ai, int aj, void *work); + + + + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_S_BLASFEO_REF_API_H_ + diff --git a/phonelibs/acados/include/blasfeo/include/blasfeo_s_kernel.h b/phonelibs/acados/include/blasfeo/include/blasfeo_s_kernel.h new file mode 100644 index 0000000000..dcbde989cc --- /dev/null +++ b/phonelibs/acados/include/blasfeo/include/blasfeo_s_kernel.h @@ -0,0 +1,682 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_S_KERNEL_H_ +#define BLASFEO_S_KERNEL_H_ + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +// utils +void blasfeo_align_2MB(void *ptr, void **ptr_align); +void blasfeo_align_4096_byte(void *ptr, void **ptr_align); +void blasfeo_align_64_byte(void *ptr, void **ptr_align); + + + +// +// lib8 +// + +// 24x4 +void kernel_sgemm_nt_24x4_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd); +void kernel_sgemm_nt_24x4_vs_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, int km, int kn); +void kernel_sgemm_nt_24x4_gen_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, int offsetC, float *C, int sdc, int offsetD, float *D, int sdd, int m0, int m1, int n0, int n1); +void kernel_sgemm_nn_24x4_lib8(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, float *beta, float *C, int sdc, float *D, int sdd); +void kernel_sgemm_nn_24x4_vs_lib8(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, float *beta, float *C, int sdc, float *D, int sdd, int km, int kn); +void kernel_sgemm_nn_24x4_gen_lib8(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, float *beta, int offsetC, float *C, int sdc, int offsetD, float *D, int sdd, int m0, int m1, int n0, int n1); +void kernel_ssyrk_nt_l_24x4_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd); +void kernel_ssyrk_nt_l_24x4_vs_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, int km, int kn); +void kernel_ssyrk_nt_l_20x4_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd); +void kernel_ssyrk_nt_l_20x4_vs_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, int km, int kn); +void kernel_spotrf_nt_l_24x4_lib8(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *inv_diag_D); +void kernel_spotrf_nt_l_24x4_vs_lib8(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *inv_diag_D, int km, int kn); +void kernel_spotrf_nt_l_20x4_lib8(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *inv_diag_D); +void kernel_spotrf_nt_l_20x4_vs_lib8(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *inv_diag_D, int km, int kn); +void kernel_strsm_nt_rl_inv_24x4_lib8(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *E, float *inv_diag_E); +void kernel_strsm_nt_rl_inv_24x4_vs_lib8(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *E, float *inv_diag_E, int km, int kn); +void kernel_sgemm_strsm_nt_rl_inv_24x4_lib8(int kp, float *Ap, int sdap, float *Bp, int km_, float *Am, int sdam, float *Bm, float *C, int sdc, float *D, int sdd, float *E, float *inv_diag_E); +void kernel_sgemm_strsm_nt_rl_inv_24x4_vs_lib8(int kp, float *Ap, int sdap, float *Bp, int km_, float *Am, int sdam, float *Bm, float *C, int sdc, float *D, int sdd, float *E, float *inv_diag_E, int km, int kn); +void kernel_ssyrk_spotrf_nt_l_20x4_vs_lib8(int kp, float *Ap, int sdap, float *Bp, int km_, float *Am, int sdam, float *Bm, float *C, int sdc, float *D, int sdd, float *inv_diag_D, int km, int kn); +void kernel_ssyrk_spotrf_nt_l_20x4_lib8(int kp, float *Ap, int sdap, float *Bp, int km_, float *Am, int sdam, float *Bm, float *C, int sdc, float *D, int sdd, float *inv_diag_D); +void kernel_ssyrk_spotrf_nt_l_24x4_vs_lib8(int kp, float *Ap, int sdap, float *Bp, int km_, float *Am, int sdam, float *Bm, float *C, int sdc, float *D, int sdd, float *inv_diag_D, int km, int kn); +void kernel_ssyrk_spotrf_nt_l_24x4_lib8(int kp, float *Ap, int sdap, float *Bp, int km_, float *Am, int sdam, float *Bm, float *C, int sdc, float *D, int sdd, float *inv_diag_D); +void kernel_strmm_nn_rl_24x4_lib8(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, float *D, int sdd); +void kernel_strmm_nn_rl_24x4_vs_lib8(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, float *D, int sdd, int km, int kn); + +// 16x8 +void kernel_sgemm_nt_16x8_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, float *spil); + +// 16x4 +void kernel_sgemm_nt_16x4_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd); +void kernel_sgemm_nt_16x4_vs_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, int km, int kn); +void kernel_sgemm_nt_16x4_gen_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, int offsetC, float *C, int sdc, int offsetD, float *D, int sdd, int m0, int m1, int n0, int n1); +void kernel_sgemm_nn_16x4_lib8(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, float *beta, float *C, int sdc, float *D, int sdd); +void kernel_sgemm_nn_16x4_vs_lib8(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, float *beta, float *C, int sdc, float *D, int sdd, int km, int kn); +void kernel_sgemm_nn_16x4_gen_lib8(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, float *beta, int offsetC, float *C, int sdc, int offsetD, float *D, int sdd, int m0, int m1, int n0, int n1); +void kernel_ssyrk_nt_l_16x4_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd); +void kernel_ssyrk_nt_l_16x4_vs_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, int km, int kn); +void kernel_ssyrk_nt_l_12x4_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd); +void kernel_ssyrk_nt_l_12x4_vs_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, int km, int kn); +void kernel_spotrf_nt_l_16x4_lib8(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *inv_diag_D); +void kernel_spotrf_nt_l_16x4_vs_lib8(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *inv_diag_D, int km, int kn); +void kernel_spotrf_nt_l_12x4_lib8(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *inv_diag_D); +void kernel_spotrf_nt_l_12x4_vs_lib8(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *inv_diag_D, int km, int kn); +void kernel_strsm_nt_rl_inv_16x4_lib8(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *E, float *inv_diag_E); +void kernel_strsm_nt_rl_inv_16x4_vs_lib8(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *E, float *inv_diag_E, int km, int kn); +void kernel_sgemm_strsm_nt_rl_inv_16x4_lib8(int kp, float *Ap, int sdap, float *Bp, int km_, float *Am, int sdam, float *Bm, float *C, int sdc, float *D, int sdd, float *E, float *inv_diag_E); +void kernel_sgemm_strsm_nt_rl_inv_16x4_vs_lib8(int kp, float *Ap, int sdap, float *Bp, int km_, float *Am, int sdam, float *Bm, float *C, int sdc, float *D, int sdd, float *E, float *inv_diag_E, int km, int kn); +void kernel_ssyrk_spotrf_nt_l_12x4_vs_lib8(int kp, float *Ap, int sdap, float *Bp, int km_, float *Am, int sdam, float *Bm, float *C, int sdc, float *D, int sdd, float *inv_diag_D, int km, int kn); +void kernel_ssyrk_spotrf_nt_l_12x4_lib8(int kp, float *Ap, int sdap, float *Bp, int km_, float *Am, int sdam, float *Bm, float *C, int sdc, float *D, int sdd, float *inv_diag_D); +void kernel_ssyrk_spotrf_nt_l_16x4_vs_lib8(int kp, float *Ap, int sdap, float *Bp, int km_, float *Am, int sdam, float *Bm, float *C, int sdc, float *D, int sdd, float *inv_diag_D, int km, int kn); +void kernel_ssyrk_spotrf_nt_l_16x4_lib8(int kp, float *Ap, int sdap, float *Bp, int km_, float *Am, int sdam, float *Bm, float *C, int sdc, float *D, int sdd, float *inv_diag_D); +void kernel_strmm_nn_rl_16x4_lib8(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, float *D, int sdd); +void kernel_strmm_nn_rl_16x4_vs_lib8(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, float *D, int sdd, int km, int kn); +void kernel_strmm_nn_rl_16x4_gen_lib8(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, int offsetD, float *D, int sdd, int m0, int m1, int n0, int n1); + +// 8x8 +void kernel_sgemm_nt_8x8_lib8(int k, float *alpha, float *A, float *B, float *beta, float *C, float *D); +void kernel_sgemm_nt_8x8_vs_lib8(int k, float *alpha, float *A, float *B, float *beta, float *C, float *D, int km, int kn); +void kernel_sgemm_nt_8x8_gen_lib8(int k, float *alpha, float *A, float *B, float *beta, int offsetC, float *C, int sdc, int offsetD, float *D, int sdd, int m0, int m1, int n0, int n1); +void kernel_sgemm_nn_8x8_lib8(int k, float *alpha, float *A, int offsetB, float *B, int sdb, float *beta, float *C, float *D); +void kernel_sgemm_nn_8x8_vs_lib8(int k, float *alpha, float *A, int offsetB, float *B, int sdb, float *beta, float *C, float *D, int km, int kn); +void kernel_sgemm_nn_8x8_gen_lib8(int k, float *alpha, float *A, int offsetB, float *B, int sdb, float *beta, int offsetC, float *C, int sdc, int offsetD, float *D, int sdd, int m0, int m1, int n0, int n1); +void kernel_ssyrk_nt_l_8x8_lib8(int k, float *alpha, float *A, float *B, float *beta, float *C, float *D); +void kernel_ssyrk_nt_l_8x8_vs_lib8(int k, float *alpha, float *A, float *B, float *beta, float *C, float *D, int km, int kn); +void kernel_spotrf_nt_l_8x8_lib8(int k, float *A, float *B, float *C, float *D, float *inv_diag_D); +void kernel_spotrf_nt_l_8x8_vs_lib8(int k, float *A, float *B, float *C, float *D, float *inv_diag_D, int km, int kn); +void kernel_strsm_nt_rl_inv_8x8_lib8(int k, float *A, float *B, float *C, float *D, float *E, float *inv_diag_E); +void kernel_strsm_nt_rl_inv_8x8_vs_lib8(int k, float *A, float *B, float *C, float *D, float *E, float *inv_diag_E, int km, int kn); +void kernel_sgemm_strsm_nt_rl_inv_8x8_lib8(int kp, float *Ap, float *Bp, int km_, float *Am, float *Bm, float *C, float *D, float *E, float *inv_diag_E); +void kernel_sgemm_strsm_nt_rl_inv_8x8_vs_lib8(int kp, float *Ap, float *Bp, int km_, float *Am, float *Bm, float *C, float *D, float *E, float *inv_diag_E, int km, int kn); +void kernel_ssyrk_spotrf_nt_l_8x8_vs_lib8(int kp, float *Ap, float *Bp, int km_, float *Am, float *Bm, float *C, float *D, float *inv_diag_D, int km, int kn); +void kernel_ssyrk_spotrf_nt_l_8x8_lib8(int kp, float *Ap, float *Bp, int km_, float *Am, float *Bm, float *C, float *D, float *inv_diag_D); + +// 8x4 +void kernel_sgemm_nt_8x4_lib8(int k, float *alpha, float *A, float *B, float *beta, float *C, float *D); +void kernel_sgemm_nt_8x4_vs_lib8(int k, float *alpha, float *A, float *B, float *beta, float *C, float *D, int km, int kn); +void kernel_sgemm_nt_8x4_gen_lib8(int k, float *alpha, float *A, float *B, float *beta, int offsetC, float *C, int sdc, int offsetD, float *D, int sdd, int m0, int m1, int n0, int n1); +void kernel_sgemm_nn_8x4_lib8(int k, float *alpha, float *A, int offsetB, float *B, int sdb, float *beta, float *C, float *D); +void kernel_sgemm_nn_8x4_vs_lib8(int k, float *alpha, float *A, int offsetB, float *B, int sdb, float *beta, float *C, float *D, int km, int kn); +void kernel_sgemm_nn_8x4_gen_lib8(int k, float *alpha, float *A, int offsetB, float *B, int sdb, float *beta, int offsetC, float *C, int sdc, int offsetD, float *D, int sdd, int m0, int m1, int n0, int n1); +//void kernel_ssyrk_nt_l_8x4_lib8(int k, float *alpha, float *A, float *B, float *beta, float *C, float *D); +void kernel_ssyrk_nt_l_8x4_vs_lib8(int k, float *alpha, float *A, float *B, float *beta, float *C, float *D, int km, int kn); +void kernel_spotrf_nt_l_8x4_lib8(int k, float *A, float *B, float *C, float *D, float *inv_diag_D); +void kernel_spotrf_nt_l_8x4_vs_lib8(int k, float *A, float *B, float *C, float *D, float *inv_diag_D, int km, int kn); +void kernel_strsm_nt_rl_inv_8x4_lib8(int k, float *A, float *B, float *C, float *D, float *E, float *inv_diag_E); +void kernel_strsm_nt_rl_inv_8x4_vs_lib8(int k, float *A, float *B, float *C, float *D, float *E, float *inv_diag_E, int km, int kn); +void kernel_sgemm_strsm_nt_rl_inv_8x4_lib8(int kp, float *Ap, float *Bp, int km_, float *Am, float *Bm, float *C, float *D, float *E, float *inv_diag_E); +void kernel_sgemm_strsm_nt_rl_inv_8x4_vs_lib8(int kp, float *Ap, float *Bp, int km_, float *Am, float *Bm, float *C, float *D, float *E, float *inv_diag_E, int km, int kn); +void kernel_ssyrk_spotrf_nt_l_8x4_vs_lib8(int kp, float *Ap, float *Bp, int km_, float *Am, float *Bm, float *C, float *D, float *inv_diag_D, int km, int kn); +void kernel_ssyrk_spotrf_nt_l_8x4_lib8(int kp, float *Ap, float *Bp, int km_, float *Am, float *Bm, float *C, float *D, float *inv_diag_D); +void kernel_strmm_nn_rl_8x4_lib8(int k, float *alpha, float *A, int offsetB, float *B, int sdb, float *D); +void kernel_strmm_nn_rl_8x4_vs_lib8(int k, float *alpha, float *A, int offsetB, float *B, int sdb, float *D, int km, int kn); +void kernel_strmm_nn_rl_8x4_gen_lib8(int k, float *alpha, float *A, int offsetB, float *B, int sdb, int offsetD, float *D, int sdd, int m0, int m1, int n0, int n1); +void kernel_strmm_nt_ru_8x4_lib8(int k, float *alpha, float *A, float *B, float *D); +void kernel_strmm_nt_ru_8x4_vs_lib8(int k, float *alpha, float *A, float *B, float *D, int km, int kn); + +// 4x8 +void kernel_sgemm_nt_4x8_lib8(int k, float *alpha, float *A, float *B, float *beta, float *C, float *D); +void kernel_sgemm_nt_4x8_vs_lib8(int k, float *alpha, float *A, float *B, float *beta, float *C, float *D, int km, int kn); +void kernel_sgemm_nt_4x8_gen_lib8(int k, float *alpha, float *A, float *B, float *beta, int offsetC, float *C, int sdc, int offsetD, float *D, int sdd, int m0, int m1, int n0, int n1); +void kernel_strsm_nt_rl_inv_4x8_lib8(int k, float *A, float *B, float *C, float *D, float *E, float *inv_diag_E); +void kernel_strsm_nt_rl_inv_4x8_vs_lib8(int k, float *A, float *B, float *C, float *D, float *E, float *inv_diag_E, int km, int kn); + +// 8 +void kernel_sgemv_n_8_lib8(int k, float *alpha, float *A, float *x, float *beta, float *y, float *z); +void kernel_sgemv_n_8_vs_lib8(int k, float *alpha, float *A, float *x, float *beta, float *y, float *z, int k1); +void kernel_sgemv_n_8_gen_lib8(int kmax, float *alpha, float *A, float *x, float *beta, float *y, float *z, int k0, int k1); +void kernel_sgemv_t_8_lib8(int k, float *alpha, int offsetA, float *A, int sda, float *x, float *beta, float *y, float *z); +void kernel_sgemv_t_8_vs_lib8(int k, float *alpha, int offsetA, float *A, int sda, float *x, float *beta, float *y, float *z, int k1); +void kernel_sgemv_t_4_lib8(int k, float *alpha, int offsetA, float *A, int sda, float *x, float *beta, float *y, float *z); +void kernel_sgemv_t_4_vs_lib8(int k, float *alpha, int offsetA, float *A, int sda, float *x, float *beta, float *y, float *z, int k1); +void kernel_strsv_ln_inv_8_lib8(int k, float *A, float *inv_diag_A, float *x, float *y, float *z); +void kernel_strsv_ln_inv_8_vs_lib8(int k, float *A, float *inv_diag_A, float *x, float *y, float *z, int km, int kn); +void kernel_strsv_lt_inv_8_lib8(int k, float *A, int sda, float *inv_diag_A, float *x, float *y, float *z); +void kernel_strsv_lt_inv_8_vs_lib8(int k, float *A, int sda, float *inv_diag_A, float *x, float *y, float *z, int km, int kn); +void kernel_sgemv_nt_4_lib8(int kmax, float *alpha_n, float *alpha_t, float *A, int sda, float *x_n, float *x_t, float *beta_t, float *y_t, float *z_n, float *z_t); +void kernel_sgemv_nt_4_vs_lib8(int kmax, float *alpha_n, float *alpha_t, float *A, int sda, float *x_n, float *x_t, float *beta_t, float *y_t, float *z_n, float *z_t, int km); +void kernel_ssymv_l_4l_lib8(int kmax, float *alpha, float *A, int sda, float *x, float *z); +void kernel_ssymv_l_4r_lib8(int kmax, float *alpha, float *A, int sda, float *x, float *z); +void kernel_ssymv_l_4l_gen_lib8(int kmax, float *alpha, int offA, float *A, int sda, float *x, float *z, int km); +void kernel_ssymv_l_4r_gen_lib8(int kmax, float *alpha, int offA, float *A, int sda, float *x, float *z, int km); + +// -------- aux + +// ---- copy + +// lib4 +// +void kernel_sgecpsc_4_0_lib4(int kmax, float *alpha, float *A, float *B); +void kernel_sgecp_4_0_lib4(int kmax, float *A, float *B); + +void kernel_sgecpsc_4_1_lib4(int kmax, float *alpha, float *A0, int sda, float *B); +void kernel_sgecp_4_1_lib4(int kmax, float *A0, int sda, float *B); +void kernel_sgecpsc_4_2_lib4(int kmax, float *alpha, float *A0, int sda, float *B); +void kernel_sgecp_4_2_lib4(int kmax, float *A0, int sda, float *B); +void kernel_sgecpsc_4_3_lib4(int kmax, float *alpha, float *A0, int sda, float *B); +void kernel_sgecp_4_3_lib4(int kmax, float *A0, int sda, float *B); + +void kernel_sgecpsc_3_0_lib4(int kmax, float *alpha, float *A, float *B); +void kernel_sgecp_3_0_lib4(int kmax, float *A, float *B); +void kernel_sgecpsc_3_2_lib4(int kmax, float *alpha, float *A0, int sda, float *B); +void kernel_sgecp_3_2_lib4(int kmax, float *A0, int sda, float *B); +void kernel_sgecpsc_3_3_lib4(int kmax, float *alpha, float *A0, int sda, float *B); +void kernel_sgecp_3_3_lib4(int kmax, float *A0, int sda, float *B); + +void kernel_sgecpsc_2_0_lib4(int kmax, float *alpha, float *A, float *B); +void kernel_sgecp_2_0_lib4(int kmax, float *A, float *B); +void kernel_sgecpsc_2_3_lib4(int kmax, float *alpha, float *A0, int sda, float *B); +void kernel_sgecp_2_3_lib4(int kmax, float *A0, int sda, float *B); + +void kernel_sgecpsc_1_0_lib4(int kmax, float *alpha, float *A, float *B); +void kernel_sgecp_1_0_lib4(int kmax, float *A, float *B); + +// lib8 +// +void kernel_sgecp_8_0_lib8(int m, float *A, float *B); +void kernel_sgecp_8_0_gen_lib8(int m, float *A, float *B, int m1); +void kernel_sgecp_8_0_gen_u_lib8(int m, float *A, float *B, int m1); + +void kernel_sgesc_8_0_lib8(int m, float *alpha, float *A); +void kernel_sgesc_8_0_gen_lib8(int m, float *alpha, float *A, int m1); +void kernel_sgesc_8_0_gen_u_lib8(int m, float *alpha, float *A, int m1); + +void kernel_sgecpsc_8_0_lib8(int m, float *alpha, float *A, float *B); +void kernel_sgecpsc_8_0_gen_lib8(int m, float *alpha, float *A, float *B, int m1); +void kernel_sgecpsc_8_0_gen_u_lib8(int m, float *alpha, float *A, float *B, int m1); + +void kernel_sgecp_8_1_lib8(int m, float *A, int sda, float *B); +void kernel_sgecp_8_1_gen_lib8(int m, float *A, int sda, float *B, int m1); +void kernel_sgecpsc_8_1_lib8(int m, float *alpha, float *A, int sda, float *B); +void kernel_sgecpsc_8_1_gen_lib8(int m, float *alpha, float *A, int sda, float *B, int m1); + +void kernel_sgecp_8_2_lib8(int m, float *A, int sda, float *B); +void kernel_sgecp_8_2_gen_lib8(int m, float *A, int sda, float *B, int m1); +void kernel_sgecpsc_8_2_lib8(int m, float *alpha, float *A, int sda, float *B); +void kernel_sgecpsc_8_2_gen_lib8(int m, float *alpha, float *A, int sda, float *B, int m1); + +void kernel_sgecp_8_3_lib8(int m, float *A, int sda, float *B); +void kernel_sgecp_8_3_gen_lib8(int m, float *A, int sda, float *B, int m1); +void kernel_sgecpsc_8_3_lib8(int m, float *alpha, float *A, int sda, float *B); +void kernel_sgecpsc_8_3_gen_lib8(int m, float *alpha, float *A, int sda, float *B, int m1); + +void kernel_sgecp_8_4_lib8(int m, float *A, int sda, float *B); +void kernel_sgecp_8_4_gen_lib8(int m, float *A, int sda, float *B, int m1); +void kernel_sgecpsc_8_4_lib8(int m, float *alpha, float *A, int sda, float *B); +void kernel_sgecpsc_8_4_gen_lib8(int m, float *alpha, float *A, int sda, float *B, int m1); + +void kernel_sgecp_8_5_lib8(int m, float *A, int sda, float *B); +void kernel_sgecp_8_5_gen_lib8(int m, float *A, int sda, float *B, int m1); +void kernel_sgecpsc_8_5_lib8(int m, float *alpha, float *A, int sda, float *B); +void kernel_sgecpsc_8_5_gen_lib8(int m, float *alpha, float *A, int sda, float *B, int m1); + +void kernel_sgecp_8_6_lib8(int m, float *A, int sda, float *B); +void kernel_sgecp_8_6_gen_lib8(int m, float *A, int sda, float *B, int m1); +void kernel_sgecpsc_8_6_lib8(int m, float *alpha, float *A, int sda, float *B); +void kernel_sgecpsc_8_6_gen_lib8(int m, float *alpha, float *A, int sda, float *B, int m1); + +void kernel_sgecp_8_7_lib8(int m, float *A, int sda, float *B); +void kernel_sgecp_8_7_gen_lib8(int m, float *A, int sda, float *B, int m1); +void kernel_sgecpsc_8_7_lib8(int m, float *alpha, float *A, int sda, float *B); +void kernel_sgecpsc_8_7_gen_lib8(int m, float *alpha, float *A, int sda, float *B, int m1); + +// transpose +// +void kernel_sgetr_8_0_lib8(int m, float *A, int sda, float *B); +void kernel_sgetr_8_0_gen_lib8(int m, float *A, int sda, float *B, int m1); +void kernel_sgetr_8_1_lib8(int m, float *A, int sda, float *B); +void kernel_sgetr_8_1_gen_lib8(int m, float *A, int sda, float *B, int m1); +void kernel_sgetr_8_2_lib8(int m, float *A, int sda, float *B); +void kernel_sgetr_8_2_gen_lib8(int m, float *A, int sda, float *B, int m1); +void kernel_sgetr_8_3_lib8(int m, float *A, int sda, float *B); +void kernel_sgetr_8_3_gen_lib8(int m, float *A, int sda, float *B, int m1); +void kernel_sgetr_8_4_lib8(int m, float *A, int sda, float *B); +void kernel_sgetr_8_4_gen_lib8(int m, float *A, int sda, float *B, int m1); +void kernel_sgetr_8_5_lib8(int m, float *A, int sda, float *B); +void kernel_sgetr_8_5_gen_lib8(int m, float *A, int sda, float *B, int m1); +void kernel_sgetr_8_6_lib8(int m, float *A, int sda, float *B); +void kernel_sgetr_8_6_gen_lib8(int m, float *A, int sda, float *B, int m1); +void kernel_sgetr_8_7_lib8(int m, float *A, int sda, float *B); +void kernel_sgetr_8_7_gen_lib8(int m, float *A, int sda, float *B, int m1); + +// add +// +void kernel_sgead_8_0_lib8(int m, float *alpha, float *A, float *B); +void kernel_sgead_8_0_gen_lib8(int m, float *alpha, float *A, float *B, int m1); +void kernel_sgead_8_1_lib8(int m, float *alpha, float *A, int sda, float *B); +void kernel_sgead_8_1_gen_lib8(int m, float *alpha, float *A, int sda, float *B, int m1); +void kernel_sgead_8_2_lib8(int m, float *alpha, float *A, int sda, float *B); +void kernel_sgead_8_2_gen_lib8(int m, float *alpha, float *A, int sda, float *B, int m1); +void kernel_sgead_8_3_lib8(int m, float *alpha, float *A, int sda, float *B); +void kernel_sgead_8_3_gen_lib8(int m, float *alpha, float *A, int sda, float *B, int m1); +void kernel_sgead_8_4_lib8(int m, float *alpha, float *A, int sda, float *B); +void kernel_sgead_8_4_gen_lib8(int m, float *alpha, float *A, int sda, float *B, int m1); +void kernel_sgead_8_5_lib8(int m, float *alpha, float *A, int sda, float *B); +void kernel_sgead_8_5_gen_lib8(int m, float *alpha, float *A, int sda, float *B, int m1); +void kernel_sgead_8_6_lib8(int m, float *alpha, float *A, int sda, float *B); +void kernel_sgead_8_6_gen_lib8(int m, float *alpha, float *A, int sda, float *B, int m1); +void kernel_sgead_8_7_lib8(int m, float *alpha, float *A, int sda, float *B); +void kernel_sgead_8_7_gen_lib8(int m, float *alpha, float *A, int sda, float *B, int m1); + + +// +// lib4 +// + + + +// level 2 BLAS +// 4 +void kernel_sgemv_n_4_lib4(int k, float *alpha, float *A, float *x, float *beta, float *y, float *z); +void kernel_sgemv_n_4_vs_lib4(int k, float *alpha, float *A, float *x, float *beta, float *y, float *z, int k1); +void kernel_sgemv_n_4_gen_lib4(int kmax, float *alpha, float *A, float *x, float *beta, float *y, float *z, int k0, int k1); +void kernel_sgemv_t_4_lib4(int k, float *alpha, int offsetA, float *A, int sda, float *x, float *beta, float *y, float *z); +void kernel_sgemv_t_4_vs_lib4(int k, float *alpha, int offsetA, float *A, int sda, float *x, float *beta, float *y, float *z, int k1); +void kernel_strsv_ln_inv_4_lib4(int k, float *A, float *inv_diag_A, float *x, float *y, float *z); +void kernel_strsv_ln_inv_4_vs_lib4(int k, float *A, float *inv_diag_A, float *x, float *y, float *z, int km, int kn); +void kernel_strsv_lt_inv_4_lib4(int k, float *A, int sda, float *inv_diag_A, float *x, float *y, float *z); +void kernel_strsv_lt_inv_3_lib4(int k, float *A, int sda, float *inv_diag_A, float *x, float *y, float *z); +void kernel_strsv_lt_inv_2_lib4(int k, float *A, int sda, float *inv_diag_A, float *x, float *y, float *z); +void kernel_strsv_lt_inv_1_lib4(int k, float *A, int sda, float *inv_diag_A, float *x, float *y, float *z); +void kernel_strmv_un_4_lib4(int k, float *A, float *x, float *z); +void kernel_strmv_ut_4_lib4(int k, float *A, int sda, float *x, float *z); +void kernel_strmv_ut_4_vs_lib4(int k, float *A, int sda, float *x, float *z, int km); +void kernel_sgemv_nt_6_lib4(int kmax, float *alpha_n, float *alpha_t, float *A, int sda, float *x_n, float *x_t, float *beta_t, float *y_t, float *z_n, float *z_t); +void kernel_sgemv_nt_4_lib4(int kmax, float *alpha_n, float *alpha_t, float *A, int sda, float *x_n, float *x_t, float *beta_t, float *y_t, float *z_n, float *z_t); +void kernel_sgemv_nt_4_vs_lib4(int kmax, float *alpha_n, float *alpha_t, float *A, int sda, float *x_n, float *x_t, float *beta_t, float *y_t, float *z_n, float *z_t, int km); +void kernel_ssymv_l_4_lib4(int kmax, float *alpha, float *A, int sda, float *x_n, float *z_n); +void kernel_ssymv_l_4_gen_lib4(int kmax, float *alpha, int offA, float *A, int sda, float *x_n, float *z_n, int km); + + + +// level 3 BLAS +// 12x4 +void kernel_sgemm_nt_16x4_lib4(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd); // +void kernel_sgemm_nt_16x4_vs_lib4(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, int m1, int n1); // +void kernel_strsm_nt_rl_inv_16x4_lib4(int k, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, float *E, float *inv_diag_E); +void kernel_strsm_nt_rl_inv_16x4_vs_lib4(int k, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, float *E, float *inv_diag_E, int m1, int n1); +// 12x4 +void kernel_sgemm_nt_12x4_lib4(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd); // +void kernel_sgemm_nt_12x4_vs_lib4(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, int m1, int n1); // +void kernel_strsm_nt_rl_inv_12x4_lib4(int k, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, float *E, float *inv_diag_E); +void kernel_strsm_nt_rl_inv_12x4_vs_lib4(int k, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, float *E, float *inv_diag_E, int m1, int n1); +// 8x8 +void kernel_sgemm_nt_8x8_lib4(int k, float *alpha, float *A, int sda, float *B, int sdb, float *beta, float *C, int sdc, float *D, int sdd); // +void kernel_sgemm_nt_8x8_vs_lib4(int k, float *alpha, float *A, int sda, float *B, int sdb, float *beta, float *C, int sdc, float *D, int sdd, int m1, int n1); // +void kernel_sgemm_nn_8x8_lib4(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, float *beta, float *C, int sdc, float *D, int sdd); // +void kernel_sgemm_nn_8x8_vs_lib4(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, float *beta, float *C, int sdc, float *D, int sdd, int m1, int n1); // +// 8x4 +void kernel_sgemm_nt_8x4_lib4(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd); // +void kernel_sgemm_nt_8x4_vs_lib4(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, int m1, int n1); // +void kernel_sgemm_nn_8x4_lib4(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, float *beta, float *C, int sdc, float *D, int sdd); // +void kernel_sgemm_nn_8x4_vs_lib4(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, float *beta, float *C, int sdc, float *D, int sdd, int m1, int n1); // +void kernel_ssyrk_nt_l_8x4_lib4(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd); // +void kernel_ssyrk_nt_l_8x4_vs_lib4(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, int km, int kn); // +void kernel_strsm_nt_rl_inv_8x4_lib4(int k, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, float *E, float *inv_diag_E); +void kernel_strsm_nt_rl_inv_8x4_vs_lib4(int k, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, float *E, float *inv_diag_E, int m1, int n1); +// 4x4 +void kernel_sgemm_nt_4x4_lib4(int k, float *alpha, float *A, float *B, float *beta, float *C, float *D); // +void kernel_sgemm_nt_4x4_vs_lib4(int k, float *alpha, float *A, float *B, float *beta, float *C, float *D, int km, int kn); // +void kernel_sgemm_nt_4x4_gen_lib4(int k, float *alpha, float *A, float *B, float *beta, int offsetC, float *C, int sdc, int offsetD, float *D, int sdd, int m0, int m1, int k0, int k1); +void kernel_sgemm_nn_4x4_lib4(int k, float *alpha, float *A, int offsetB, float *B, int sdb, float *beta, float *C, float *D); // +void kernel_sgemm_nn_4x4_vs_lib4(int k, float *alpha, float *A, int offsetB, float *B, int sdb, float *beta, float *C, float *D, int km, int kn); // +void kernel_sgemm_nn_4x4_gen_lib4(int k, float *alpha, float *A, int offsetB, float *B, int sdb, float *beta, int offsetC, float *C, int sdc, int offsetD, float *D, int sdd, int m0, int m1, int n0, int n1); // +void kernel_ssyrk_nt_l_4x4_lib4(int k, float *alpha, float *A, float *B, float *beta, float *C, float *D); // +void kernel_ssyrk_nt_l_4x4_vs_lib4(int k, float *alpha, float *A, float *B, float *beta, float *C, float *D, int km, int kn); // +void kernel_ssyrk_nt_l_4x4_gen_lib4(int k, float *alpha, float *A, float *B, float *beta, int offsetC, float *C, int sdc, int offsetD, float *D, int sdd, int m0, int m1, int n0, int n1); +void kernel_strmm_nt_ru_4x4_lib4(int k, float *alpha, float *A, float *B, float *D); // +void kernel_strmm_nt_ru_4x4_vs_lib4(int k, float *alpha, float *A, float *B, float *D, int km, int kn); // +void kernel_strmm_nn_rl_4x4_lib4(int k, float *alpha, float *A, int offsetB, float *B, int sdb, float *D); +void kernel_strmm_nn_rl_4x4_gen_lib4(int k, float *alpha, float *A, int offsetB, float *B, int sdb, int offsetD, float *D, int sdd, int m0, int m1, int n0, int n1); +void kernel_strsm_nt_rl_inv_4x4_lib4(int k, float *A, float *B, float *beta, float *C, float *D, float *E, float *inv_diag_E); +void kernel_strsm_nt_rl_inv_4x4_vs_lib4(int k, float *A, float *B, float *beta, float *C, float *D, float *E, float *inv_diag_E, int km, int kn); +void kernel_strsm_nt_rl_one_4x4_lib4(int k, float *A, float *B, float *beta, float *C, float *D, float *E); +void kernel_strsm_nt_rl_one_4x4_vs_lib4(int k, float *A, float *B, float *beta, float *C, float *D, float *E, int km, int kn); +void kernel_strsm_nt_ru_inv_4x4_lib4(int k, float *A, float *B, float *beta, float *C, float *D, float *E, float *inv_diag_E); +void kernel_strsm_nt_ru_inv_4x4_vs_lib4(int k, float *A, float *B, float *beta, float *C, float *D, float *E, float *inv_diag_E, int km, int kn); +void kernel_strsm_nt_ru_one_4x4_lib4(int k, float *A, float *B, float *beta, float *C, float *D, float *E); +void kernel_strsm_nt_ru_one_4x4_vs_lib4(int k, float *A, float *B, float *beta, float *C, float *D, float *E, int km, int kn); +void kernel_strsm_nn_ru_inv_4x4_lib4(int k, float *A, float *B, int sdb, float *beta, float *C, float *D, float *E, float *inv_diag_E); +void kernel_strsm_nn_ru_inv_4x4_vs_lib4(int k, float *A, float *B, int sdb, float *beta, float *C, float *D, float *E, float *inv_diag_E, int km, int kn); +void kernel_strsm_nn_ll_one_4x4_lib4(int k, float *A, float *B, int sdb, float *C, float *D, float *E); +void kernel_strsm_nn_ll_one_4x4_vs_lib4(int k, float *A, float *B, int sdb, float *C, float *D, float *E, int km, int kn); +void kernel_strsm_nn_lu_inv_4x4_lib4(int kmax, float *A, float *B, int sdb, float *C, float *D, float *E, float *inv_diag_E); +void kernel_strsm_nn_lu_inv_4x4_vs_lib4(int kmax, float *A, float *B, int sdb, float *C, float *D, float *E, float *inv_diag_E, int km, int kn); +// diag +void kernel_sgemm_diag_right_4_a0_lib4(int kmax, float *alpha, float *A, int sda, float *B, float *D, int sdd); +void kernel_sgemm_diag_right_4_lib4(int kmax, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd); +void kernel_sgemm_diag_right_3_lib4(int kmax, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd); +void kernel_sgemm_diag_right_2_lib4(int kmax, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd); +void kernel_sgemm_diag_right_1_lib4(int kmax, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd); +void kernel_sgemm_diag_left_4_a0_lib4(int kmax, float *alpha, float *A, float *B, float *D); +void kernel_sgemm_diag_left_4_lib4(int kmax, float *alpha, float *A, float *B, float *beta, float *C, float *D); +void kernel_sgemm_diag_left_3_lib4(int kmax, float *alpha, float *A, float *B, float *beta, float *C, float *D); +void kernel_sgemm_diag_left_2_lib4(int kmax, float *alpha, float *A, float *B, float *beta, float *C, float *D); +void kernel_sgemm_diag_left_1_lib4(int kmax, float *alpha, float *A, float *B, float *beta, float *C, float *D); + + + +// LAPACK +// 16x4 +void kernel_spotrf_nt_l_16x4_lib4(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *inv_diag_D); +void kernel_spotrf_nt_l_16x4_vs_lib4(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *inv_diag_D, int m1, int n1); +// 12x4 +void kernel_spotrf_nt_l_12x4_lib4(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *inv_diag_D); +void kernel_spotrf_nt_l_12x4_vs_lib4(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *inv_diag_D, int m1, int n1); +// 8x4 +void kernel_spotrf_nt_l_8x4_lib4(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *inv_diag_D); +void kernel_spotrf_nt_l_8x4_vs_lib4(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *inv_diag_D, int m1, int n1); +// 4x4 +void kernel_spotrf_nt_l_4x4_lib4(int k, float *A, float *B, float *C, float *D, float *inv_diag_D); +void kernel_spotrf_nt_l_4x4_vs_lib4(int k, float *A, float *B, float *C, float *D, float *inv_diag_D, int km, int kn); +void kernel_sgetrf_nn_4x4_lib4(int k, float *A, float *B, int sdb, float *C, float *D, float *inv_diag_D); +void kernel_sgetrf_nn_4x4_vs_lib4(int k, float *A, float *B, int sdb, float *C, float *D, float *inv_diag_D, int km, int kn); +void kernel_sgetrf_pivot_4_lib4(int m, float *pA, int sda, float *inv_diag_A, int* ipiv); +void kernel_sgetrf_pivot_4_vs_lib4(int m, int n, float *pA, int sda, float *inv_diag_A, int* ipiv); + + + +// merged routines +// 4x4 +void kernel_sgemm_strsm_nt_rl_inv_4x4_lib4(int kp, float *Ap, float *Bp, int km_, float *Am, float *Bm, float *C, float *D, float *E, float *inv_diag_E); +void kernel_sgemm_strsm_nt_rl_inv_4x4_vs_lib4(int kp, float *Ap, float *Bp, int km_, float *Am, float *Bm, float *C, float *D, float *E, float *inv_diag_E, int km, int kn); +void kernel_ssyrk_spotrf_nt_l_4x4_vs_lib4(int kp, float *Ap, float *Bp, int km_, float *Am, float *Bm, float *C, float *D, float *inv_diag_D, int km, int kn); +void kernel_ssyrk_spotrf_nt_l_4x4_lib4(int kp, float *Ap, float *Bp, int km_, float *Am, float *Bm, float *C, float *D, float *inv_diag_D); + + + +// auxiliary routines +void kernel_strcp_l_4_0_lib4(int kmax, float *A, float *B); +void kernel_strcp_l_4_1_lib4(int kmax, float *A0, int sda, float *B); +void kernel_strcp_l_4_2_lib4(int kmax, float *A0, int sda, float *B); +void kernel_strcp_l_4_3_lib4(int kmax, float *A0, int sda, float *B); +void kernel_strcp_l_3_0_lib4(int kmax, float *A, float *B); +void kernel_strcp_l_3_2_lib4(int kmax, float *A0, int sda, float *B); +void kernel_strcp_l_3_3_lib4(int kmax, float *A0, int sda, float *B); +void kernel_strcp_l_2_0_lib4(int kmax, float *A, float *B); +void kernel_strcp_l_2_3_lib4(int kmax, float *A0, int sda, float *B); +void kernel_strcp_l_1_0_lib4(int kmax, float *A, float *B); +void kernel_sgead_4_0_lib4(int kmax, float *alpha, float *A, float *B); +void kernel_sgead_4_1_lib4(int kmax, float *alpha, float *A0, int sda, float *B); +void kernel_sgead_4_2_lib4(int kmax, float *alpha, float *A0, int sda, float *B); +void kernel_sgead_4_3_lib4(int kmax, float *alpha, float *A0, int sda, float *B); +void kernel_sgead_3_0_lib4(int kmax, float *alpha, float *A, float *B); +void kernel_sgead_3_2_lib4(int kmax, float *alpha, float *A0, int sda, float *B); +void kernel_sgead_3_3_lib4(int kmax, float *alpha, float *A0, int sda, float *B); +void kernel_sgead_2_0_lib4(int kmax, float *alpha, float *A, float *B); +void kernel_sgead_2_3_lib4(int kmax, float *alpha, float *A0, int sda, float *B); +void kernel_sgead_1_0_lib4(int kmax, float *alpha, float *A, float *B); +// TODO +void kernel_sgeset_4_lib4(int kmax, float alpha, float *A); +void kernel_strset_4_lib4(int kmax, float alpha, float *A); +void kernel_sgetr_4_lib4(int tri, int kmax, int kna, float alpha, float *A, float *C, int sdc); +void kernel_sgetr_3_lib4(int tri, int kmax, int kna, float alpha, float *A, float *C, int sdc); +void kernel_sgetr_2_lib4(int tri, int kmax, int kna, float alpha, float *A, float *C, int sdc); +void kernel_sgetr_1_lib4(int tri, int kmax, int kna, float alpha, float *A, float *C, int sdc); + + + +// pack +// 24 lib 8 +void kernel_spack_nn_24_lib8(int kmax, float *A, int lda, float *B, int sdb); +void kernel_spack_nn_24_vs_lib8(int kmax, float *A, int lda, float *B, int sdb, int m1); +// 16 lib 8 +void kernel_spack_nn_16_lib8(int kmax, float *A, int lda, float *B, int sdb); +void kernel_spack_nn_16_vs_lib8(int kmax, float *A, int lda, float *B, int sdb, int m1); +// 8 lib 8 +void kernel_spack_nn_8_lib8(int kmax, float *A, int lda, float *B); +void kernel_spack_nn_8_vs_lib8(int kmax, float *A, int lda, float *B, int m1); +void kernel_spack_tn_8_lib8(int kmax, float *A, int lda, float *B); +void kernel_spack_tn_8_vs_lib8(int kmax, float *A, int lda, float *B, int m1); +void kernel_spack_tt_8_lib8(int kmax, float *A, int lda, float *B, int sdb); +void kernel_spack_tt_8_vs_lib8(int kmax, float *A, int lda, float *B, int sdb, int m1); +// 8 lib 4 +void kernel_spack_nn_8_lib4(int kmax, float *A, int lda, float *B, int sdb); +void kernel_spack_nn_8_vs_lib4(int kmax, float *A, int lda, float *B, int sdb, int m1); +//void kernel_spack_tt_8_lib4(int kmax, float *A, int lda, float *B, int sdb); +// 4 +void kernel_spack_nn_4_lib4(int kmax, float *A, int lda, float *B); +void kernel_spack_nn_4_vs_lib4(int kmax, float *A, int lda, float *B, int m1); +void kernel_spack_tn_4_lib4(int kmax, float *A, int lda, float *B); +void kernel_spack_tn_4_vs_lib4(int kmax, float *A, int lda, float *B, int m1); +void kernel_spack_tt_4_lib4(int kmax, float *A, int lda, float *B, int sdb); +void kernel_spack_tt_4_vs_lib4(int kmax, float *A, int lda, float *B, int sdb, int m1); +// unpack +// 8 +void kernel_sunpack_nn_8_lib4(int kmax, float *A, int sda, float *B, int ldb); +void kernel_sunpack_nn_8_vs_lib4(int kmax, float *A, int sda, float *B, int ldb, int m1); +//void kernel_sunpack_tt_8_lib4(int kmax, float *A, int sda, float *B, int ldb); +// 4 +void kernel_sunpack_nn_4_lib4(int kmax, float *A, float *B, int ldb); +void kernel_sunpack_nn_4_vs_lib4(int kmax, float *A, float *B, int ldb, int m1); +void kernel_sunpack_nt_4_lib4(int kmax, float *A, float *B, int ldb); +void kernel_sunpack_nt_4_vs_lib4(int kmax, float *A, float *B, int ldb, int m1); +void kernel_sunpack_tt_4_lib4(int kmax, float *A, int sda, float *B, int ldb); + +// panel copy +// 4 +void kernel_spacp_nt_4_lib4(int kmax, float *A, int offsetB, float *B, int sdb); +void kernel_spacp_tn_4_lib4(int kmax, int offsetA, float *A, int sda, float *B); +void kernel_spacp_nn_4_lib4(int kmax, int offsetA, float *A, int sda, float *B); +void kernel_spacp_nn_4_vs_lib4(int kmax, int offsetA, float *A, int sda, float *B, int m1); + + + +/************************************************ +* BLAS API kernels +************************************************/ + +//#if defined(BLAS_API) + +// A, B panel-major bs=8; C, D column-major +// 24x4 +void kernel_sgemm_nt_24x4_lib88cc(int kmax, float *alpha, float *A, int sda, float *B, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nt_24x4_vs_lib88cc(int kmax, float *alpha, float *A, int sda, float *B, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +// 16x4 +void kernel_sgemm_nt_16x4_lib88cc(int kmax, float *alpha, float *A, int sda, float *B, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nt_16x4_vs_lib88cc(int kmax, float *alpha, float *A, int sda, float *B, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +// 8x8 +void kernel_sgemm_nt_8x8_lib88cc(int kmax, float *alpha, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nt_8x8_vs_lib88cc(int kmax, float *alpha, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +void kernel_spotrf_nt_l_8x8_lib88cc(int kmax, float *A, float *B, float *C, int ldc, float *D, int ldd, float *dD); +void kernel_spotrf_nt_l_8x8_vs_lib88cc(int kmax, float *A, float *B, float *C, int ldc, float *D, int ldd, float *dD, int m1, int n1); +void kernel_strsm_nt_rl_inv_8x8_lib88ccc(int kmax, float *A, float *B, float *C, int ldc, float *D, int ldd, float *E, int lde, float *dE); +void kernel_strsm_nt_rl_inv_8x8_vs_lib88ccc(int kmax, float *A, float *B, float *C, int ldc, float *D, int ldd, float *E, int lde, float *dE, int m1, int n1); +// 8x4 +void kernel_sgemm_nt_8x4_lib88cc(int kmax, float *alpha, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nt_8x4_vs_lib88cc(int kmax, float *alpha, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); + +// A, B panel-major bs=4; C, D column-major +// 8x8 +void kernel_sgemm_nt_8x8_lib44cc(int kmax, float *alpha, float *A, int sda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd); +// 8x4 +void kernel_sgemm_nt_8x4_lib44cc(int kmax, float *alpha, float *A, int sda, float *B, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nt_8x4_vs_lib44cc(int kmax, float *alpha, float *A, int sda, float *B, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +void kernel_spotrf_nt_l_8x4_lib44cc(int kmax, float *A, int sda, float *B, float *C, int ldc, float *D, int ldd, float *dD); +void kernel_strsm_nt_rl_inv_8x4_lib44ccc(int kmax, float *A, int sda, float *B, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, float *dE); +void kernel_strsm_nt_rl_inv_8x4_vs_lib44ccc(int kmax, float *A, int sda, float *B, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, float *dE, int m1, int n1); +// 4x4 +void kernel_sgemm_nt_4x4_lib44cc(int kmax, float *alpha, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nt_4x4_vs_lib44cc(int kmax, float *alpha, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +void kernel_strsm_nt_rl_inv_4x4_lib44cc4(int kmax, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd, float *E, float *dE); +void kernel_strsm_nt_rl_inv_4x4_vs_lib44cc4(int kmax, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd, float *E, float *dE, int m1, int n1); +void kernel_strsm_nt_rl_inv_4x4_lib44ccc(int kmax, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, float *dE); +void kernel_strsm_nt_rl_inv_4x4_vs_lib44ccc(int kmax, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, float *dE, int m1, int n1); +void kernel_strsm_nt_rl_one_4x4_lib44cc4(int kmax, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd, float *E); +void kernel_strsm_nt_rl_one_4x4_vs_lib44cc4(int kmax, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd, float *E, int m1, int n1); +void kernel_strsm_nt_ru_inv_4x4_lib44cc4(int kmax, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd, float *E, float *dE); +void kernel_strsm_nt_ru_inv_4x4_vs_lib44cc4(int kmax, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd, float *E, float *dE, int m1, int n1); +void kernel_strsm_nt_ru_one_4x4_lib44cc4(int kmax, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd, float *E); +void kernel_strsm_nt_ru_one_4x4_vs_lib44cc4(int kmax, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd, float *E, int m1, int n1); +void kernel_spotrf_nt_l_4x4_lib44cc(int kmax, float *A, float *B, float *C, int ldc, float *D, int ldd, float *dD); +void kernel_spotrf_nt_l_4x4_vs_lib44cc(int kmax, float *A, float *B, float *C, int ldc, float *D, int ldd, float *dD, int m1, int n1); + +// B panel-major bs=8; A, C, D column-major +// 8x8 +void kernel_sgemm_nt_8x8_libc8cc(int kmax, float *alpha, float *A, int lda, float *B, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nt_8x8_vs_libc8cc(int kmax, float *alpha, float *A, int lda, float *B, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +void kernel_sgemm_tt_8x8_libc8cc(int kmax, float *alpha, float *A, int lda, float *B, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_tt_8x8_vs_libc8cc(int kmax, float *alpha, float *A, int lda, float *B, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +// 4x8 +void kernel_sgemm_nt_4x8_libc8cc(int kmax, float *alpha, float *A, int lda, float *B, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nt_4x8_vs_libc8cc(int kmax, float *alpha, float *A, int lda, float *B, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +void kernel_sgemm_tt_4x8_libc8cc(int kmax, float *alpha, float *A, int lda, float *B, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_tt_4x8_vs_libc8cc(int kmax, float *alpha, float *A, int lda, float *B, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); + +// B panel-major bs=4; A, C, D column-major +// 8x8 +void kernel_sgemm_nt_8x8_libc4cc(int kmax, float *alpha, float *A, int lda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nt_8x8_vs_libc4cc(int kmax, float *alpha, float *A, int lda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +void kernel_sgemm_tt_8x8_libc4cc(int kmax, float *alpha, float *A, int lda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_tt_8x8_vs_libc4cc(int kmax, float *alpha, float *A, int lda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +// 4x8 +void kernel_sgemm_nt_4x8_libc4cc(int kmax, float *alpha, float *A, int lda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nt_4x8_vs_libc4cc(int kmax, float *alpha, float *A, int lda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +void kernel_sgemm_tt_4x8_libc4cc(int kmax, float *alpha, float *A, int lda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_tt_4x8_vs_libc4cc(int kmax, float *alpha, float *A, int lda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +// 4x4 +void kernel_sgemm_nt_4x4_libc4cc(int kmax, float *alpha, float *A, int lda, float *B, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nt_4x4_vs_libc4cc(int kmax, float *alpha, float *A, int lda, float *B, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +void kernel_sgemm_tt_4x4_libc4cc(int kmax, float *alpha, float *A, int lda, float *B, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_tt_4x4_vs_libc4cc(int kmax, float *alpha, float *A, int lda, float *B, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); + +// A panel-major bs=8; B, C, D column-major +// 24x4 +void kernel_sgemm_nn_24x4_lib8ccc(int kmax, float *alpha, float *A, int lda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nn_24x4_vs_lib8ccc(int kmax, float *alpha, float *A, int lda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +void kernel_sgemm_nt_24x4_lib8ccc(int kmax, float *alpha, float *A, int lda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nt_24x4_vs_lib8ccc(int kmax, float *alpha, float *A, int lda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +// 16x4 +void kernel_sgemm_nn_16x4_lib8ccc(int kmax, float *alpha, float *A, int lda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nn_16x4_vs_lib8ccc(int kmax, float *alpha, float *A, int lda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +void kernel_sgemm_nt_16x4_lib8ccc(int kmax, float *alpha, float *A, int lda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nt_16x4_vs_lib8ccc(int kmax, float *alpha, float *A, int lda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +// 8x8 +void kernel_sgemm_nn_8x8_lib8ccc(int kmax, float *alpha, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nn_8x8_vs_lib8ccc(int kmax, float *alpha, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +void kernel_sgemm_nt_8x8_lib8ccc(int kmax, float *alpha, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nt_8x8_vs_lib8ccc(int kmax, float *alpha, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +// 8x4 +void kernel_sgemm_nn_8x4_lib8ccc(int kmax, float *alpha, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nn_8x4_vs_lib8ccc(int kmax, float *alpha, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +void kernel_sgemm_nt_8x4_lib8ccc(int kmax, float *alpha, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nt_8x4_vs_lib8ccc(int kmax, float *alpha, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); + +// A panel-major bs=4; B, C, D column-major +// 8x8 +void kernel_sgemm_nn_8x8_lib4ccc(int kmax, float *alpha, float *A, int sda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nt_8x8_lib4ccc(int kmax, float *alpha, float *A, int sda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd); +// 8x4 +void kernel_sgemm_nn_8x4_lib4ccc(int kmax, float *alpha, float *A, int sda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nn_8x4_vs_lib4ccc(int kmax, float *alpha, float *A, int sda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +void kernel_sgemm_nt_8x4_lib4ccc(int kmax, float *alpha, float *A, int sda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nt_8x4_vs_lib4ccc(int kmax, float *alpha, float *A, int sda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +// 4x4 +void kernel_sgemm_nn_4x4_lib4ccc(int kmax, float *alpha, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nn_4x4_vs_lib4ccc(int kmax, float *alpha, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +void kernel_sgemm_nt_4x4_lib4ccc(int kmax, float *alpha, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nt_4x4_vs_lib4ccc(int kmax, float *alpha, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +void kernel_strsm_nn_rl_inv_4x4_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, float *dE); +void kernel_strsm_nn_rl_inv_4x4_vs_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, float *dE, int m1, int n1); +void kernel_strsm_nn_rl_one_4x4_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde); +void kernel_strsm_nn_rl_one_4x4_vs_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, int m1, int n1); +void kernel_strsm_nt_rl_inv_4x4_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, float *dE); +void kernel_strsm_nt_rl_inv_4x4_vs_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, float *dE, int m1, int n1); +void kernel_strsm_nt_rl_one_4x4_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde); +void kernel_strsm_nt_rl_one_4x4_vs_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, int m1, int n1); +void kernel_strsm_nn_ru_inv_4x4_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, float *dE); +void kernel_strsm_nn_ru_inv_4x4_vs_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, float *dE, int m1, int n1); +void kernel_strsm_nn_ru_one_4x4_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde); +void kernel_strsm_nn_ru_one_4x4_vs_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, int m1, int n1); +void kernel_strsm_nt_ru_inv_4x4_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, float *dE); +void kernel_strsm_nt_ru_inv_4x4_vs_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, float *dE, int m1, int n1); +void kernel_strsm_nt_ru_one_4x4_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde); +void kernel_strsm_nt_ru_one_4x4_vs_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, int m1, int n1); + +// A, C, D panel-major; B, E column-major +// TODO merge with above +// 4x4 +void kernel_strsm_nn_rl_inv_4x4_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde, float *dE); +void kernel_strsm_nn_rl_inv_4x4_vs_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde, float *dE, int m1, int n1); +void kernel_strsm_nn_rl_one_4x4_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde); +void kernel_strsm_nn_rl_one_4x4_vs_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde, int m1, int n1); +void kernel_strsm_nn_ru_inv_4x4_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde, float *dE); +void kernel_strsm_nn_ru_inv_4x4_vs_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde, float *dE, int m1, int n1); +void kernel_strsm_nn_ru_one_4x4_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde); +void kernel_strsm_nn_ru_one_4x4_vs_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde, int m1, int n1); +void kernel_strsm_nt_rl_inv_4x4_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde, float *dE); +void kernel_strsm_nt_rl_inv_4x4_vs_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde, float *dE, int m1, int n1); +void kernel_strsm_nt_rl_one_4x4_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde); +void kernel_strsm_nt_rl_one_4x4_vs_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde, int m1, int n1); +void kernel_strsm_nt_ru_inv_4x4_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde, float *dE); +void kernel_strsm_nt_ru_inv_4x4_vs_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde, float *dE, int m1, int n1); +void kernel_strsm_nt_ru_one_4x4_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde); +void kernel_strsm_nt_ru_one_4x4_vs_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde, int m1, int n1); + +// A, B, C, D column-major +void kernel_sgemm_nn_4x4_libcccc(int kmax, float *alpha, float *A, int lda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nn_4x4_vs_libcccc(int kmax, float *alpha, float *A, int lda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +void kernel_sgemm_nt_4x4_libcccc(int kmax, float *alpha, float *A, int lda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nt_4x4_vs_libcccc(int kmax, float *alpha, float *A, int lda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +void kernel_sgemm_tt_4x4_libcccc(int kmax, float *alpha, float *A, int lda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_tt_4x4_vs_libcccc(int kmax, float *alpha, float *A, int lda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); + +// vector +void kernel_sdot_11_lib(int n, float *x, float *y, float *res); +void kernel_saxpy_11_lib(int n, float *alpha, float *x, float *y); + + +//#endif // BLAS_API + + + +// larger kernels +// 24 +void kernel_sgemm_nt_24xn_p0_lib88cc(int n, int k, float *alpha, float *A, int sda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd, float *A_p, float *B_p); + + + + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_S_KERNEL_H_ diff --git a/phonelibs/acados/include/blasfeo/include/blasfeo_stdlib.h b/phonelibs/acados/include/blasfeo/include/blasfeo_stdlib.h new file mode 100644 index 0000000000..9bd248b1d4 --- /dev/null +++ b/phonelibs/acados/include/blasfeo/include/blasfeo_stdlib.h @@ -0,0 +1,62 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_STDLIB_H_ +#define BLASFEO_STDLIB_H_ + +#ifdef __cplusplus +extern "C" { +#endif + + + +#include + +// +void blasfeo_malloc(void **ptr, size_t size); +// +void blasfeo_malloc_align(void **ptr, size_t size); +// +void blasfeo_free(void *ptr); +// +void blasfeo_free_align(void *ptr); + + + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_STDLIB_H_ diff --git a/phonelibs/acados/include/blasfeo/include/blasfeo_target.h b/phonelibs/acados/include/blasfeo/include/blasfeo_target.h new file mode 100644 index 0000000000..51f617a649 --- /dev/null +++ b/phonelibs/acados/include/blasfeo/include/blasfeo_target.h @@ -0,0 +1,73 @@ +#ifndef TARGET_X64_INTEL_HASWELL +#define TARGET_X64_INTEL_HASWELL +#endif + +#ifndef TARGET_NEED_FEATURE_AVX2 +#define TARGET_NEED_FEATURE_AVX2 1 +#endif + +#ifndef TARGET_NEED_FEATURE_FMA +#define TARGET_NEED_FEATURE_FMA 1 +#endif + +#ifndef TARGET_NEED_FEATURE_SSE3 +/* #undef TARGET_NEED_FEATURE_SSE3 */ +#endif + +#ifndef TARGET_NEED_FEATURE_AVX +/* #undef TARGET_NEED_FEATURE_AVX */ +#endif + +#ifndef TARGET_NEED_FEATURE_VFPv3 +/* #undef TARGET_NEED_FEATURE_VFPv3 */ +#endif + +#ifndef TARGET_NEED_FEATURE_NEON +/* #undef TARGET_NEED_FEATURE_NEON */ +#endif + +#ifndef TARGET_NEED_FEATURE_VFPv4 +/* #undef TARGET_NEED_FEATURE_VFPv4 */ +#endif + +#ifndef TARGET_NEED_FEATURE_NEONv2 +/* #undef TARGET_NEED_FEATURE_NEONv2 */ +#endif + +#ifndef LA_HIGH_PERFORMANCE +#define LA_HIGH_PERFORMANCE +#endif + +#ifndef MF_PANELMAJ +#define MF_PANELMAJ +#endif + +#ifndef EXT_DEP +#define ON 1 +#define OFF 0 +#if ON==ON +#define EXT_DEP +#endif +#undef ON +#undef OFF +#endif + +#ifndef BLAS_API +#define ON 1 +#define OFF 0 +#if OFF==ON +#define BLAS_API +#endif +#undef ON +#undef OFF +#endif + +#ifndef FORTRAN_BLAS_API +#define ON 1 +#define OFF 0 +#if OFF==ON +#define FORTRAN_BLAS_API +#endif +#undef ON +#undef OFF +#endif diff --git a/phonelibs/acados/include/blasfeo/include/blasfeo_timing.h b/phonelibs/acados/include/blasfeo/include/blasfeo_timing.h new file mode 100644 index 0000000000..5671b888fe --- /dev/null +++ b/phonelibs/acados/include/blasfeo/include/blasfeo_timing.h @@ -0,0 +1,114 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_TIMING_H_ +#define BLASFEO_TIMING_H_ + +//#include + +#if (defined _WIN32 || defined _WIN64) && !(defined __MINGW32__ || defined __MINGW64__) + + /* Use Windows QueryPerformanceCounter for timing. */ + #include + + /** A structure for keeping internal timer data. */ + typedef struct blasfeo_timer_ { + LARGE_INTEGER tic; + LARGE_INTEGER toc; + LARGE_INTEGER freq; + } blasfeo_timer; + +#elif(defined __APPLE__) + + #include + + /** A structure for keeping internal timer data. */ + typedef struct blasfeo_timer_ { + uint64_t tic; + uint64_t toc; + mach_timebase_info_data_t tinfo; + } blasfeo_timer; + +#elif(defined __DSPACE__) + + #include + + typedef struct blasfeo_timer_ { + double time; + } blasfeo_timer; + +#elif(defined __XILINX_NONE_ELF__ || defined __XILINX_ULTRASCALE_NONE_ELF_JAILHOUSE__) + + #include "xtime_l.h" + + typedef struct blasfeo_timer_ { + uint64_t tic; + uint64_t toc; + } blasfeo_timer; + +#else + + /* Use POSIX clock_gettime() for timing on non-Windows machines. */ + #include + + #if __STDC_VERSION__ >= 199901L // C99 Mode + + #include + #include + + typedef struct blasfeo_timer_ { + struct timeval tic; + struct timeval toc; + } blasfeo_timer; + + #else // ANSI C Mode + + /** A structure for keeping internal timer data. */ + typedef struct blasfeo_timer_ { + struct timespec tic; + struct timespec toc; + } blasfeo_timer; + + #endif // __STDC_VERSION__ >= 199901L + +#endif // (defined _WIN32 || defined _WIN64) + +/** A function for measurement of the current time. */ +void blasfeo_tic(blasfeo_timer* t); + +/** A function which returns the elapsed time. */ +double blasfeo_toc(blasfeo_timer* t); + +#endif // BLASFEO_TIMING_H_ diff --git a/phonelibs/acados/include/blasfeo/include/blasfeo_v_aux_ext_dep.h b/phonelibs/acados/include/blasfeo/include/blasfeo_v_aux_ext_dep.h new file mode 100644 index 0000000000..1598551185 --- /dev/null +++ b/phonelibs/acados/include/blasfeo/include/blasfeo_v_aux_ext_dep.h @@ -0,0 +1,83 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_V_AUX_EXT_DEP_H_ +#define BLASFEO_V_AUX_EXT_DEP_H_ + + + +#include "blasfeo_target.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +/************************************************ +* d_aux_extern_depend_lib.c +************************************************/ + +#ifdef EXT_DEP + +void v_zeros(void **ptrA, int size); +// dynamically allocate size bytes of memory aligned to 64-byte boundaries and set accordingly a pointer to void; set allocated memory to zero +void v_zeros_align(void **ptrA, int size); +// free the memory allocated by v_zeros +void v_free(void *ptrA); +// free the memory allocated by v_zeros_aligned +void v_free_align(void *ptrA); +// dynamically allocate size bytes of memory and set accordingly a pointer to char; set allocated memory to zero +void c_zeros(char **ptrA, int size); +// dynamically allocate size bytes of memory aligned to 64-byte boundaries and set accordingly a pointer to char; set allocated memory to zero +void c_zeros_align(char **ptrA, int size); +// free the memory allocated by c_zeros +void c_free(char *ptrA); +// free the memory allocated by c_zeros_aligned +void c_free_align(char *ptrA); + +#endif // EXT_DEP + + + +#ifdef __cplusplus +} +#endif + + + +#endif // BLASFEO_V_AUX_EXT_DEP_H_ diff --git a/phonelibs/acados/include/blasfeo/include/d_blas.h b/phonelibs/acados/include/blasfeo/include/d_blas.h new file mode 100644 index 0000000000..d6f8786721 --- /dev/null +++ b/phonelibs/acados/include/blasfeo/include/d_blas.h @@ -0,0 +1,77 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifdef __cplusplus +extern "C" { +#endif + + + +// headers to reference BLAS and LAPACK routines employed in BLASFEO WR + +// level 1 +double ddot_(int *m, double *x, int *incx, double *y, int *incy); +void dcopy_(int *m, double *x, int *incx, double *y, int *incy); +void daxpy_(int *m, double *alpha, double *x, int *incx, double *y, int *incy); +void dscal_(int *m, double *alpha, double *x, int *incx); +void drot_(int *m, double *x, int *incx, double *y, int *incy, double *c, double *s); +void drotg_(double *a, double *b, double *c, double *s); + +// level 2 +void dgemv_(char *ta, int *m, int *n, double *alpha, double *A, int *lda, double *x, int *incx, double *beta, double *y, int *incy); +void dsymv_(char *uplo, int *m, double *alpha, double *A, int *lda, double *x, int *incx, double *beta, double *y, int *incy); +void dtrmv_(char *uplo, char *trans, char *diag, int *n, double *A, int *lda, double *x, int *incx); +void dtrsv_(char *uplo, char *trans, char *diag, int *n, double *A, int *lda, double *x, int *incx); +void dger_(int *m, int *n, double *alpha, double *x, int *incx, double *y, int *incy, double *A, int *lda); + +// level 3 +void dgemm_(char *ta, char *tb, int *m, int *n, int *k, double *alpha, double *A, int *lda, double *B, int *ldb, double *beta, double *C, int *ldc); +void dsyrk_(char *uplo, char *trans, int *n, int *k, double *alpha, double *A, int *lda, double *beta, double *C, int *ldc); +void dtrmm_(char *side, char *uplo, char *trans, char *diag, int *m, int *n, double *alpha, double *A, int *lda, double *B, int *ldb); +void dtrsm_(char *side, char *uplo, char *trans, char *diag, int *m, int *n, double *alpha, double *A, int *lda, double *B, int *ldb); + +// lapack +void dpotrf_(char *uplo, int *m, double *A, int *lda, int *info); +void dgetrf_(int *m, int *n, double *A, int *lda, int *ipiv, int *info); +void dgeqrf_(int *m, int *n, double *A, int *lda, double *tau, double *work, int *lwork, int *info); +void dgeqr2_(int *m, int *n, double *A, int *lda, double *tau, double *work, int *info); +void dgelqf_(int *m, int *n, double *A, int *lda, double *tau, double *work, int *lwork, int *info); +void dorglq_(int *m, int *n, int *k, double *A, int *lda, double *tau, double *work, int *lwork, int *info); + + + +#ifdef __cplusplus +} +#endif diff --git a/phonelibs/acados/include/blasfeo/include/d_blas_64.h b/phonelibs/acados/include/blasfeo/include/d_blas_64.h new file mode 100644 index 0000000000..4f40d00dfb --- /dev/null +++ b/phonelibs/acados/include/blasfeo/include/d_blas_64.h @@ -0,0 +1,73 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifdef __cplusplus +extern "C" { +#endif + + + +// headers to reference BLAS and LAPACK routines employed in BLASFEO WR + +// level 1 +double ddot_(long long *m, double *x, long long *incx, double *y, long long *incy); +void dcopy_(long long *m, double *x, long long *incx, double *y, long long *incy); +void daxpy_(long long *m, double *alpha, double *x, long long *incx, double *y, long long *incy); +void dscal_(long long *m, double *alpha, double *x, long long *incx); + +// level 2 +void dgemv_(char *ta, long long *m, long long *n, double *alpha, double *A, long long *lda, double *x, long long *incx, double *beta, double *y, long long *incy); +void dsymv_(char *uplo, long long *m, double *alpha, double *A, long long *lda, double *x, long long *incx, double *beta, double *y, long long *incy); +void dtrmv_(char *uplo, char *trans, char *diag, long long *n, double *A, long long *lda, double *x, long long *incx); +void dtrsv_(char *uplo, char *trans, char *diag, long long *n, double *A, long long *lda, double *x, long long *incx); +void dger_(long long *m, long long *n, double *alpha, double *x, long long *incx, double *y, long long *incy, double *A, long long *lda); + +// level 3 +void dgemm_(char *ta, char *tb, long long *m, long long *n, long long *k, double *alpha, double *A, long long *lda, double *B, long long *ldb, double *beta, double *C, long long *ldc); +void dsyrk_(char *uplo, char *trans, long long *n, long long *k, double *alpha, double *A, long long *lda, double *beta, double *C, long long *ldc); +void dtrmm_(char *side, char *uplo, char *trans, char *diag, long long *m, long long *n, double *alpha, double *A, long long *lda, double *B, long long *ldb); +void dtrsm_(char *side, char *uplo, char *trans, char *diag, long long *m, long long *n, double *alpha, double *A, long long *lda, double *B, long long *ldb); + +// lapack +void dpotrf_(char *uplo, long long *m, double *A, long long *lda, long long *info); +void dgetrf_(long long *m, long long *n, double *A, long long *lda, long long *ipiv, long long *info); +void dgeqrf_(long long *m, long long *n, double *A, long long *lda, double *tau, double *work, long long *lwork, long long *info); +void dgeqr2_(long long *m, long long *n, double *A, long long *lda, double *tau, double *work, long long *info); + + + +#ifdef __cplusplus +} +#endif diff --git a/phonelibs/acados/include/blasfeo/include/s_blas.h b/phonelibs/acados/include/blasfeo/include/s_blas.h new file mode 100644 index 0000000000..58b2bb0a1a --- /dev/null +++ b/phonelibs/acados/include/blasfeo/include/s_blas.h @@ -0,0 +1,77 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifdef __cplusplus +extern "C" { +#endif + + + +// headers to reference BLAS and LAPACK routines employed in BLASFEO WR + +// level 1 +float sdot_(int *m, float *x, int *incx, float *y, int *incy); +void scopy_(int *m, float *x, int *incx, float *y, int *incy); +void saxpy_(int *m, float *alpha, float *x, int *incx, float *y, int *incy); +void sscal_(int *m, float *alpha, float *x, int *incx); +void srot_(int *m, float *x, int *incx, float *y, int *incy, float *c, float *s); +void srotg_(float *a, float *b, float *c, float *s); + +// level 2 +void sgemv_(char *ta, int *m, int *n, float *alpha, float *A, int *lda, float *x, int *incx, float *beta, float *y, int *incy); +void ssymv_(char *uplo, int *m, float *alpha, float *A, int *lda, float *x, int *incx, float *beta, float *y, int *incy); +void strmv_(char *uplo, char *trans, char *diag, int *n, float *A, int *lda, float *x, int *incx); +void strsv_(char *uplo, char *trans, char *diag, int *n, float *A, int *lda, float *x, int *incx); +void sger_(int *m, int *n, float *alpha, float *x, int *incx, float *y, int *incy, float *A, int *lda); + +// level 3 +void sgemm_(char *ta, char *tb, int *m, int *n, int *k, float *alpha, float *A, int *lda, float *B, int *ldb, float *beta, float *C, int *ldc); +void ssyrk_(char *uplo, char *trans, int *n, int *k, float *alpha, float *A, int *lda, float *beta, float *C, int *ldc); +void strmm_(char *side, char *uplo, char *transa, char *diag, int *m, int *n, float *alpha, float *A, int *lda, float *B, int *ldb); +void strsm_(char *side, char *uplo, char *transa, char *diag, int *m, int *n, float *alpha, float *A, int *lda, float *B, int *ldb); + +// lapack +void spotrf_(char *uplo, int *m, float *A, int *lda, int *info); +void sgetrf_(int *m, int *n, float *A, int *lda, int *ipiv, int *info); +void sgeqrf_(int *m, int *n, float *A, int *lda, float *tau, float *work, int *lwork, int *info); +void sgeqr2_(int *m, int *n, float *A, int *lda, float *tau, float *work, int *info); +void sgelqf_(int *m, int *n, float *A, int *lda, float *tau, float *work, int *lwork, int *info); +void sorglq_(int *m, int *n, int *k, float *A, int *lda, float *tau, float *work, int *lwork, int *info); + + + +#ifdef __cplusplus +} +#endif diff --git a/phonelibs/acados/include/blasfeo/include/s_blas_64.h b/phonelibs/acados/include/blasfeo/include/s_blas_64.h new file mode 100644 index 0000000000..b9efab6c23 --- /dev/null +++ b/phonelibs/acados/include/blasfeo/include/s_blas_64.h @@ -0,0 +1,73 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifdef __cplusplus +extern "C" { +#endif + + + +// headers to reference BLAS and LAPACK routines employed in BLASFEO WR + +// level 1 +float sdot_(long long *m, float *x, long long *incx, float *y, long long *incy); +void scopy_(long long *m, float *x, long long *incx, float *y, long long *incy); +void saxpy_(long long *m, float *alpha, float *x, long long *incx, float *y, long long *incy); +void sscal_(long long *m, float *alpha, float *x, long long *incx); + +// level 2 +void sgemv_(char *ta, long long *m, long long *n, float *alpha, float *A, long long *lda, float *x, long long *incx, float *beta, float *y, long long *incy); +void ssymv_(char *uplo, long long *m, float *alpha, float *A, long long *lda, float *x, long long *incx, float *beta, float *y, long long *incy); +void strmv_(char *uplo, char *trans, char *diag, long long *n, float *A, long long *lda, float *x, long long *incx); +void strsv_(char *uplo, char *trans, char *diag, long long *n, float *A, long long *lda, float *x, long long *incx); +void sger_(long long *m, long long *n, float *alpha, float *x, long long *incx, float *y, long long *incy, float *A, long long *lda); + +// level 3 +void sgemm_(char *ta, char *tb, long long *m, long long *n, long long *k, float *alpha, float *A, long long *lda, float *B, long long *ldb, float *beta, float *C, long long *ldc); +void ssyrk_(char *uplo, char *trans, long long *n, long long *k, float *alpha, float *A, long long *lda, float *beta, float *C, long long *ldc); +void strmm_(char *side, char *uplo, char *transa, char *diag, long long *m, long long *n, float *alpha, float *A, long long *lda, float *B, long long *ldb); +void strsm_(char *side, char *uplo, char *transa, char *diag, long long *m, long long *n, float *alpha, float *A, long long *lda, float *B, long long *ldb); + +// lapack +void spotrf_(char *uplo, long long *m, float *A, long long *lda, long long *info); +void sgetrf_(long long *m, long long *n, float *A, long long *lda, long long *ipiv, long long *info); +void sgeqrf_(long long *m, long long *n, float *A, long long *lda, float *tau, float *work, long long *lwork, long long *info); +void sgeqr2_(long long *m, long long *n, float *A, long long *lda, float *tau, float *work, long long *info); + + + +#ifdef __cplusplus +} +#endif diff --git a/phonelibs/acados/include/hpipm/include/hpipm_aux_mem.h b/phonelibs/acados/include/hpipm/include/hpipm_aux_mem.h new file mode 100644 index 0000000000..7bd3d7e8bc --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_aux_mem.h @@ -0,0 +1,52 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_AUX_MEM_H_ +#define HPIPM_AUX_MEM_H_ + +#include "hpipm_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +void hpipm_zero_memset(hpipm_size_t memsize, void *mem); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // HPIPM_AUX_MEM_H_ + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_aux_string.h b/phonelibs/acados/include/hpipm/include/hpipm_aux_string.h new file mode 100644 index 0000000000..804cba5dc1 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_aux_string.h @@ -0,0 +1,50 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_AUX_STRING_H_ +#define HPIPM_AUX_STRING_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#define MAX_STR_LEN 5 +int hpipm_strcmp(char *str1, char *str2); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // HPIPM_AUX_STRING_H_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_common.h b/phonelibs/acados/include/hpipm/include/hpipm_common.h new file mode 100644 index 0000000000..0cc96a7b50 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_common.h @@ -0,0 +1,76 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_COMMON_H_ +#define HPIPM_COMMON_H_ + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +typedef size_t hpipm_size_t; + +enum hpipm_mode + { + SPEED_ABS, // focus on speed, absolute IPM formulation + SPEED, // focus on speed, relative IPM formulation + BALANCE, // balanced mode, relative IPM formulation + ROBUST, // focus on robustness, relative IPM formulation + }; + +enum hpipm_status + { + SUCCESS, // found solution satisfying accuracy tolerance + MAX_ITER, // maximum iteration number reached + MIN_STEP, // minimum step length reached + NAN_SOL, // NaN in solution detected + INCONS_EQ, // unconsistent equality constraints + }; + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_COMMON_H_ + + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_cast_qcqp.h b/phonelibs/acados/include/hpipm/include/hpipm_d_cast_qcqp.h new file mode 100644 index 0000000000..0e4c41f221 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_cast_qcqp.h @@ -0,0 +1,71 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_D_CAST_QCQP_H_ +#define HPIPM_D_CAST_QCQP_H_ + + + +#include +#include + +#include "hpipm_d_dense_qcqp.h" +#include "hpipm_d_dense_qcqp_sol.h" +#include "hpipm_d_ocp_qcqp.h" +#include "hpipm_d_ocp_qcqp_dim.h" +#include "hpipm_d_ocp_qcqp_sol.h" + +#ifdef __cplusplus +extern "C" { +#endif + + + +// +void d_cast_qcqp_compute_dim(struct d_ocp_qcqp_dim *ocp_dim, struct d_dense_qcqp_dim *dense_dim); +// +void d_cast_qcqp_cond(struct d_ocp_qcqp *ocp_qp, struct d_dense_qcqp *dense_qp); + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_D_CAST_QCQP_H_ + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_cond.h b/phonelibs/acados/include/hpipm/include/hpipm_d_cond.h new file mode 100644 index 0000000000..5900a2ab10 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_cond.h @@ -0,0 +1,135 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_D_COND_H_ +#define HPIPM_D_COND_H_ + + + +#include +#include + +#include "hpipm_d_dense_qp.h" +#include "hpipm_d_dense_qp_sol.h" +#include "hpipm_d_ocp_qp.h" +#include "hpipm_d_ocp_qp_dim.h" +#include "hpipm_d_ocp_qp_sol.h" + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_cond_qp_arg + { + int cond_last_stage; // condense last stage + int cond_alg; // condensing algorithm: 0 N2-nx3, 1 N3-nx2 + int comp_prim_sol; // primal solution (v) + int comp_dual_sol_eq; // dual solution equality constr (pi) + int comp_dual_sol_ineq; // dual solution inequality constr (lam t) + int square_root_alg; // square root algorithm (faster but requires RSQ>0) + hpipm_size_t memsize; + }; + + + +struct d_cond_qp_ws + { + struct blasfeo_dmat *Gamma; + struct blasfeo_dmat *GammaQ; + struct blasfeo_dmat *L; + struct blasfeo_dmat *Lx; + struct blasfeo_dmat *AL; + struct blasfeo_dvec *Gammab; + struct blasfeo_dvec *l; + struct blasfeo_dvec *tmp_nbgM; + struct blasfeo_dvec *tmp_nuxM; + int bs; // block size + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_cond_qp_arg_memsize(); +// +void d_cond_qp_arg_create(struct d_cond_qp_arg *cond_arg, void *mem); +// +void d_cond_qp_arg_set_default(struct d_cond_qp_arg *cond_arg); +// condensing algorithm: 0 N2-nx3, 1 N3-nx2 +void d_cond_qp_arg_set_cond_alg(int cond_alg, struct d_cond_qp_arg *cond_arg); +// set riccati-like algorithm: 0 classical, 1 square-root +void d_cond_qp_arg_set_ric_alg(int ric_alg, struct d_cond_qp_arg *cond_arg); +// condense last stage: 0 last stage disregarded, 1 last stage condensed too +void d_cond_qp_arg_set_cond_last_stage(int cond_last_stage, struct d_cond_qp_arg *cond_arg); +// +void d_cond_qp_arg_set_comp_prim_sol(int value, struct d_cond_qp_arg *cond_arg); +// +void d_cond_qp_arg_set_comp_dual_sol_eq(int value, struct d_cond_qp_arg *cond_arg); +// +void d_cond_qp_arg_set_comp_dual_sol_ineq(int value, struct d_cond_qp_arg *cond_arg); + +// +void d_cond_qp_compute_dim(struct d_ocp_qp_dim *ocp_dim, struct d_dense_qp_dim *dense_dim); +// +hpipm_size_t d_cond_qp_ws_memsize(struct d_ocp_qp_dim *ocp_dim, struct d_cond_qp_arg *cond_arg); +// +void d_cond_qp_ws_create(struct d_ocp_qp_dim *ocp_dim, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws, void *mem); +// +void d_cond_qp_cond(struct d_ocp_qp *ocp_qp, struct d_dense_qp *dense_qp, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws); +// +void d_cond_qp_cond_lhs(struct d_ocp_qp *ocp_qp, struct d_dense_qp *dense_qp, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws); +// +void d_cond_qp_cond_rhs(struct d_ocp_qp *ocp_qp, struct d_dense_qp *dense_qp, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws); +// +void d_cond_qp_expand_sol(struct d_ocp_qp *ocp_qp, struct d_dense_qp_sol *dense_qp_sol, struct d_ocp_qp_sol *ocp_qp_sol, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws); +// TODO remove +void d_cond_qp_expand_primal_sol(struct d_ocp_qp *ocp_qp, struct d_dense_qp_sol *dense_qp_sol, struct d_ocp_qp_sol *ocp_qp_sol, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws); + +// +void d_cond_qp_update(int *idxc, struct d_ocp_qp *ocp_qp, struct d_dense_qp *dense_qp, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_D_COND_H_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_cond_aux.h b/phonelibs/acados/include/hpipm/include/hpipm_d_cond_aux.h new file mode 100644 index 0000000000..73afba3c7e --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_cond_aux.h @@ -0,0 +1,92 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_D_COND_AUX_H_ +#define HPIPM_D_COND_AUX_H_ + + + +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +// +void d_cond_BAbt(struct d_ocp_qp *ocp_qp, struct blasfeo_dmat *BAbt2, struct blasfeo_dvec *b, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws); +// +void d_cond_BAt(struct d_ocp_qp *ocp_qp, struct blasfeo_dmat *BAbt2, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws); +// +void d_cond_b(struct d_ocp_qp *ocp_qp, struct blasfeo_dvec *b, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws); +// +void d_cond_RSQrq(struct d_ocp_qp *ocp_qp, struct blasfeo_dmat *RSQrq2, struct blasfeo_dvec *rq, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws); +// +void d_cond_RSQ(struct d_ocp_qp *ocp_qp, struct blasfeo_dmat *RSQrq2, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws); +// +void d_cond_rq(struct d_ocp_qp *ocp_qp, struct blasfeo_dvec *rq, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws); +// +void d_cond_DCtd(struct d_ocp_qp *ocp_qp, int *idxb2, struct blasfeo_dmat *DCt2, struct blasfeo_dvec *d2, struct blasfeo_dvec *d_mask2, int *idxs_rev2, struct blasfeo_dvec *Z2, struct blasfeo_dvec *z, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws); +// +void d_cond_DCt(struct d_ocp_qp *ocp_qp, int *idxb2, struct blasfeo_dmat *DCt2, int *idxs_rev2, struct blasfeo_dvec *Z2, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws); +// +void d_cond_d(struct d_ocp_qp *ocp_qp, struct blasfeo_dvec *d2, struct blasfeo_dvec *d_mask2, struct blasfeo_dvec *z, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws); +// +void d_expand_sol(struct d_ocp_qp *ocp_qp, struct d_dense_qp_sol *dense_qp_sol, struct d_ocp_qp_sol *ocp_qp_so, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws); +// +void d_expand_primal_sol(struct d_ocp_qp *ocp_qp, struct d_dense_qp_sol *dense_qp_sol, struct d_ocp_qp_sol *ocp_qp_so, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws); + +// +void d_update_cond_BAbt(int *idxc, struct d_ocp_qp *ocp_qp, struct blasfeo_dmat *BAbt2, struct blasfeo_dvec *b, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws); +// +void d_update_cond_RSQrq_N2nx3(int *idxc, struct d_ocp_qp *ocp_qp, struct blasfeo_dmat *RSQrq2, struct blasfeo_dvec *rq, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws); +// +void d_update_cond_DCtd(int *idxc, struct d_ocp_qp *ocp_qp, int *idxb2, struct blasfeo_dmat *DCt2, struct blasfeo_dvec *d2, int *idxs2, struct blasfeo_dvec *Z2, struct blasfeo_dvec *z, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_D_COND_AUX_H_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_cond_qcqp.h b/phonelibs/acados/include/hpipm/include/hpipm_d_cond_qcqp.h new file mode 100644 index 0000000000..266567bb8d --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_cond_qcqp.h @@ -0,0 +1,129 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_D_COND_QCQP_H_ +#define HPIPM_D_COND_QCQP_H_ + + + +#include +#include + +#include "hpipm_d_dense_qcqp.h" +#include "hpipm_d_dense_qcqp_sol.h" +#include "hpipm_d_ocp_qcqp.h" +#include "hpipm_d_ocp_qcqp_dim.h" +#include "hpipm_d_ocp_qcqp_sol.h" + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_cond_qcqp_arg + { + struct d_cond_qp_arg *qp_arg; + int cond_last_stage; // condense last stage +// int cond_variant; // TODO + int comp_prim_sol; // primal solution (v) + int comp_dual_sol_eq; // dual solution equality constr (pi) + int comp_dual_sol_ineq; // dual solution equality constr (lam t) + int square_root_alg; // square root algorithm (faster but requires RSQ>0) + hpipm_size_t memsize; + }; + + + +struct d_cond_qcqp_ws + { + struct d_cond_qp_ws *qp_ws; + struct blasfeo_dmat *hess_array; // TODO remove + struct blasfeo_dmat *zero_hess; // TODO remove + struct blasfeo_dvec *grad_array; // TODO remove + struct blasfeo_dvec *zero_grad; // TODO remove + struct blasfeo_dvec *tmp_nvc; + struct blasfeo_dvec *tmp_nuxM; + struct blasfeo_dmat *GammaQ; + struct blasfeo_dmat *tmp_DCt; + struct blasfeo_dmat *tmp_nuM_nxM; +// struct blasfeo_dvec *d_qp; +// struct blasfeo_dvec *d_mask_qp; + hpipm_size_t memsize; + }; + + +// +hpipm_size_t d_cond_qcqp_arg_memsize(); +// +void d_cond_qcqp_arg_create(struct d_cond_qcqp_arg *cond_arg, void *mem); +// +void d_cond_qcqp_arg_set_default(struct d_cond_qcqp_arg *cond_arg); +// set riccati-like algorithm: 0 classical, 1 square-root +void d_cond_qcqp_arg_set_ric_alg(int ric_alg, struct d_cond_qcqp_arg *cond_arg); +// condense last stage: 0 last stage disregarded, 1 last stage condensed too +void d_cond_qcqp_arg_set_cond_last_stage(int cond_last_stage, struct d_cond_qcqp_arg *cond_arg); + +// +void d_cond_qcqp_compute_dim(struct d_ocp_qcqp_dim *ocp_dim, struct d_dense_qcqp_dim *dense_dim); +// +hpipm_size_t d_cond_qcqp_ws_memsize(struct d_ocp_qcqp_dim *ocp_dim, struct d_cond_qcqp_arg *cond_arg); +// +void d_cond_qcqp_ws_create(struct d_ocp_qcqp_dim *ocp_dim, struct d_cond_qcqp_arg *cond_arg, struct d_cond_qcqp_ws *cond_ws, void *mem); +// +void d_cond_qcqp_qc(struct d_ocp_qcqp *ocp_qp, struct blasfeo_dmat *Hq2, int *Hq_nzero2, struct blasfeo_dmat *Ct2, struct blasfeo_dvec *d2, struct d_cond_qcqp_arg *cond_arg, struct d_cond_qcqp_ws *cond_ws); +// +void d_cond_qcqp_qc_lhs(struct d_ocp_qcqp *ocp_qp, struct blasfeo_dmat *Hq2, int *Hq_nzero2, struct blasfeo_dmat *Ct2, struct d_cond_qcqp_arg *cond_arg, struct d_cond_qcqp_ws *cond_ws); +// +void d_cond_qcqp_qc_rhs(struct d_ocp_qcqp *ocp_qp, struct blasfeo_dvec *d2, struct d_cond_qcqp_arg *cond_arg, struct d_cond_qcqp_ws *cond_ws); +// +void d_cond_qcqp_cond(struct d_ocp_qcqp *ocp_qp, struct d_dense_qcqp *dense_qp, struct d_cond_qcqp_arg *cond_arg, struct d_cond_qcqp_ws *cond_ws); +// +void d_cond_qcqp_cond_rhs(struct d_ocp_qcqp *ocp_qp, struct d_dense_qcqp *dense_qp, struct d_cond_qcqp_arg *cond_arg, struct d_cond_qcqp_ws *cond_ws); +// +void d_cond_qcqp_cond_lhs(struct d_ocp_qcqp *ocp_qp, struct d_dense_qcqp *dense_qp, struct d_cond_qcqp_arg *cond_arg, struct d_cond_qcqp_ws *cond_ws); +// +void d_cond_qcqp_expand_sol(struct d_ocp_qcqp *ocp_qp, struct d_dense_qcqp_sol *dense_qp_sol, struct d_ocp_qcqp_sol *ocp_qp_sol, struct d_cond_qcqp_arg *cond_arg, struct d_cond_qcqp_ws *cond_ws); + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_D_COND_QCQP_H_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_core_qp_ipm.h b/phonelibs/acados/include/hpipm/include/hpipm_d_core_qp_ipm.h new file mode 100644 index 0000000000..f39d9a9b50 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_core_qp_ipm.h @@ -0,0 +1,101 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_CORE_QP_IPM_ +#define HPIPM_D_CORE_QP_IPM_ + +#include "hpipm_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +struct d_core_qp_ipm_workspace + { + double *v; // primal variables + double *pi; // equality constraints multipliers + double *lam; // inequality constraints multipliers + double *t; // inequality constraints slacks + double *t_inv; // inverse of t + double *v_bkp; // backup of primal variables + double *pi_bkp; // backup of equality constraints multipliers + double *lam_bkp; // backup of inequality constraints multipliers + double *t_bkp; // backup of inequality constraints slacks + double *dv; // step in v + double *dpi; // step in pi + double *dlam; // step in lam + double *dt; // step in t + double *res_g; // q-residuals + double *res_b; // b-residuals + double *res_d; // d-residuals + double *res_m; // m-residuals + double *res_m_bkp; // m-residuals + double *Gamma; // Hessian update + double *gamma; // gradient update + double alpha; // step length + double alpha_prim; // step length + double alpha_dual; // step length + double sigma; // centering XXX + double mu; // duality measuere + double mu_aff; // affine duality measuere + double nc_inv; // 1.0/nc, where nc is the total number of inequality constraints + double nc_mask_inv; // 1.0/nc_mask + double lam_min; // min value in lam vector + double t_min; // min value in t vector + double t_min_inv; // inverse of min value in t vector + double tau_min; // min value of barrier parameter + int nv; // number of primal variables + int ne; // number of equality constraints + int nc; // (twice the) number of (two-sided) inequality constraints + int nc_mask; // total number of ineq constr after masking + int split_step; // use different step for primal and dual variables + int t_lam_min; // clip t and lam also in solution, or only in Gamma computation + hpipm_size_t memsize; // memory size (in bytes) of workspace + }; + + + +// +hpipm_size_t d_memsize_core_qp_ipm(int nv, int ne, int nc); +// +void d_create_core_qp_ipm(int nv, int ne, int nc, struct d_core_qp_ipm_workspace *workspace, void *mem); +// +void d_core_qp_ipm(struct d_core_qp_ipm_workspace *workspace); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // HPIPM_D_CORE_QP_IPM_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_core_qp_ipm_aux.h b/phonelibs/acados/include/hpipm/include/hpipm_d_core_qp_ipm_aux.h new file mode 100644 index 0000000000..30cc824bad --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_core_qp_ipm_aux.h @@ -0,0 +1,68 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_CORE_QP_IPM_AUX_ +#define HPIPM_S_CORE_QP_IPM_AUX_ + +#ifdef __cplusplus +extern "C" { +#endif + +// +void d_compute_Gamma_gamma_qp(double *res_d, double *res_m, struct d_core_qp_ipm_workspace *rws); +// +void d_compute_gamma_qp(double *res_d, double *res_m, struct d_core_qp_ipm_workspace *rws); +// +void d_compute_lam_t_qp(double *res_d, double *res_m, double *dlam, double *dt, struct d_core_qp_ipm_workspace *rws); +// +void d_compute_alpha_qp(struct d_core_qp_ipm_workspace *rws); +// +void d_update_var_qp(struct d_core_qp_ipm_workspace *rws); +// +void d_compute_mu_aff_qp(struct d_core_qp_ipm_workspace *rws); +// +void d_backup_res_m(struct d_core_qp_ipm_workspace *rws); +// +void d_compute_centering_correction_qp(struct d_core_qp_ipm_workspace *rws); +// +void d_compute_centering_qp(struct d_core_qp_ipm_workspace *rws); +// +void d_compute_tau_min_qp(struct d_core_qp_ipm_workspace *rws); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // HPIPM_S_CORE_QP_IPM_AUX_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_dense_qcqp.h b/phonelibs/acados/include/hpipm/include/hpipm_d_dense_qcqp.h new file mode 100644 index 0000000000..3da5716493 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_dense_qcqp.h @@ -0,0 +1,199 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_D_DENSE_QCQP_H_ +#define HPIPM_D_DENSE_QCQP_H_ + + + +#include +#include + +#include "hpipm_d_dense_qcqp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_dense_qcqp + { + struct d_dense_qcqp_dim *dim; + struct blasfeo_dmat *Hv; // hessian of cost & vector work space + struct blasfeo_dmat *A; // equality constraint matrix + struct blasfeo_dmat *Ct; // inequality constraints matrix + struct blasfeo_dmat *Hq; // hessians of quadratic constraints + struct blasfeo_dvec *gz; // gradient of cost & gradient of slacks + struct blasfeo_dvec *b; // equality constraint vector + struct blasfeo_dvec *d; // inequality constraints vector + struct blasfeo_dvec *d_mask; // inequality constraints mask vector + struct blasfeo_dvec *m; // rhs of complementarity condition + struct blasfeo_dvec *Z; // (diagonal) hessian of slacks + int *idxb; // indices of box constrained variables within [u; x] + int *idxs_rev; // index of soft constraints (reverse storage) + int *Hq_nzero; // for each int, the last 3 bits ...abc, {a,b,c}=0 => {R,S,Q}=0 + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +hpipm_size_t d_dense_qcqp_memsize(struct d_dense_qcqp_dim *dim); +// +void d_dense_qcqp_create(struct d_dense_qcqp_dim *dim, struct d_dense_qcqp *qp, void *memory); + +// +void d_dense_qcqp_set(char *field, void *value, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_H(double *H, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_g(double *g, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_A(double *A, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_b(double *b, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_idxb(int *idxb, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_lb(double *lb, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_lb_mask(double *lb, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_ub(double *ub, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_ub_mask(double *ub, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_C(double *C, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_lg(double *lg, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_lg_mask(double *lg, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_ug(double *ug, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_ug_mask(double *ug, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_Hq(double *Hq, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_gq(double *gq, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_uq(double *uq, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_uq_mask(double *uq, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_idxs(int *idxs, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_idxs_rev(int *idxs_rev, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_Zl(double *Zl, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_Zu(double *Zu, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_zl(double *zl, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_zu(double *zu, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_ls(double *ls, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_ls_mask(double *ls, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_us(double *us, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_us_mask(double *us, struct d_dense_qcqp *qp); + +// getters (COLMAJ) + +void d_dense_qcqp_get_H(struct d_dense_qcqp *qp, double *H); +// +void d_dense_qcqp_get_g(struct d_dense_qcqp *qp, double *g); +// +void d_dense_qcqp_get_A(struct d_dense_qcqp *qp, double *A); +// +void d_dense_qcqp_get_b(struct d_dense_qcqp *qp, double *b); +// +void d_dense_qcqp_get_idxb(struct d_dense_qcqp *qp, int *idxb); +// +void d_dense_qcqp_get_lb(struct d_dense_qcqp *qp, double *lb); +// +void d_dense_qcqp_get_lb_mask(struct d_dense_qcqp *qp, double *lb); +// +void d_dense_qcqp_get_ub(struct d_dense_qcqp *qp, double *ub); +// +void d_dense_qcqp_get_ub_mask(struct d_dense_qcqp *qp, double *ub); +// +void d_dense_qcqp_get_C(struct d_dense_qcqp *qp, double *C); +// +void d_dense_qcqp_get_lg(struct d_dense_qcqp *qp, double *lg); +// +void d_dense_qcqp_get_lg_mask(struct d_dense_qcqp *qp, double *lg); +// +void d_dense_qcqp_get_ug(struct d_dense_qcqp *qp, double *ug); +// +void d_dense_qcqp_get_ug_mask(struct d_dense_qcqp *qp, double *ug); +// +void d_dense_qcqp_get_idxs(struct d_dense_qcqp *qp, int *idxs); +// +void d_dense_qcqp_get_idxs_rev(struct d_dense_qcqp *qp, int *idxs_rev); +// +void d_dense_qcqp_get_Zl(struct d_dense_qcqp *qp, double *Zl); +// +void d_dense_qcqp_get_Zu(struct d_dense_qcqp *qp, double *Zu); +// +void d_dense_qcqp_get_zl(struct d_dense_qcqp *qp, double *zl); +// +void d_dense_qcqp_get_zu(struct d_dense_qcqp *qp, double *zu); +// +void d_dense_qcqp_get_ls(struct d_dense_qcqp *qp, double *ls); +// +void d_dense_qcqp_get_ls_mask(struct d_dense_qcqp *qp, double *ls); +// +void d_dense_qcqp_get_us(struct d_dense_qcqp *qp, double *us); +// +void d_dense_qcqp_get_us_mask(struct d_dense_qcqp *qp, double *us); + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_D_DENSE_QCQP_H_ + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_dense_qcqp_dim.h b/phonelibs/acados/include/hpipm/include/hpipm_d_dense_qcqp_dim.h new file mode 100644 index 0000000000..fa8c574a1e --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_dense_qcqp_dim.h @@ -0,0 +1,98 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_DENSE_QCQP_DIM_H_ +#define HPIPM_D_DENSE_QCQP_DIM_H_ + +#include "hpipm_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_dense_qcqp_dim + { + struct d_dense_qp_dim *qp_dim; // dim of qp approximation + int nv; // number of variables + int ne; // number of equality constraints + int nb; // number of box constraints + int ng; // number of general constraints + int nq; // number of quadratic constraints + int nsb; // number of softened box constraints + int nsg; // number of softened general constraints + int nsq; // number of softened quadratic constraints + int ns; // number of softened constraints (nsb+nsg+nsq) TODO number of slacks + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_dense_qcqp_dim_memsize(); +// +void d_dense_qcqp_dim_create(struct d_dense_qcqp_dim *dim, void *memory); +// +void d_dense_qcqp_dim_set(char *field_name, int value, struct d_dense_qcqp_dim *dim); +// +void d_dense_qcqp_dim_set_nv(int value, struct d_dense_qcqp_dim *dim); +// +void d_dense_qcqp_dim_set_ne(int value, struct d_dense_qcqp_dim *dim); +// +void d_dense_qcqp_dim_set_nb(int value, struct d_dense_qcqp_dim *dim); +// +void d_dense_qcqp_dim_set_ng(int value, struct d_dense_qcqp_dim *dim); +// +void d_dense_qcqp_dim_set_nq(int value, struct d_dense_qcqp_dim *dim); +// +void d_dense_qcqp_dim_set_nsb(int value, struct d_dense_qcqp_dim *dim); +// +void d_dense_qcqp_dim_set_nsg(int value, struct d_dense_qcqp_dim *dim); +// +void d_dense_qcqp_dim_set_nsq(int value, struct d_dense_qcqp_dim *dim); +// +void d_dense_qcqp_dim_set_ns(int value, struct d_dense_qcqp_dim *dim); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_D_DENSE_QCQP_DIM_H_ + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_dense_qcqp_ipm.h b/phonelibs/acados/include/hpipm/include/hpipm_d_dense_qcqp_ipm.h new file mode 100644 index 0000000000..fa3f98fa79 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_dense_qcqp_ipm.h @@ -0,0 +1,193 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_D_DENSE_QCQP_IPM_H_ +#define HPIPM_D_DENSE_QCQP_IPM_H_ + + + +#include +#include + +#include +#include +#include +#include +#include + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_dense_qcqp_ipm_arg + { + struct d_dense_qp_ipm_arg *qp_arg; + double mu0; // initial value for duality measure + double alpha_min; // exit cond on step length + double res_g_max; // exit cond on inf norm of residuals + double res_b_max; // exit cond on inf norm of residuals + double res_d_max; // exit cond on inf norm of residuals + double res_m_max; // exit cond on inf norm of residuals + double reg_prim; // reg of primal hessian + double reg_dual; // reg of dual hessian + double lam_min; // min value in lam vector + double t_min; // min value in t vector + int iter_max; // exit cond in iter number + int stat_max; // iterations saved in stat + int pred_corr; // Mehrotra's predictor-corrector IPM algirthm + int cond_pred_corr; // conditional Mehrotra's predictor-corrector + int scale; // scale hessian + int itref_pred_max; // max number of iterative refinement steps for predictor step + int itref_corr_max; // max number of iterative refinement steps for corrector step + int warm_start; // 0 no warm start, 1 warm start primal sol, 2 warm start primal and dual sol + int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq + int abs_form; // absolute IPM formulation + int comp_res_exit; // compute residuals on exit (only for abs_form==1) + int comp_res_pred; // compute residuals of prediction + int split_step; // use different step for primal and dual variables + int t_lam_min; // clip t and lam: 0 no, 1 in Gamma computation, 2 in solution + int mode; + hpipm_size_t memsize; + }; + + + +struct d_dense_qcqp_ipm_ws + { + struct d_dense_qp_ipm_ws *qp_ws; + struct d_dense_qp *qp; + struct d_dense_qp_sol *qp_sol; + struct d_dense_qcqp_res_ws *qcqp_res_ws; + struct d_dense_qcqp_res *qcqp_res; + struct blasfeo_dvec *tmp_nv; + int iter; // iteration number + int status; + hpipm_size_t memsize; // memory size (in bytes) of workspace + }; + + + +// +hpipm_size_t d_dense_qcqp_ipm_arg_memsize(struct d_dense_qcqp_dim *dim); +// +void d_dense_qcqp_ipm_arg_create(struct d_dense_qcqp_dim *dim, struct d_dense_qcqp_ipm_arg *arg, void *mem); +// +void d_dense_qcqp_ipm_arg_set_default(enum hpipm_mode mode, struct d_dense_qcqp_ipm_arg *arg); +// +void d_dense_qcqp_ipm_arg_set(char *field, void *value, struct d_dense_qcqp_ipm_arg *arg); +// +void d_dense_qcqp_ipm_arg_set_iter_max(int *iter_max, struct d_dense_qcqp_ipm_arg *arg); +// +void d_dense_qcqp_ipm_arg_set_alpha_min(double *alpha_min, struct d_dense_qcqp_ipm_arg *arg); +// +void d_dense_qcqp_ipm_arg_set_mu0(double *mu0, struct d_dense_qcqp_ipm_arg *arg); +// +void d_dense_qcqp_ipm_arg_set_tol_stat(double *tol_stat, struct d_dense_qcqp_ipm_arg *arg); +// +void d_dense_qcqp_ipm_arg_set_tol_eq(double *tol_eq, struct d_dense_qcqp_ipm_arg *arg); +// +void d_dense_qcqp_ipm_arg_set_tol_ineq(double *tol_ineq, struct d_dense_qcqp_ipm_arg *arg); +// +void d_dense_qcqp_ipm_arg_set_tol_comp(double *tol_comp, struct d_dense_qcqp_ipm_arg *arg); +// +void d_dense_qcqp_ipm_arg_set_reg_prim(double *reg, struct d_dense_qcqp_ipm_arg *arg); +// +void d_dense_qcqp_ipm_arg_set_reg_dual(double *reg, struct d_dense_qcqp_ipm_arg *arg); +// +void d_dense_qcqp_ipm_arg_set_warm_start(int *warm_start, struct d_dense_qcqp_ipm_arg *arg); +// +void d_dense_qcqp_ipm_arg_set_pred_corr(int *pred_corr, struct d_dense_qcqp_ipm_arg *arg); +// +void d_dense_qcqp_ipm_arg_set_cond_pred_corr(int *cond_pred_corr, struct d_dense_qcqp_ipm_arg *arg); +// +void d_dense_qcqp_ipm_arg_set_comp_res_pred(int *comp_res_pred, struct d_dense_qcqp_ipm_arg *arg); +// +void d_dense_qcqp_ipm_arg_set_comp_res_exit(int *comp_res_exit, struct d_dense_qcqp_ipm_arg *arg); +// +void d_dense_qcqp_ipm_arg_set_lam_min(double *value, struct d_dense_qcqp_ipm_arg *arg); +// +void d_dense_qcqp_ipm_arg_set_t_min(double *value, struct d_dense_qcqp_ipm_arg *arg); +// +void d_dense_qcqp_ipm_arg_set_split_step(int *value, struct d_dense_qcqp_ipm_arg *arg); +// +void d_dense_qcqp_ipm_arg_set_t_lam_min(int *value, struct d_dense_qcqp_ipm_arg *arg); + +// +hpipm_size_t d_dense_qcqp_ipm_ws_memsize(struct d_dense_qcqp_dim *qp_dim, struct d_dense_qcqp_ipm_arg *arg); +// +void d_dense_qcqp_ipm_ws_create(struct d_dense_qcqp_dim *qp_dim, struct d_dense_qcqp_ipm_arg *arg, struct d_dense_qcqp_ipm_ws *ws, void *mem); +// +void d_dense_qcqp_ipm_get(char *field, struct d_dense_qcqp_ipm_ws *ws, void *value); +// +void d_dense_qcqp_ipm_get_status(struct d_dense_qcqp_ipm_ws *ws, int *status); +// +void d_dense_qcqp_ipm_get_iter(struct d_dense_qcqp_ipm_ws *ws, int *iter); +// +void d_dense_qcqp_ipm_get_max_res_stat(struct d_dense_qcqp_ipm_ws *ws, double *res_stat); +// +void d_dense_qcqp_ipm_get_max_res_eq(struct d_dense_qcqp_ipm_ws *ws, double *res_eq); +// +void d_dense_qcqp_ipm_get_max_res_ineq(struct d_dense_qcqp_ipm_ws *ws, double *res_ineq); +// +void d_dense_qcqp_ipm_get_max_res_comp(struct d_dense_qcqp_ipm_ws *ws, double *res_comp); +// +void d_dense_qcqp_ipm_get_stat(struct d_dense_qcqp_ipm_ws *ws, double **stat); +// +void d_dense_qcqp_ipm_get_stat_m(struct d_dense_qcqp_ipm_ws *ws, int *stat_m); +// +void d_dense_qcqp_init_var(struct d_dense_qcqp *qp, struct d_dense_qcqp_sol *qp_sol, struct d_dense_qcqp_ipm_arg *arg, struct d_dense_qcqp_ipm_ws *ws); +// +void d_dense_qcqp_ipm_solve(struct d_dense_qcqp *qp, struct d_dense_qcqp_sol *qp_sol, struct d_dense_qcqp_ipm_arg *arg, struct d_dense_qcqp_ipm_ws *ws); +#if 0 +// +void d_dense_qcqp_ipm_predict(struct d_dense_qcqp *qp, struct d_dense_qcqp_sol *qp_sol, struct d_dense_qcqp_ipm_arg *arg, struct d_dense_qcqp_ipm_ws *ws); +// +void d_dense_qcqp_ipm_sens(struct d_dense_qcqp *qp, struct d_dense_qcqp_sol *qp_sol, struct d_dense_qcqp_ipm_arg *arg, struct d_dense_qcqp_ipm_ws *ws); +#endif + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_D_DENSE_QCQP_IPM_H_ + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_dense_qcqp_res.h b/phonelibs/acados/include/hpipm/include/hpipm_d_dense_qcqp_res.h new file mode 100644 index 0000000000..a76f16215e --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_dense_qcqp_res.h @@ -0,0 +1,107 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_DENSE_QCQP_RES_H_ +#define HPIPM_D_DENSE_QCQP_RES_H_ + + + +#include +#include + +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_dense_qcqp_res + { + struct d_dense_qcqp_dim *dim; + struct blasfeo_dvec *res_g; // q-residuals + struct blasfeo_dvec *res_b; // b-residuals + struct blasfeo_dvec *res_d; // d-residuals + struct blasfeo_dvec *res_m; // m-residuals + double res_max[4]; // infinity norm of residuals + double res_mu; // mu-residual + hpipm_size_t memsize; + }; + + + +struct d_dense_qcqp_res_ws + { + struct blasfeo_dvec *tmp_nv; // work space of size nv + struct blasfeo_dvec *tmp_nbgq; // work space of size nbM+ngM+nqM + struct blasfeo_dvec *tmp_ns; // work space of size nsM + struct blasfeo_dvec *q_fun; // value for evaluation of quadr constr + struct blasfeo_dvec *q_adj; // value for adjoint of quadr constr + int use_q_fun; // reuse cached value for evaluation of quadr constr + int use_q_adj; // reuse cached value for adjoint of quadr constr + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_dense_qcqp_res_memsize(struct d_dense_qcqp_dim *dim); +// +void d_dense_qcqp_res_create(struct d_dense_qcqp_dim *dim, struct d_dense_qcqp_res *res, void *mem); +// +hpipm_size_t d_dense_qcqp_res_ws_memsize(struct d_dense_qcqp_dim *dim); +// +void d_dense_qcqp_res_ws_create(struct d_dense_qcqp_dim *dim, struct d_dense_qcqp_res_ws *workspace, void *mem); +// +void d_dense_qcqp_res_compute(struct d_dense_qcqp *qp, struct d_dense_qcqp_sol *qp_sol, struct d_dense_qcqp_res *res, struct d_dense_qcqp_res_ws *ws); +// +void d_dense_qcqp_res_compute_inf_norm(struct d_dense_qcqp_res *res); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + +#endif // HPIPM_D_DENSE_QCQP_RES_H_ + + + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_dense_qcqp_sol.h b/phonelibs/acados/include/hpipm/include/hpipm_d_dense_qcqp_sol.h new file mode 100644 index 0000000000..6c697a8e69 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_dense_qcqp_sol.h @@ -0,0 +1,85 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_D_DENSE_QCQP_SOL_H_ +#define HPIPM_D_DENSE_QCQP_SOL_H_ + + + +#include +#include + +#include "hpipm_d_dense_qcqp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_dense_qcqp_sol + { + struct d_dense_qcqp_dim *dim; + struct blasfeo_dvec *v; + struct blasfeo_dvec *pi; + struct blasfeo_dvec *lam; + struct blasfeo_dvec *t; + void *misc; + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_dense_qcqp_sol_memsize(struct d_dense_qcqp_dim *dim); +// +void d_dense_qcqp_sol_create(struct d_dense_qcqp_dim *dim, struct d_dense_qcqp_sol *qp_sol, void *memory); +// +void d_dense_qcqp_sol_get_v(struct d_dense_qcqp_sol *qp_sol, double *v); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_D_DENSE_QCQP_SOL_H_ + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_dense_qcqp_utils.h b/phonelibs/acados/include/hpipm/include/hpipm_d_dense_qcqp_utils.h new file mode 100644 index 0000000000..a34218bae4 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_dense_qcqp_utils.h @@ -0,0 +1,82 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_DENSE_QCQP_UTILS_H_ +#define HPIPM_D_DENSE_QCQP_UTILS_H_ + + + +#include +#include + +#include "hpipm_d_dense_qcqp_dim.h" +#include "hpipm_d_dense_qcqp.h" +#include "hpipm_d_dense_qcqp_sol.h" +//#include "hpipm_d_dense_qcqp_ipm.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +// +void d_dense_qcqp_dim_print(struct d_dense_qcqp_dim *qp_dim); +// +//void d_dense_qcqp_dim_codegen(char *file_name, char *mode, struct d_dense_qcqp_dim *qp_dim); +// +void d_dense_qcqp_print(struct d_dense_qcqp_dim *qp_dim, struct d_dense_qcqp *qp); +// +//void d_dense_qcqp_codegen(char *file_name, char *mode, struct d_dense_qcqp_dim *qp_dim, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_sol_print(struct d_dense_qcqp_dim *qp_dim, struct d_dense_qcqp_sol *dense_qcqp_sol); +// +//void d_dense_qcqp_ipm_arg_codegen(char *file_name, char *mode, struct d_dense_qcqp_dim *qp_dim, struct d_dense_qcqp_ipm_arg *arg); +// +void d_dense_qcqp_res_print(struct d_dense_qcqp_dim *qp_dim, struct d_dense_qcqp_res *dense_qcqp_res); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_D_DENSE_QCQP_UTILS_H_ + + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_dense_qp.h b/phonelibs/acados/include/hpipm/include/hpipm_d_dense_qp.h new file mode 100644 index 0000000000..02fba5a922 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_dense_qp.h @@ -0,0 +1,207 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_D_DENSE_QP_H_ +#define HPIPM_D_DENSE_QP_H_ + + + +#include +#include + +#include "hpipm_d_dense_qp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_dense_qp + { + struct d_dense_qp_dim *dim; + struct blasfeo_dmat *Hv; // hessian of cost & vector work space + struct blasfeo_dmat *A; // equality constraint matrix + struct blasfeo_dmat *Ct; // inequality constraints matrix + struct blasfeo_dvec *gz; // gradient of cost & gradient of slacks + struct blasfeo_dvec *b; // equality constraint vector + struct blasfeo_dvec *d; // inequality constraints vector + struct blasfeo_dvec *d_mask; // inequality constraints mask vector + struct blasfeo_dvec *m; // rhs of complementarity condition + struct blasfeo_dvec *Z; // (diagonal) hessian of slacks + int *idxb; // indices of box constrained variables within [u; x] + int *idxs_rev; // index of soft constraints (reverse storage) + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +hpipm_size_t d_dense_qp_memsize(struct d_dense_qp_dim *dim); +// +void d_dense_qp_create(struct d_dense_qp_dim *dim, struct d_dense_qp *qp, void *memory); + +// setters - colmaj +// +void d_dense_qp_set_all(double *H, double *g, double *A, double *b, int *idxb, double *d_lb, double *d_ub, double *C, double *d_lg, double *d_ug, double *Zl, double *Zu, double *zl, double *zu, int *idxs, double *d_ls, double *d_us, struct d_dense_qp *qp); +// +void d_dense_qp_get_all(struct d_dense_qp *qp, double *H, double *g, double *A, double *b, int *idxb, double *d_lb, double *d_ub, double *C, double *d_lg, double *d_ug, double *Zl, double *Zu, double *zl, double *zu, int *idxs, double *d_ls, double *d_us); +// +void d_dense_qp_set(char *field, void *value, struct d_dense_qp *qp); +// +void d_dense_qp_set_H(double *H, struct d_dense_qp *qp); +// +void d_dense_qp_set_g(double *g, struct d_dense_qp *qp); +// +void d_dense_qp_set_A(double *A, struct d_dense_qp *qp); +// +void d_dense_qp_set_b(double *b, struct d_dense_qp *qp); +// +void d_dense_qp_set_idxb(int *idxb, struct d_dense_qp *qp); +// +void d_dense_qp_set_Jb(double *Jb, struct d_dense_qp *qp); +// +void d_dense_qp_set_lb(double *lb, struct d_dense_qp *qp); +// +void d_dense_qp_set_lb_mask(double *lb, struct d_dense_qp *qp); +// +void d_dense_qp_set_ub(double *ub, struct d_dense_qp *qp); +// +void d_dense_qp_set_ub_mask(double *ub, struct d_dense_qp *qp); +// +void d_dense_qp_set_C(double *C, struct d_dense_qp *qp); +// +void d_dense_qp_set_lg(double *lg, struct d_dense_qp *qp); +// +void d_dense_qp_set_lg_mask(double *lg, struct d_dense_qp *qp); +// +void d_dense_qp_set_ug(double *ug, struct d_dense_qp *qp); +// +void d_dense_qp_set_ug_mask(double *ug, struct d_dense_qp *qp); +// +void d_dense_qp_set_idxs(int *idxs, struct d_dense_qp *qp); +// +void d_dense_qp_set_idxs_rev(int *idxs_rev, struct d_dense_qp *qp); +// +void d_dense_qp_set_Jsb(double *Jsb, struct d_dense_qp *qp); +// +void d_dense_qp_set_Jsg(double *Jsg, struct d_dense_qp *qp); +// +void d_dense_qp_set_Zl(double *Zl, struct d_dense_qp *qp); +// +void d_dense_qp_set_Zu(double *Zu, struct d_dense_qp *qp); +// +void d_dense_qp_set_zl(double *zl, struct d_dense_qp *qp); +// +void d_dense_qp_set_zu(double *zu, struct d_dense_qp *qp); +// +void d_dense_qp_set_ls(double *ls, struct d_dense_qp *qp); +// +void d_dense_qp_set_ls_mask(double *ls, struct d_dense_qp *qp); +// +void d_dense_qp_set_us(double *us, struct d_dense_qp *qp); +// +void d_dense_qp_set_us_mask(double *us, struct d_dense_qp *qp); + +// getters - colmaj +// +void d_dense_qp_get_H(struct d_dense_qp *qp, double *H); +// +void d_dense_qp_get_g(struct d_dense_qp *qp, double *g); +// +void d_dense_qp_get_A(struct d_dense_qp *qp, double *A); +// +void d_dense_qp_get_b(struct d_dense_qp *qp, double *b); +// +void d_dense_qp_get_idxb(struct d_dense_qp *qp, int *idxb); +// +void d_dense_qp_get_lb(struct d_dense_qp *qp, double *lb); +// +void d_dense_qp_get_lb_mask(struct d_dense_qp *qp, double *lb); +// +void d_dense_qp_get_ub(struct d_dense_qp *qp, double *ub); +// +void d_dense_qp_get_ub_mask(struct d_dense_qp *qp, double *ub); +// +void d_dense_qp_get_C(struct d_dense_qp *qp, double *C); +// +void d_dense_qp_get_lg(struct d_dense_qp *qp, double *lg); +// +void d_dense_qp_get_lg_mask(struct d_dense_qp *qp, double *lg); +// +void d_dense_qp_get_ug(struct d_dense_qp *qp, double *ug); +// +void d_dense_qp_get_ug_mask(struct d_dense_qp *qp, double *ug); +// +void d_dense_qp_get_idxs(struct d_dense_qp *qp, int *idxs); +// +void d_dense_qp_get_idxs_rev(struct d_dense_qp *qp, int *idxs_rev); +// +void d_dense_qp_get_Zl(struct d_dense_qp *qp, double *Zl); +// +void d_dense_qp_get_Zu(struct d_dense_qp *qp, double *Zu); +// +void d_dense_qp_get_zl(struct d_dense_qp *qp, double *zl); +// +void d_dense_qp_get_zu(struct d_dense_qp *qp, double *zu); +// +void d_dense_qp_get_ls(struct d_dense_qp *qp, double *ls); +// +void d_dense_qp_get_ls_mask(struct d_dense_qp *qp, double *ls); +// +void d_dense_qp_get_us(struct d_dense_qp *qp, double *us); +// +void d_dense_qp_get_us_mask(struct d_dense_qp *qp, double *us); + +// setters - rowmaj +// +void d_dense_qp_set_all_rowmaj(double *H, double *g, double *A, double *b, int *idxb, double *d_lb, double *d_ub, double *C, double *d_lg, double *d_ug, double *Zl, double *Zu, double *zl, double *zu, int *idxs, double *d_ls, double *d_us, struct d_dense_qp *qp); + +// getters - rowmaj +// +void d_dense_qp_get_all_rowmaj(struct d_dense_qp *qp, double *H, double *g, double *A, double *b, int *idxb, double *d_lb, double *d_ub, double *C, double *d_lg, double *d_ug, double *Zl, double *Zu, double *zl, double *zu, int *idxs, double *d_ls, double *d_us); + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_D_DENSE_QP_H_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_dense_qp_dim.h b/phonelibs/acados/include/hpipm/include/hpipm_d_dense_qp_dim.h new file mode 100644 index 0000000000..98a551f312 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_dense_qp_dim.h @@ -0,0 +1,92 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_DENSE_QP_DIM_H_ +#define HPIPM_D_DENSE_QP_DIM_H_ + +#include "hpipm_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_dense_qp_dim + { + int nv; // number of variables + int ne; // number of equality constraints + int nb; // number of box constraints + int ng; // number of general constraints + int nsb; // number of softened box constraints + int nsg; // number of softened general constraints + int ns; // number of softened constraints (nsb+nsg) + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_dense_qp_dim_memsize(); +// +void d_dense_qp_dim_create(struct d_dense_qp_dim *qp_dim, void *memory); +// +void d_dense_qp_dim_set_all(int nv, int ne, int nb, int ng, int nsb, int nsg, struct d_dense_qp_dim *dim); +// +void d_dense_qp_dim_set(char *field_name, int value, struct d_dense_qp_dim *dim); +// +void d_dense_qp_dim_set_nv(int value, struct d_dense_qp_dim *dim); +// +void d_dense_qp_dim_set_ne(int value, struct d_dense_qp_dim *dim); +// +void d_dense_qp_dim_set_nb(int value, struct d_dense_qp_dim *dim); +// +void d_dense_qp_dim_set_ng(int value, struct d_dense_qp_dim *dim); +// +void d_dense_qp_dim_set_nsb(int value, struct d_dense_qp_dim *dim); +// +void d_dense_qp_dim_set_nsg(int value, struct d_dense_qp_dim *dim); +// +void d_dense_qp_dim_set_ns(int value, struct d_dense_qp_dim *dim); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_D_DENSE_QP_DIM_H_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_dense_qp_ipm.h b/phonelibs/acados/include/hpipm/include/hpipm_d_dense_qp_ipm.h new file mode 100644 index 0000000000..e6e5d5d9b5 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_dense_qp_ipm.h @@ -0,0 +1,260 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_D_DENSE_QP_IPM_H_ +#define HPIPM_D_DENSE_QP_IPM_H_ + + + +#include +#include + +#include +#include +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_dense_qp_ipm_arg + { + double mu0; // initial value for duality measure + double alpha_min; // exit cond on step length + double res_g_max; // exit cond on inf norm of residuals + double res_b_max; // exit cond on inf norm of residuals + double res_d_max; // exit cond on inf norm of residuals + double res_m_max; // exit cond on inf norm of residuals + double reg_prim; // reg of primal hessian + double reg_dual; // reg of dual hessian + double lam_min; // min value in lam vector + double t_min; // min value in t vector + double tau_min; // min value of barrier parameter + int iter_max; // exit cond in iter number + int stat_max; // iterations saved in stat + int pred_corr; // Mehrotra's predictor-corrector IPM algirthm + int cond_pred_corr; // conditional Mehrotra's predictor-corrector + int scale; // scale hessian + int itref_pred_max; // max number of iterative refinement steps for predictor step + int itref_corr_max; // max number of iterative refinement steps for corrector step + int warm_start; // 0 no warm start, 1 warm start primal sol, 2 warm start primal and dual sol + int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq + int abs_form; // absolute IPM formulation + int comp_res_exit; // compute residuals on exit (only for abs_form==1) + int comp_res_pred; // compute residuals of prediction + int kkt_fact_alg; // 0 null-space, 1 schur-complement + int remove_lin_dep_eq; // 0 do not, 1 do check and remove linearly dependent equality constraints + int compute_obj; // compute obj on exit + int split_step; // use different steps for primal and dual variables + int t_lam_min; // clip t and lam: 0 no, 1 in Gamma computation, 2 in solution + int mode; + hpipm_size_t memsize; + }; + + + +struct d_dense_qp_ipm_ws + { + struct d_core_qp_ipm_workspace *core_workspace; + struct d_dense_qp_res_ws *res_ws; + struct d_dense_qp_sol *sol_step; + struct d_dense_qp_sol *sol_itref; + struct d_dense_qp *qp_step; + struct d_dense_qp *qp_itref; + struct d_dense_qp_res *res; + struct d_dense_qp_res *res_itref; + struct d_dense_qp_res *res_step; + struct blasfeo_dvec *Gamma; // + struct blasfeo_dvec *gamma; // + struct blasfeo_dvec *Zs_inv; // + struct blasfeo_dmat *Lv; // + struct blasfeo_dmat *AL; // + struct blasfeo_dmat *Le; // + struct blasfeo_dmat *Ctx; // + struct blasfeo_dvec *lv; // + struct blasfeo_dvec *sv; // scale for Lv + struct blasfeo_dvec *se; // scale for Le + struct blasfeo_dvec *tmp_nbg; // work space of size nb+ng + struct blasfeo_dvec *tmp_ns; // work space of size ns + struct blasfeo_dmat *lq0; + struct blasfeo_dmat *lq1; + struct blasfeo_dvec *tmp_m; + struct blasfeo_dmat *A_LQ; + struct blasfeo_dmat *A_Q; + struct blasfeo_dmat *Zt; + struct blasfeo_dmat *ZtH; + struct blasfeo_dmat *ZtHZ; + struct blasfeo_dvec *xy; + struct blasfeo_dvec *Yxy; + struct blasfeo_dvec *xz; + struct blasfeo_dvec *tmp_nv; + struct blasfeo_dvec *tmp_2ns; + struct blasfeo_dvec *tmp_nv2ns; + struct blasfeo_dmat *A_li; // A of linearly independent equality constraints + struct blasfeo_dvec *b_li; // b of linearly independent equality constraints + struct blasfeo_dmat *A_bkp; // pointer to backup A + struct blasfeo_dvec *b_bkp; // pointer to backup b + struct blasfeo_dmat *Ab_LU; + double *stat; // convergence statistics + int *ipiv_v; + int *ipiv_e; + int *ipiv_e1; + void *lq_work0; + void *lq_work1; + void *lq_work_null; + void *orglq_work_null; + int iter; // iteration number + int stat_max; // iterations saved in stat + int stat_m; // numer of recorded stat per ipm iter + int scale; + int use_hess_fact; + int use_A_fact; + int status; + int lq_fact; // cache from arg + int mask_constr; // use constr mask + int ne_li; // number of linearly independent equality constraints + int ne_bkp; // ne backup + hpipm_size_t memsize; // memory size (in bytes) of workspace + }; + + + +// +hpipm_size_t d_dense_qp_ipm_arg_memsize(struct d_dense_qp_dim *dim); +// +void d_dense_qp_ipm_arg_create(struct d_dense_qp_dim *dim, struct d_dense_qp_ipm_arg *arg, void *mem); +// +void d_dense_qp_ipm_arg_set_default(enum hpipm_mode mode, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set(char *field, void *value, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set_iter_max(int *iter_max, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set_alpha_min(double *alpha_min, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set_mu0(double *mu0, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set_tol_stat(double *tol_stat, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set_tol_eq(double *tol_eq, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set_tol_ineq(double *tol_ineq, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set_tol_comp(double *tol_comp, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set_reg_prim(double *reg, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set_reg_dual(double *reg, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set_warm_start(int *warm_start, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set_pred_corr(int *pred_corr, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set_cond_pred_corr(int *cond_pred_corr, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set_comp_res_pred(int *comp_res_pred, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set_comp_res_exit(int *comp_res_exit, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set_lam_min(double *value, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set_t_min(double *value, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set_tau_min(double *value, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set_kkt_fact_alg(int *value, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set_remove_lin_dep_eq(int *value, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set_compute_obj(int *value, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set_t_lam_min(int *value, struct d_dense_qp_ipm_arg *arg); + +// +void d_dense_qp_ipm_arg_set_split_step(int *value, struct d_dense_qp_ipm_arg *arg); + +// +hpipm_size_t d_dense_qp_ipm_ws_memsize(struct d_dense_qp_dim *qp_dim, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_ws_create(struct d_dense_qp_dim *qp_dim, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws, void *mem); +// +void d_dense_qp_ipm_get(char *field, struct d_dense_qp_ipm_ws *ws, void *value); +// +void d_dense_qp_ipm_get_status(struct d_dense_qp_ipm_ws *ws, int *status); +// +void d_dense_qp_ipm_get_iter(struct d_dense_qp_ipm_ws *ws, int *iter); +// +void d_dense_qp_ipm_get_max_res_stat(struct d_dense_qp_ipm_ws *ws, double *res_stat); +// +void d_dense_qp_ipm_get_max_res_eq(struct d_dense_qp_ipm_ws *ws, double *res_eq); +// +void d_dense_qp_ipm_get_max_res_ineq(struct d_dense_qp_ipm_ws *ws, double *res_ineq); +// +void d_dense_qp_ipm_get_max_res_comp(struct d_dense_qp_ipm_ws *ws, double *res_comp); +// +void d_dense_qp_ipm_get_stat(struct d_dense_qp_ipm_ws *ws, double **stat); +// +void d_dense_qp_ipm_get_stat_m(struct d_dense_qp_ipm_ws *ws, int *stat_m); +// +void d_dense_qp_init_var(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws); +// +void d_dense_qp_ipm_abs_step(int kk, struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws); +// +void d_dense_qp_ipm_delta_step(int kk, struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws); +// +void d_dense_qp_ipm_solve(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws); +// +void d_dense_qp_ipm_predict(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws); +// +void d_dense_qp_ipm_sens(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws); +// +void d_dense_qp_compute_step_length(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws); + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_D_DENSE_QP_IPM_H_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_dense_qp_kkt.h b/phonelibs/acados/include/hpipm/include/hpipm_d_dense_qp_kkt.h new file mode 100644 index 0000000000..6d05779f4b --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_dense_qp_kkt.h @@ -0,0 +1,72 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_D_DENSE_QP_KKT_H_ +#define HPIPM_D_DENSE_QP_KKT_H_ + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +// +void d_fact_solve_kkt_unconstr_dense_qp(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws); +// +void d_fact_solve_kkt_step_dense_qp(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws); +// +void d_fact_lq_solve_kkt_step_dense_qp(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws); +// +void d_solve_kkt_step_dense_qp(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws); +// +void d_dense_qp_remove_lin_dep_eq(struct d_dense_qp *qp, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws); +// +void d_dense_qp_restore_lin_dep_eq(struct d_dense_qp *qp, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws); +// +void d_dense_qp_compute_obj(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_D_DENSE_QP_KKT_H_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_dense_qp_res.h b/phonelibs/acados/include/hpipm/include/hpipm_d_dense_qp_res.h new file mode 100644 index 0000000000..7c2023257a --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_dense_qp_res.h @@ -0,0 +1,105 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_DENSE_QP_RES_H_ +#define HPIPM_D_DENSE_QP_RES_H_ + + + +#include +#include + +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_dense_qp_res + { + struct d_dense_qp_dim *dim; + struct blasfeo_dvec *res_g; // q-residuals + struct blasfeo_dvec *res_b; // b-residuals + struct blasfeo_dvec *res_d; // d-residuals + struct blasfeo_dvec *res_m; // m-residuals + double res_max[4]; // max of residuals + double res_mu; // mu-residual + hpipm_size_t memsize; + }; + + + +struct d_dense_qp_res_ws + { + struct blasfeo_dvec *tmp_nbg; // work space of size nbM+ngM + struct blasfeo_dvec *tmp_ns; // work space of size nsM + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_dense_qp_res_memsize(struct d_dense_qp_dim *dim); +// +void d_dense_qp_res_create(struct d_dense_qp_dim *dim, struct d_dense_qp_res *res, void *mem); +// +hpipm_size_t d_dense_qp_res_ws_memsize(struct d_dense_qp_dim *dim); +// +void d_dense_qp_res_ws_create(struct d_dense_qp_dim *dim, struct d_dense_qp_res_ws *workspace, void *mem); +// +void d_dense_qp_res_compute(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_dense_qp_res *res, struct d_dense_qp_res_ws *ws); +// +void d_dense_qp_res_compute_lin(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_dense_qp_sol *qp_step, struct d_dense_qp_res *res, struct d_dense_qp_res_ws *ws); +// +void d_dense_qp_res_compute_inf_norm(struct d_dense_qp_res *res); +// +void d_dense_qp_res_get_all(struct d_dense_qp_res *res, double *res_g, double *res_ls, double *res_us, double *res_b, double *res_d_lb, double *res_d_ub, double *res_d_lg, double *res_d_ug, double *res_d_ls, double *res_d_us, double *res_m_lb, double *res_m_ub, double *res_m_lg, double *res_m_ug, double *res_m_ls, double *res_m_us); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + +#endif // HPIPM_D_DENSE_QP_RES_H_ + + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_dense_qp_sol.h b/phonelibs/acados/include/hpipm/include/hpipm_d_dense_qp_sol.h new file mode 100644 index 0000000000..aaa3fdb0e4 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_dense_qp_sol.h @@ -0,0 +1,94 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_D_DENSE_QP_SOL_H_ +#define HPIPM_D_DENSE_QP_SOL_H_ + + + +#include +#include + +#include "hpipm_d_dense_qp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_dense_qp_sol + { + struct d_dense_qp_dim *dim; + struct blasfeo_dvec *v; + struct blasfeo_dvec *pi; + struct blasfeo_dvec *lam; + struct blasfeo_dvec *t; + void *misc; + double obj; + int valid_obj; + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_dense_qp_sol_memsize(struct d_dense_qp_dim *dim); +// +void d_dense_qp_sol_create(struct d_dense_qp_dim *dim, struct d_dense_qp_sol *qp_sol, void *memory); +// +void d_dense_qp_sol_get_all(struct d_dense_qp_sol *qp_sol, double *v, double *ls, double *us, double *pi, double *lam_lb, double *lam_ub, double *lam_lg, double *lam_ug, double *lam_ls, double *lam_us); +// +void d_dense_qp_sol_get(char *field, struct d_dense_qp_sol *sol, void *value); +// +void d_dense_qp_sol_get_v(struct d_dense_qp_sol *sol, double *v); +// +void d_dense_qp_sol_get_valid_obj(struct d_dense_qp_sol *sol, int *valid_obj); +// +void d_dense_qp_sol_get_obj(struct d_dense_qp_sol *sol, double *obj); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_D_DENSE_QP_SOL_H_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_dense_qp_utils.h b/phonelibs/acados/include/hpipm/include/hpipm_d_dense_qp_utils.h new file mode 100644 index 0000000000..ccb77aaca3 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_dense_qp_utils.h @@ -0,0 +1,83 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_DENSE_QP_UTILS_H_ +#define HPIPM_D_DENSE_QP_UTILS_H_ + + + +#include +#include + +#include "hpipm_d_dense_qp_dim.h" +#include "hpipm_d_dense_qp.h" +#include "hpipm_d_dense_qp_sol.h" +#include "hpipm_d_dense_qp_ipm.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +// +void d_dense_qp_dim_print(struct d_dense_qp_dim *qp_dim); +// +//void d_dense_qp_dim_codegen(char *file_name, char *mode, struct d_dense_qp_dim *qp_dim); +// +void d_dense_qp_print(struct d_dense_qp_dim *qp_dim, struct d_dense_qp *qp); +// +//void d_dense_qp_codegen(char *file_name, char *mode, struct d_dense_qp_dim *qp_dim, struct d_dense_qp *qp); +// +void d_dense_qp_sol_print(struct d_dense_qp_dim *qp_dim, struct d_dense_qp_sol *dense_qp_sol); +// +//void d_dense_qp_ipm_arg_codegen(char *file_name, char *mode, struct d_dense_qp_dim *qp_dim, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_res_print(struct d_dense_qp_dim *qp_dim, struct d_dense_qp_res *dense_qp_res); +// +void d_dense_qp_arg_print(struct d_dense_qp_dim *qp_dim, struct d_dense_qp_ipm_arg *qp_ipm_arg); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_D_DENSE_QP_UTILS_H_ + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_ocp_qcqp.h b/phonelibs/acados/include/hpipm/include/hpipm_d_ocp_qcqp.h new file mode 100644 index 0000000000..240c9b04be --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_ocp_qcqp.h @@ -0,0 +1,303 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_OCP_QCQP_H_ +#define HPIPM_D_OCP_QCQP_H_ + + + +#include +#include + +#include "hpipm_d_ocp_qcqp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_ocp_qcqp + { + struct d_ocp_qcqp_dim *dim; + struct blasfeo_dmat *BAbt; // dynamics matrix & vector work space + struct blasfeo_dmat *RSQrq; // hessian of cost & vector work space + struct blasfeo_dmat *DCt; // inequality constraints matrix + struct blasfeo_dmat **Hq; // hessians of quadratic constraints + struct blasfeo_dvec *b; // dynamics vector + struct blasfeo_dvec *rqz; // gradient of cost & gradient of slacks + struct blasfeo_dvec *d; // inequality constraints vector + struct blasfeo_dvec *d_mask; // inequality constraints mask vector + struct blasfeo_dvec *m; // rhs of complementarity condition + struct blasfeo_dvec *Z; // (diagonal) hessian of slacks + int **idxb; // indices of box constrained variables within [u; x] + int **idxs_rev; // index of soft constraints (reverse storage) + int **Hq_nzero; // for each int, the last 3 bits ...abc, {a,b,c}=0 => {R,S,Q}=0 + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +hpipm_size_t d_ocp_qcqp_strsize(); +// +hpipm_size_t d_ocp_qcqp_memsize(struct d_ocp_qcqp_dim *dim); +// +void d_ocp_qcqp_create(struct d_ocp_qcqp_dim *dim, struct d_ocp_qcqp *qp, void *memory); +// +void d_ocp_qcqp_copy_all(struct d_ocp_qcqp *qp_orig, struct d_ocp_qcqp *qp_dest); + +// setters +// +void d_ocp_qcqp_set_all_zero(struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_rhs_zero(struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set(char *field_name, int stage, void *value, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_el(char *field_name, int stage, int index, void *value, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_A(int stage, double *mat, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_B(int stage, double *mat, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_b(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_Q(int stage, double *mat, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_S(int stage, double *mat, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_R(int stage, double *mat, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_q(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_r(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_lb(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_lb_mask(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_ub(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_ub_mask(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_lbx(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_lbx_mask(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_el_lbx(int stage, int index, double *elem, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_ubx(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_ubx_mask(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_el_ubx(int stage, int index, double *elem, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_lbu(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_lbu_mask(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_ubu(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_ubu_mask(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_idxb(int stage, int *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_idxbx(int stage, int *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_Jbx(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_idxbu(int stage, int *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_Jbu(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_C(int stage, double *mat, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_D(int stage, double *mat, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_lg(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_lg_mask(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_ug(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_ug_mask(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_Qq(int stage, double *mat, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_Sq(int stage, double *mat, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_Rq(int stage, double *mat, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_qq(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_rq(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_uq(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_uq_mask(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_Zl(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_Zu(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_zl(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_zu(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_lls(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_lls_mask(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_lus(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_lus_mask(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_idxs(int stage, int *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_idxs_rev(int stage, int *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_Jsbu(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_Jsbx(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_Jsg(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_Jsq(int stage, double *vec, struct d_ocp_qcqp *qp); + +// getters +// +void d_ocp_qcqp_get(char *field, int stage, struct d_ocp_qcqp *qp, void *value); +// +void d_ocp_qcqp_get_A(int stage, struct d_ocp_qcqp *qp, double *mat); +// +void d_ocp_qcqp_get_B(int stage, struct d_ocp_qcqp *qp, double *mat); +// +void d_ocp_qcqp_get_b(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_Q(int stage, struct d_ocp_qcqp *qp, double *mat); +// +void d_ocp_qcqp_get_S(int stage, struct d_ocp_qcqp *qp, double *mat); +// +void d_ocp_qcqp_get_R(int stage, struct d_ocp_qcqp *qp, double *mat); +// +void d_ocp_qcqp_get_q(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_r(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_ub(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_ub_mask(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_lb(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_lb_mask(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_lbx(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_lbx_mask(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_ubx(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_ubx_mask(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_lbu(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_lbu_mask(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_ubu(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_ubu_mask(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_idxb(int stage, struct d_ocp_qcqp *qp, int *vec); +// +//void d_ocp_qcqp_get_idxbx(int stage, struct d_ocp_qcqp *qp, int *vec); +// +//void d_ocp_qcqp_get_Jbx(int stage, struct d_ocp_qcqp *qp, double *vec); +// +//void d_ocp_qcqp_get_idxbu(int stage, struct d_ocp_qcqp *qp, int *vec); +// +//void d_ocp_qcqp_get_Jbu(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_C(int stage, struct d_ocp_qcqp *qp, double *mat); +// +void d_ocp_qcqp_get_D(int stage, struct d_ocp_qcqp *qp, double *mat); +// +void d_ocp_qcqp_get_lg(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_lg_mask(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_ug(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_ug_mask(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_Zl(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_Zu(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_zl(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_zu(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_lls(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_lls_mask(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_lus(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_lus_mask(int stage, struct d_ocp_qcqp *qp, double *vec); +// XXX only valid if there is one slack per softed constraint !!! +void d_ocp_qcqp_get_idxs(int stage, struct d_ocp_qcqp *qp, int *vec); +// +void d_ocp_qcqp_get_idxs_rev(int stage, struct d_ocp_qcqp *qp, int *vec); +// +//void d_ocp_qcqp_get_Jsbu(int stage, struct d_ocp_qcqp *qp, float *vec); +// +//void d_ocp_qcqp_get_Jsbx(int stage, struct d_ocp_qcqp *qp, float *vec); +// +//void d_ocp_qcqp_get_Jsg(int stage, struct d_ocp_qcqp *qp, float *vec); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_D_OCP_QCQP_H_ + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_ocp_qcqp_dim.h b/phonelibs/acados/include/hpipm/include/hpipm_d_ocp_qcqp_dim.h new file mode 100644 index 0000000000..268628a2b2 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_ocp_qcqp_dim.h @@ -0,0 +1,118 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_OCP_QCQP_DIM_H_ +#define HPIPM_D_OCP_QCQP_DIM_H_ + +#include "hpipm_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_ocp_qcqp_dim + { + struct d_ocp_qp_dim *qp_dim; // dim of qp approximation + int *nx; // number of states + int *nu; // number of inputs + int *nb; // number of box constraints + int *nbx; // number of (two-sided) state box constraints + int *nbu; // number of (two-sided) input box constraints + int *ng; // number of (two-sided) general constraints + int *nq; // number of (upper) quadratic constraints + int *ns; // number of soft constraints + int *nsbx; // number of (two-sided) soft state box constraints + int *nsbu; // number of (two-sided) soft input box constraints + int *nsg; // number of (two-sided) soft general constraints + int *nsq; // number of (upper) soft quadratic constraints + int N; // horizon length + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_ocp_qcqp_dim_strsize(); +// +hpipm_size_t d_ocp_qcqp_dim_memsize(int N); +// +void d_ocp_qcqp_dim_create(int N, struct d_ocp_qcqp_dim *qp_dim, void *memory); +// +void d_ocp_qcqp_dim_copy_all(struct d_ocp_qcqp_dim *dim_orig, struct d_ocp_qcqp_dim *dim_dest); +// +void d_ocp_qcqp_dim_set(char *field, int stage, int value, struct d_ocp_qcqp_dim *dim); +// +void d_ocp_qcqp_dim_set_nx(int stage, int value, struct d_ocp_qcqp_dim *dim); +// +void d_ocp_qcqp_dim_set_nu(int stage, int value, struct d_ocp_qcqp_dim *dim); +// +void d_ocp_qcqp_dim_set_nbx(int stage, int value, struct d_ocp_qcqp_dim *dim); +// +void d_ocp_qcqp_dim_set_nbu(int stage, int value, struct d_ocp_qcqp_dim *dim); +// +void d_ocp_qcqp_dim_set_ng(int stage, int value, struct d_ocp_qcqp_dim *dim); +// +void d_ocp_qcqp_dim_set_nq(int stage, int value, struct d_ocp_qcqp_dim *dim); +// +void d_ocp_qcqp_dim_set_ns(int stage, int value, struct d_ocp_qcqp_dim *dim); +// +void d_ocp_qcqp_dim_set_nsbx(int stage, int value, struct d_ocp_qcqp_dim *dim); +// +void d_ocp_qcqp_dim_set_nsbu(int stage, int value, struct d_ocp_qcqp_dim *dim); +// +void d_ocp_qcqp_dim_set_nsg(int stage, int value, struct d_ocp_qcqp_dim *dim); +// +void d_ocp_qcqp_dim_set_nsq(int stage, int value, struct d_ocp_qcqp_dim *dim); +// +void d_ocp_qcqp_dim_get(struct d_ocp_qcqp_dim *dim, char *field, int stage, int *value); +// +void d_ocp_qcqp_dim_get_N(struct d_ocp_qcqp_dim *dim, int *value); +// +void d_ocp_qcqp_dim_get_nx(struct d_ocp_qcqp_dim *dim, int stage, int *value); +// +void d_ocp_qcqp_dim_get_nu(struct d_ocp_qcqp_dim *dim, int stage, int *value); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_D_OCP_QCQP_DIM_H_ + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_ocp_qcqp_ipm.h b/phonelibs/acados/include/hpipm/include/hpipm_d_ocp_qcqp_ipm.h new file mode 100644 index 0000000000..99f2329dcc --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_ocp_qcqp_ipm.h @@ -0,0 +1,190 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_OCP_QCQP_IPM_H_ +#define HPIPM_D_OCP_QCQP_IPM_H_ + + + +#include +#include + +#include +#include +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_ocp_qcqp_ipm_arg + { + struct d_ocp_qp_ipm_arg *qp_arg; + double mu0; // initial value for complementarity slackness + double alpha_min; // exit cond on step length + double res_g_max; // exit cond on inf norm of residuals + double res_b_max; // exit cond on inf norm of residuals + double res_d_max; // exit cond on inf norm of residuals + double res_m_max; // exit cond on inf norm of residuals + double reg_prim; // reg of primal hessian + double lam_min; // min value in lam vector + double t_min; // min value in t vector + int iter_max; // exit cond in iter number + int stat_max; // iterations saved in stat + int pred_corr; // use Mehrotra's predictor-corrector IPM algirthm + int cond_pred_corr; // conditional Mehrotra's predictor-corrector + int itref_pred_max; // max number of iterative refinement steps for predictor step + int itref_corr_max; // max number of iterative refinement steps for corrector step + int warm_start; // 0 no warm start, 1 warm start primal sol, 2 warm start primal and dual sol + int square_root_alg; // 0 classical Riccati, 1 square-root Riccati + int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq (for square_root_alg==1) + int abs_form; // absolute IPM formulation + int comp_dual_sol_eq; // dual solution of equality constrains (only for abs_form==1) + int comp_res_exit; // compute residuals on exit (only for abs_form==1 and comp_dual_sol_eq==1) + int comp_res_pred; // compute residuals of prediction + int split_step; // use different step for primal and dual variables + int t_lam_min; // clip t and lam: 0 no, 1 in Gamma computation, 2 in solution + int mode; + hpipm_size_t memsize; + }; + + + +struct d_ocp_qcqp_ipm_ws + { + struct d_ocp_qp_ipm_ws *qp_ws; + struct d_ocp_qp *qp; + struct d_ocp_qp_sol *qp_sol; + struct d_ocp_qcqp_res_ws *qcqp_res_ws; + struct d_ocp_qcqp_res *qcqp_res; + struct blasfeo_dvec *tmp_nuxM; + int iter; // iteration number + int status; + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_ocp_qcqp_ipm_arg_strsize(); +// +hpipm_size_t d_ocp_qcqp_ipm_arg_memsize(struct d_ocp_qcqp_dim *ocp_dim); +// +void d_ocp_qcqp_ipm_arg_create(struct d_ocp_qcqp_dim *ocp_dim, struct d_ocp_qcqp_ipm_arg *arg, void *mem); +// +void d_ocp_qcqp_ipm_arg_set_default(enum hpipm_mode mode, struct d_ocp_qcqp_ipm_arg *arg); +// +void d_ocp_qcqp_ipm_arg_set(char *field, void *value, struct d_ocp_qcqp_ipm_arg *arg); +// set maximum number of iterations +void d_ocp_qcqp_ipm_arg_set_iter_max(int *value, struct d_ocp_qcqp_ipm_arg *arg); +// set minimum step lenght +void d_ocp_qcqp_ipm_arg_set_alpha_min(double *value, struct d_ocp_qcqp_ipm_arg *arg); +// set initial value of barrier parameter +void d_ocp_qcqp_ipm_arg_set_mu0(double *value, struct d_ocp_qcqp_ipm_arg *arg); +// set exit tolerance on stationarity condition +void d_ocp_qcqp_ipm_arg_set_tol_stat(double *value, struct d_ocp_qcqp_ipm_arg *arg); +// set exit tolerance on equality constr +void d_ocp_qcqp_ipm_arg_set_tol_eq(double *value, struct d_ocp_qcqp_ipm_arg *arg); +// set exit tolerance on inequality constr +void d_ocp_qcqp_ipm_arg_set_tol_ineq(double *value, struct d_ocp_qcqp_ipm_arg *arg); +// set exit tolerance on complementarity condition +void d_ocp_qcqp_ipm_arg_set_tol_comp(double *value, struct d_ocp_qcqp_ipm_arg *arg); +// set regularization of primal variables +void d_ocp_qcqp_ipm_arg_set_reg_prim(double *value, struct d_ocp_qcqp_ipm_arg *arg); +// set warm start: 0 no warm start, 1 primal var +void d_ocp_qcqp_ipm_arg_set_warm_start(int *value, struct d_ocp_qcqp_ipm_arg *arg); +// Mehrotra's predictor-corrector IPM algorithm: 0 no predictor-corrector, 1 use predictor-corrector +void d_ocp_qcqp_ipm_arg_set_pred_corr(int *value, struct d_ocp_qcqp_ipm_arg *arg); +// conditional predictor-corrector: 0 no conditinal predictor-corrector, 1 conditional predictor-corrector +void d_ocp_qcqp_ipm_arg_set_cond_pred_corr(int *value, struct d_ocp_qcqp_ipm_arg *arg); +// set riccati algorithm: 0 classic, 1 square-root +void d_ocp_qcqp_ipm_arg_set_ric_alg(int *value, struct d_ocp_qcqp_ipm_arg *arg); +// compute residuals after solution +void d_ocp_qcqp_ipm_arg_set_comp_res_exit(int *value, struct d_ocp_qcqp_ipm_arg *arg); +// compute residuals of prediction +void d_ocp_qcqp_ipm_arg_set_comp_res_pred(int *value, struct d_ocp_qcqp_ipm_arg *arg); +// min value of lam in the solution +void d_ocp_qcqp_ipm_arg_set_lam_min(double *value, struct d_ocp_qcqp_ipm_arg *arg); +// min value of t in the solution +void d_ocp_qcqp_ipm_arg_set_t_min(double *value, struct d_ocp_qcqp_ipm_arg *arg); +// use different step for primal and dual variables +void d_ocp_qcqp_ipm_arg_set_split_step(int *value, struct d_ocp_qcqp_ipm_arg *arg); +// clip t and lam: 0 no, 1 in Gamma computation, 2 in solution +void d_ocp_qcqp_ipm_arg_set_t_lam_min(int *value, struct d_ocp_qcqp_ipm_arg *arg); + +// +hpipm_size_t d_ocp_qcqp_ipm_ws_strsize(); +// +hpipm_size_t d_ocp_qcqp_ipm_ws_memsize(struct d_ocp_qcqp_dim *ocp_dim, struct d_ocp_qcqp_ipm_arg *arg); +// +void d_ocp_qcqp_ipm_ws_create(struct d_ocp_qcqp_dim *ocp_dim, struct d_ocp_qcqp_ipm_arg *arg, struct d_ocp_qcqp_ipm_ws *ws, void *mem); +// +void d_ocp_qcqp_ipm_get(char *field, struct d_ocp_qcqp_ipm_ws *ws, void *value); +// +void d_ocp_qcqp_ipm_get_status(struct d_ocp_qcqp_ipm_ws *ws, int *status); +// +void d_ocp_qcqp_ipm_get_iter(struct d_ocp_qcqp_ipm_ws *ws, int *iter); +// +void d_ocp_qcqp_ipm_get_max_res_stat(struct d_ocp_qcqp_ipm_ws *ws, double *res_stat); +// +void d_ocp_qcqp_ipm_get_max_res_eq(struct d_ocp_qcqp_ipm_ws *ws, double *res_eq); +// +void d_ocp_qcqp_ipm_get_max_res_ineq(struct d_ocp_qcqp_ipm_ws *ws, double *res_ineq); +// +void d_ocp_qcqp_ipm_get_max_res_comp(struct d_ocp_qcqp_ipm_ws *ws, double *res_comp); +// +void d_ocp_qcqp_ipm_get_stat(struct d_ocp_qcqp_ipm_ws *ws, double **stat); +// +void d_ocp_qcqp_ipm_get_stat_m(struct d_ocp_qcqp_ipm_ws *ws, int *stat_m); +// +void d_ocp_qcqp_init_var(struct d_ocp_qcqp *qp, struct d_ocp_qcqp_sol *qp_sol, struct d_ocp_qcqp_ipm_arg *arg, struct d_ocp_qcqp_ipm_ws *ws); +// +void d_ocp_qcqp_ipm_solve(struct d_ocp_qcqp *qp, struct d_ocp_qcqp_sol *qp_sol, struct d_ocp_qcqp_ipm_arg *arg, struct d_ocp_qcqp_ipm_ws *ws); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + +#endif // HPIPM_D_OCP_QCQP_IPM_H_ + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_ocp_qcqp_res.h b/phonelibs/acados/include/hpipm/include/hpipm_d_ocp_qcqp_res.h new file mode 100644 index 0000000000..077f134677 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_ocp_qcqp_res.h @@ -0,0 +1,114 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_OCP_QCQP_RES_H_ +#define HPIPM_D_OCP_QCQP_RES_H_ + + + +#include +#include + +#include +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_ocp_qcqp_res + { + struct d_ocp_qcqp_dim *dim; + struct blasfeo_dvec *res_g; // q-residuals + struct blasfeo_dvec *res_b; // b-residuals + struct blasfeo_dvec *res_d; // d-residuals + struct blasfeo_dvec *res_m; // m-residuals + double res_max[4]; // max of residuals + double res_mu; // mu-residual + hpipm_size_t memsize; + }; + + + +struct d_ocp_qcqp_res_ws + { + struct blasfeo_dvec *tmp_nuxM; // work space of size nuM+nxM + struct blasfeo_dvec *tmp_nbgqM; // work space of size nbM+ngM+nqM + struct blasfeo_dvec *tmp_nsM; // work space of size nsM + struct blasfeo_dvec *q_fun; // value for evaluation of quadr constr + struct blasfeo_dvec *q_adj; // value for adjoint of quadr constr + int use_q_fun; // reuse cached value for evaluation of quadr constr + int use_q_adj; // reuse cached value for adjoint of quadr constr + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_ocp_qcqp_res_memsize(struct d_ocp_qcqp_dim *ocp_dim); +// +void d_ocp_qcqp_res_create(struct d_ocp_qcqp_dim *ocp_dim, struct d_ocp_qcqp_res *res, void *mem); +// +hpipm_size_t d_ocp_qcqp_res_ws_memsize(struct d_ocp_qcqp_dim *ocp_dim); +// +void d_ocp_qcqp_res_ws_create(struct d_ocp_qcqp_dim *ocp_dim, struct d_ocp_qcqp_res_ws *workspace, void *mem); +// +void d_ocp_qcqp_res_compute(struct d_ocp_qcqp *qp, struct d_ocp_qcqp_sol *qp_sol, struct d_ocp_qcqp_res *res, struct d_ocp_qcqp_res_ws *ws); +// +void d_ocp_qcqp_res_compute_inf_norm(struct d_ocp_qcqp_res *res); +// +void d_ocp_qcqp_res_get_max_res_stat(struct d_ocp_qcqp_res *res, double *value); +// +void d_ocp_qcqp_res_get_max_res_eq(struct d_ocp_qcqp_res *res, double *value); +// +void d_ocp_qcqp_res_get_max_res_ineq(struct d_ocp_qcqp_res *res, double *value); +// +void d_ocp_qcqp_res_get_max_res_comp(struct d_ocp_qcqp_res *res, double *value); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + +#endif // HPIPM_D_OCP_QCQP_RES_H_ + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_ocp_qcqp_sol.h b/phonelibs/acados/include/hpipm/include/hpipm_d_ocp_qcqp_sol.h new file mode 100644 index 0000000000..68adbfce2d --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_ocp_qcqp_sol.h @@ -0,0 +1,114 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_OCP_QCQP_SOL_H_ +#define HPIPM_D_OCP_QCQP_SOL_H_ + + + +#include +#include + +#include "hpipm_d_ocp_qcqp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_ocp_qcqp_sol + { + struct d_ocp_qcqp_dim *dim; + struct blasfeo_dvec *ux; + struct blasfeo_dvec *pi; + struct blasfeo_dvec *lam; + struct blasfeo_dvec *t; + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +hpipm_size_t d_ocp_qcqp_sol_strsize(); +// +hpipm_size_t d_ocp_qcqp_sol_memsize(struct d_ocp_qcqp_dim *dim); +// +void d_ocp_qcqp_sol_create(struct d_ocp_qcqp_dim *dim, struct d_ocp_qcqp_sol *qp_sol, void *memory); +// +void d_ocp_qcqp_sol_copy_all(struct d_ocp_qcqp_sol *qp_sol_orig, struct d_ocp_qcqp_sol *qp_sol_dest); +// +void d_ocp_qcqp_sol_get(char *field, int stage, struct d_ocp_qcqp_sol *qp_sol, double *vec); +// +void d_ocp_qcqp_sol_get_u(int stage, struct d_ocp_qcqp_sol *qp_sol, double *vec); +// +void d_ocp_qcqp_sol_get_x(int stage, struct d_ocp_qcqp_sol *qp_sol, double *vec); +// +void d_ocp_qcqp_sol_get_sl(int stage, struct d_ocp_qcqp_sol *qp_sol, double *vec); +// +void d_ocp_qcqp_sol_get_su(int stage, struct d_ocp_qcqp_sol *qp_sol, double *vec); +// +void d_ocp_qcqp_sol_get_pi(int stage, struct d_ocp_qcqp_sol *qp_sol, double *vec); +// +void d_ocp_qcqp_sol_get_lam_lb(int stage, struct d_ocp_qcqp_sol *qp_sol, double *vec); +// +void d_ocp_qcqp_sol_get_lam_ub(int stage, struct d_ocp_qcqp_sol *qp_sol, double *vec); +// +void d_ocp_qcqp_sol_get_lam_lg(int stage, struct d_ocp_qcqp_sol *qp_sol, double *vec); +// +void d_ocp_qcqp_sol_get_lam_ug(int stage, struct d_ocp_qcqp_sol *qp_sol, double *vec); +// +void d_ocp_qcqp_sol_set(char *field, int stage, double *vec, struct d_ocp_qcqp_sol *qp_sol); +// +void d_ocp_qcqp_sol_set_u(int stage, double *vec, struct d_ocp_qcqp_sol *qp_sol); +// +void d_ocp_qcqp_sol_set_x(int stage, double *vec, struct d_ocp_qcqp_sol *qp_sol); +// +void d_ocp_qcqp_sol_set_sl(int stage, double *vec, struct d_ocp_qcqp_sol *qp_sol); +// +void d_ocp_qcqp_sol_set_su(int stage, double *vec, struct d_ocp_qcqp_sol *qp_sol); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_D_OCP_QCQP_SOL_H_ + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_ocp_qcqp_utils.h b/phonelibs/acados/include/hpipm/include/hpipm_d_ocp_qcqp_utils.h new file mode 100644 index 0000000000..00248f1dbe --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_ocp_qcqp_utils.h @@ -0,0 +1,81 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_OCP_QCQP_UTILS_H_ +#define HPIPM_D_OCP_QCQP_UTILS_H_ + + + +#include +#include + +#include "hpipm_d_ocp_qcqp_dim.h" +#include "hpipm_d_ocp_qp.h" +#include "hpipm_d_ocp_qcqp_sol.h" +#include "hpipm_d_ocp_qcqp_ipm.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +// +void d_ocp_qcqp_dim_print(struct d_ocp_qcqp_dim *qcqp_dim); +// +void d_ocp_qcqp_dim_codegen(char *file_name, char *mode, struct d_ocp_qcqp_dim *qcqp_dim); +// +void d_ocp_qcqp_print(struct d_ocp_qcqp_dim *qcqp_dim, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_codegen(char *file_name, char *mode, struct d_ocp_qcqp_dim *qcqp_dim, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_sol_print(struct d_ocp_qcqp_dim *qcqp_dim, struct d_ocp_qcqp_sol *ocp_qcqp_sol); +// +void d_ocp_qcqp_ipm_arg_codegen(char *file_name, char *mode, struct d_ocp_qcqp_dim *qcqp_dim, struct d_ocp_qcqp_ipm_arg *arg); +// +void d_ocp_qcqp_res_print(struct d_ocp_qcqp_dim *qcqp_dim, struct d_ocp_qcqp_res *ocp_qcqp_res); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_D_OCP_QCQP_UTILS_H_ + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_ocp_qp.h b/phonelibs/acados/include/hpipm/include/hpipm_d_ocp_qp.h new file mode 100644 index 0000000000..ca26cdba34 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_ocp_qp.h @@ -0,0 +1,306 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_OCP_QP_H_ +#define HPIPM_D_OCP_QP_H_ + + + +#include +#include + +#include "hpipm_d_ocp_qp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_ocp_qp + { + struct d_ocp_qp_dim *dim; + struct blasfeo_dmat *BAbt; // dynamics matrix & vector work space + struct blasfeo_dmat *RSQrq; // hessian of cost & vector work space + struct blasfeo_dmat *DCt; // inequality constraints matrix + struct blasfeo_dvec *b; // dynamics vector + struct blasfeo_dvec *rqz; // gradient of cost & gradient of slacks + struct blasfeo_dvec *d; // inequality constraints vector + struct blasfeo_dvec *d_mask; // inequality constraints mask vector + struct blasfeo_dvec *m; // rhs of complementarity condition + struct blasfeo_dvec *Z; // (diagonal) hessian of slacks + int **idxb; // indices of box constrained variables within [u; x] + int **idxs_rev; // index of soft constraints (reverse storage) + int **idxe; // indices of constraints within [bu, bx, g] that are equalities, subset of [0, ..., nbu+nbx+ng-1] + int *diag_H_flag; // flag the fact that Hessian is diagonal + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +hpipm_size_t d_ocp_qp_strsize(); +// +hpipm_size_t d_ocp_qp_memsize(struct d_ocp_qp_dim *dim); +// +void d_ocp_qp_create(struct d_ocp_qp_dim *dim, struct d_ocp_qp *qp, void *memory); +// +void d_ocp_qp_copy_all(struct d_ocp_qp *qp_orig, struct d_ocp_qp *qp_dest); + +// setters +// +void d_ocp_qp_set_all_zero(struct d_ocp_qp *qp); +// +void d_ocp_qp_set_rhs_zero(struct d_ocp_qp *qp); +// +void d_ocp_qp_set_all(double **A, double **B, double **b, double **Q, double **S, double **R, double **q, double **r, int **idxbx, double **lbx, double **ubx, int **idxbu, double **lbu, double **ubu, double **C, double **D, double **lg, double **ug, double **Zl, double **Zu, double **zl, double **zu, int **idxs, double **ls, double **us, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_all_rowmaj(double **A, double **B, double **b, double **Q, double **S, double **R, double **q, double **r, int **idxbx, double **lbx, double **ubx, int **idxbu, double **lbu, double **ubu, double **C, double **D, double **lg, double **ug, double **Zl, double **Zu, double **zl, double **zu, int **idxs, double **ls, double **us, struct d_ocp_qp *qp); +// +void d_ocp_qp_set(char *field_name, int stage, void *value, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_el(char *field_name, int stage, int index, void *value, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_A(int stage, double *mat, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_B(int stage, double *mat, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_b(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_Q(int stage, double *mat, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_S(int stage, double *mat, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_R(int stage, double *mat, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_q(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_r(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_lb(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_lb_mask(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_ub(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_ub_mask(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_lbx(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_lbx_mask(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_el_lbx(int stage, int index, double *elem, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_ubx(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_ubx_mask(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_el_ubx(int stage, int index, double *elem, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_lbu(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_lbu_mask(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_ubu(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_ubu_mask(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_idxb(int stage, int *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_idxbx(int stage, int *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_Jbx(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_idxbu(int stage, int *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_Jbu(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_C(int stage, double *mat, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_D(int stage, double *mat, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_lg(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_lg_mask(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_ug(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_ug_mask(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_Zl(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_Zu(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_zl(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_zu(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_lls(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_lls_mask(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_lus(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_lus_mask(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_idxs(int stage, int *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_idxs_rev(int stage, int *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_Jsbu(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_Jsbx(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_Jsg(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_idxe(int stage, int *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_idxbxe(int stage, int *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_idxbue(int stage, int *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_idxge(int stage, int *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_Jbxe(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_Jbue(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_Jge(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_diag_H_flag(int stage, int *value, struct d_ocp_qp *qp); + +// getters +// +void d_ocp_qp_get(char *field, int stage, struct d_ocp_qp *qp, void *value); +// +void d_ocp_qp_get_A(int stage, struct d_ocp_qp *qp, double *mat); +// +void d_ocp_qp_get_B(int stage, struct d_ocp_qp *qp, double *mat); +// +void d_ocp_qp_get_b(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_Q(int stage, struct d_ocp_qp *qp, double *mat); +// +void d_ocp_qp_get_S(int stage, struct d_ocp_qp *qp, double *mat); +// +void d_ocp_qp_get_R(int stage, struct d_ocp_qp *qp, double *mat); +// +void d_ocp_qp_get_q(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_r(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_ub(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_ub_mask(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_lb(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_lb_mask(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_lbx(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_lbx_mask(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_ubx(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_ubx_mask(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_lbu(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_lbu_mask(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_ubu(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_ubu_mask(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_idxb(int stage, struct d_ocp_qp *qp, int *vec); +// +//void d_ocp_qp_get_idxbx(int stage, struct d_ocp_qp *qp, int *vec); +// +//void d_ocp_qp_get_Jbx(int stage, struct d_ocp_qp *qp, double *vec); +// +//void d_ocp_qp_get_idxbu(int stage, struct d_ocp_qp *qp, int *vec); +// +//void d_ocp_qp_get_Jbu(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_C(int stage, struct d_ocp_qp *qp, double *mat); +// +void d_ocp_qp_get_D(int stage, struct d_ocp_qp *qp, double *mat); +// +void d_ocp_qp_get_lg(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_lg_mask(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_ug(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_ug_mask(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_Zl(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_Zu(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_zl(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_zu(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_lls(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_lls_mask(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_lus(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_lus_mask(int stage, struct d_ocp_qp *qp, double *vec); +// XXX only valid if there is one slack per softed constraint !!! +void d_ocp_qp_get_idxs(int stage, struct d_ocp_qp *qp, int *vec); +// +void d_ocp_qp_get_idxs_rev(int stage, struct d_ocp_qp *qp, int *vec); +// +//void d_ocp_qp_get_Jsbu(int stage, struct d_ocp_qp *qp, float *vec); +// +//void d_ocp_qp_get_Jsbx(int stage, struct d_ocp_qp *qp, float *vec); +// +//void d_ocp_qp_get_Jsg(int stage, struct d_ocp_qp *qp, float *vec); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_D_OCP_QP_H_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_ocp_qp_dim.h b/phonelibs/acados/include/hpipm/include/hpipm_d_ocp_qp_dim.h new file mode 100644 index 0000000000..dcfb801d31 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_ocp_qp_dim.h @@ -0,0 +1,142 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_OCP_QP_DIM_H_ +#define HPIPM_D_OCP_QP_DIM_H_ + +#include "hpipm_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_ocp_qp_dim + { + int *nx; // number of states + int *nu; // number of inputs + int *nb; // number of box constraints + int *nbx; // number of state box constraints + int *nbu; // number of input box constraints + int *ng; // number of general constraints + int *ns; // number of soft constraints + int *nsbx; // number of soft state box constraints + int *nsbu; // number of soft input box constraints + int *nsg; // number of soft general constraints + int *nbxe; // number of state box constraints which are equality + int *nbue; // number of input box constraints which are equality + int *nge; // number of general constraints which are equality + int N; // horizon length + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_ocp_qp_dim_strsize(); +// +hpipm_size_t d_ocp_qp_dim_memsize(int N); +// +void d_ocp_qp_dim_create(int N, struct d_ocp_qp_dim *qp_dim, void *memory); +// +void d_ocp_qp_dim_copy_all(struct d_ocp_qp_dim *dim_orig, struct d_ocp_qp_dim *dim_dest); +// +void d_ocp_qp_dim_set_all(int *nx, int *nu, int *nbx, int *nbu, int *ng, int *nsbx, int *nsbu, int *nsg, struct d_ocp_qp_dim *dim); +// +void d_ocp_qp_dim_set(char *field, int stage, int value, struct d_ocp_qp_dim *dim); +// +void d_ocp_qp_dim_set_nx(int stage, int value, struct d_ocp_qp_dim *dim); +// +void d_ocp_qp_dim_set_nu(int stage, int value, struct d_ocp_qp_dim *dim); +// +void d_ocp_qp_dim_set_nbx(int stage, int value, struct d_ocp_qp_dim *dim); +// +void d_ocp_qp_dim_set_nbu(int stage, int value, struct d_ocp_qp_dim *dim); +// +void d_ocp_qp_dim_set_ng(int stage, int value, struct d_ocp_qp_dim *dim); +// +void d_ocp_qp_dim_set_ns(int stage, int value, struct d_ocp_qp_dim *dim); +// +void d_ocp_qp_dim_set_nsbx(int stage, int value, struct d_ocp_qp_dim *dim); +// +void d_ocp_qp_dim_set_nsbu(int stage, int value, struct d_ocp_qp_dim *dim); +// +void d_ocp_qp_dim_set_nsg(int stage, int value, struct d_ocp_qp_dim *dim); +// +void d_ocp_qp_dim_set_nbxe(int stage, int value, struct d_ocp_qp_dim *dim); +// +void d_ocp_qp_dim_set_nbue(int stage, int value, struct d_ocp_qp_dim *dim); +// +void d_ocp_qp_dim_set_nge(int stage, int value, struct d_ocp_qp_dim *dim); +// +void d_ocp_qp_dim_get(struct d_ocp_qp_dim *dim, char *field, int stage, int *value); +// +void d_ocp_qp_dim_get_N(struct d_ocp_qp_dim *dim, int *value); +// +void d_ocp_qp_dim_get_nx(struct d_ocp_qp_dim *dim, int stage, int *value); +// +void d_ocp_qp_dim_get_nu(struct d_ocp_qp_dim *dim, int stage, int *value); +// +void d_ocp_qp_dim_get_nbx(struct d_ocp_qp_dim *dim, int stage, int *value); +// +void d_ocp_qp_dim_get_nbu(struct d_ocp_qp_dim *dim, int stage, int *value); +// +void d_ocp_qp_dim_get_ng(struct d_ocp_qp_dim *dim, int stage, int *value); +// +void d_ocp_qp_dim_get_ns(struct d_ocp_qp_dim *dim, int stage, int *value); +// +void d_ocp_qp_dim_get_nsbx(struct d_ocp_qp_dim *dim, int stage, int *value); +// +void d_ocp_qp_dim_get_nsbu(struct d_ocp_qp_dim *dim, int stage, int *value); +// +void d_ocp_qp_dim_get_nsg(struct d_ocp_qp_dim *dim, int stage, int *value); +// +void d_ocp_qp_dim_get_nbxe(struct d_ocp_qp_dim *dim, int stage, int *value); +// +void d_ocp_qp_dim_get_nbue(struct d_ocp_qp_dim *dim, int stage, int *value); +// +void d_ocp_qp_dim_get_nge(struct d_ocp_qp_dim *dim, int stage, int *value); + + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_D_OCP_QP_DIM_H_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_ocp_qp_ipm.h b/phonelibs/acados/include/hpipm/include/hpipm_d_ocp_qp_ipm.h new file mode 100644 index 0000000000..d2a2833f22 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_ocp_qp_ipm.h @@ -0,0 +1,250 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_OCP_QP_IPM_H_ +#define HPIPM_D_OCP_QP_IPM_H_ + + + +#include +#include + +#include +#include +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_ocp_qp_ipm_arg + { + double mu0; // initial value for complementarity slackness + double alpha_min; // exit cond on step length + double res_g_max; // exit cond on inf norm of residuals + double res_b_max; // exit cond on inf norm of residuals + double res_d_max; // exit cond on inf norm of residuals + double res_m_max; // exit cond on inf norm of residuals + double reg_prim; // reg of primal hessian + double lam_min; // min value in lam vector + double t_min; // min value in t vector + double tau_min; // min value of barrier parameter + int iter_max; // exit cond in iter number + int stat_max; // iterations saved in stat + int pred_corr; // use Mehrotra's predictor-corrector IPM algirthm + int cond_pred_corr; // conditional Mehrotra's predictor-corrector + int itref_pred_max; // max number of iterative refinement steps for predictor step + int itref_corr_max; // max number of iterative refinement steps for corrector step + int warm_start; // 0 no warm start, 1 warm start primal sol, 2 warm start primal and dual sol + int square_root_alg; // 0 classical Riccati, 1 square-root Riccati + int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq (for square_root_alg==1) + int abs_form; // absolute IPM formulation + int comp_dual_sol_eq; // dual solution of equality constraints (only for abs_form==1) + int comp_res_exit; // compute residuals on exit (only for abs_form==1 and comp_dual_sol_eq==1) + int comp_res_pred; // compute residuals of prediction + int split_step; // use different steps for primal and dual variables + int var_init_scheme; // variables initialization scheme + int t_lam_min; // clip t and lam: 0 no, 1 in Gamma computation, 2 in solution + int mode; + hpipm_size_t memsize; + }; + + + +struct d_ocp_qp_ipm_ws + { + double qp_res[4]; // infinity norm of residuals + struct d_core_qp_ipm_workspace *core_workspace; + struct d_ocp_qp_dim *dim; // cache dim + struct d_ocp_qp_res_ws *res_workspace; + struct d_ocp_qp_sol *sol_step; + struct d_ocp_qp_sol *sol_itref; + struct d_ocp_qp *qp_step; + struct d_ocp_qp *qp_itref; + struct d_ocp_qp_res *res_itref; + struct d_ocp_qp_res *res; + struct blasfeo_dvec *Gamma; // hessian update + struct blasfeo_dvec *gamma; // hessian update + struct blasfeo_dvec *tmp_nuxM; // work space of size nxM + struct blasfeo_dvec *tmp_nbgM; // work space of size nbM+ngM + struct blasfeo_dvec *tmp_nsM; // work space of size nsM + struct blasfeo_dvec *Pb; // Pb + struct blasfeo_dvec *Zs_inv; + struct blasfeo_dvec *tmp_m; + struct blasfeo_dvec *l; // cache linear part for _get_ric_xxx + struct blasfeo_dmat *L; + struct blasfeo_dmat *Ls; + struct blasfeo_dmat *P; + struct blasfeo_dmat *Lh; + struct blasfeo_dmat *AL; + struct blasfeo_dmat *lq0; + struct blasfeo_dmat *tmp_nxM_nxM; + double *stat; // convergence statistics + int *use_hess_fact; + void *lq_work0; + int iter; // iteration number + int stat_max; // iterations saved in stat + int stat_m; // number of recorded stat per IPM iter + int use_Pb; + int status; // solver status + int square_root_alg; // cache from arg + int lq_fact; // cache from arg + int mask_constr; // use constr mask + int valid_ric_vec; // meaningful riccati vectors + int valid_ric_p; // form of riccati p: 0 p*inv(L), 1 p + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_ocp_qp_ipm_arg_strsize(); +// +hpipm_size_t d_ocp_qp_ipm_arg_memsize(struct d_ocp_qp_dim *ocp_dim); +// +void d_ocp_qp_ipm_arg_create(struct d_ocp_qp_dim *ocp_dim, struct d_ocp_qp_ipm_arg *arg, void *mem); +// +void d_ocp_qp_ipm_arg_set_default(enum hpipm_mode mode, struct d_ocp_qp_ipm_arg *arg); +// +void d_ocp_qp_ipm_arg_set(char *field, void *value, struct d_ocp_qp_ipm_arg *arg); +// set maximum number of iterations +void d_ocp_qp_ipm_arg_set_iter_max(int *iter_max, struct d_ocp_qp_ipm_arg *arg); +// set minimum step lenght +void d_ocp_qp_ipm_arg_set_alpha_min(double *alpha_min, struct d_ocp_qp_ipm_arg *arg); +// set initial value of barrier parameter +void d_ocp_qp_ipm_arg_set_mu0(double *mu0, struct d_ocp_qp_ipm_arg *arg); +// set exit tolerance on stationarity condition +void d_ocp_qp_ipm_arg_set_tol_stat(double *tol_stat, struct d_ocp_qp_ipm_arg *arg); +// set exit tolerance on equality constr +void d_ocp_qp_ipm_arg_set_tol_eq(double *tol_eq, struct d_ocp_qp_ipm_arg *arg); +// set exit tolerance on inequality constr +void d_ocp_qp_ipm_arg_set_tol_ineq(double *tol_ineq, struct d_ocp_qp_ipm_arg *arg); +// set exit tolerance on complementarity condition +void d_ocp_qp_ipm_arg_set_tol_comp(double *tol_comp, struct d_ocp_qp_ipm_arg *arg); +// set regularization of primal variables +void d_ocp_qp_ipm_arg_set_reg_prim(double *tol_comp, struct d_ocp_qp_ipm_arg *arg); +// set warm start: 0 no warm start, 1 primal var +void d_ocp_qp_ipm_arg_set_warm_start(int *warm_start, struct d_ocp_qp_ipm_arg *arg); +// Mehrotra's predictor-corrector IPM algorithm: 0 no predictor-corrector, 1 use predictor-corrector +void d_ocp_qp_ipm_arg_set_pred_corr(int *pred_corr, struct d_ocp_qp_ipm_arg *arg); +// conditional predictor-corrector: 0 no conditinal predictor-corrector, 1 conditional predictor-corrector +void d_ocp_qp_ipm_arg_set_cond_pred_corr(int *value, struct d_ocp_qp_ipm_arg *arg); +// set riccati algorithm: 0 classic, 1 square-root +void d_ocp_qp_ipm_arg_set_ric_alg(int *value, struct d_ocp_qp_ipm_arg *arg); +// dual solution of equality constraints (only for abs_form==1) +void d_ocp_qp_ipm_arg_set_comp_dual_sol_eq(int *value, struct d_ocp_qp_ipm_arg *arg); +// compute residuals after solution +void d_ocp_qp_ipm_arg_set_comp_res_exit(int *value, struct d_ocp_qp_ipm_arg *arg); +// compute residuals of prediction +void d_ocp_qp_ipm_arg_set_comp_res_pred(int *value, struct d_ocp_qp_ipm_arg *arg); +// min value of lam in the solution +void d_ocp_qp_ipm_arg_set_lam_min(double *value, struct d_ocp_qp_ipm_arg *arg); +// min value of t in the solution +void d_ocp_qp_ipm_arg_set_t_min(double *value, struct d_ocp_qp_ipm_arg *arg); +// min value of tau in the solution +void d_ocp_qp_ipm_arg_set_tau_min(double *value, struct d_ocp_qp_ipm_arg *arg); +// set split step: 0 same step, 1 different step for primal and dual variables +void d_ocp_qp_ipm_arg_set_split_step(int *value, struct d_ocp_qp_ipm_arg *arg); +// variables initialization scheme +void d_ocp_qp_ipm_arg_set_var_init_scheme(int *value, struct d_ocp_qp_ipm_arg *arg); +// clip t and lam: 0 no, 1 in Gamma computation, 2 in solution +void d_ocp_qp_ipm_arg_set_t_lam_min(int *value, struct d_ocp_qp_ipm_arg *arg); + +// +hpipm_size_t d_ocp_qp_ipm_ws_strsize(); +// +hpipm_size_t d_ocp_qp_ipm_ws_memsize(struct d_ocp_qp_dim *ocp_dim, struct d_ocp_qp_ipm_arg *arg); +// +void d_ocp_qp_ipm_ws_create(struct d_ocp_qp_dim *ocp_dim, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws, void *mem); +// +void d_ocp_qp_ipm_get(char *field, struct d_ocp_qp_ipm_ws *ws, void *value); +// +void d_ocp_qp_ipm_get_status(struct d_ocp_qp_ipm_ws *ws, int *status); +// +void d_ocp_qp_ipm_get_iter(struct d_ocp_qp_ipm_ws *ws, int *iter); +// +void d_ocp_qp_ipm_get_max_res_stat(struct d_ocp_qp_ipm_ws *ws, double *res_stat); +// +void d_ocp_qp_ipm_get_max_res_eq(struct d_ocp_qp_ipm_ws *ws, double *res_eq); +// +void d_ocp_qp_ipm_get_max_res_ineq(struct d_ocp_qp_ipm_ws *ws, double *res_ineq); +// +void d_ocp_qp_ipm_get_max_res_comp(struct d_ocp_qp_ipm_ws *ws, double *res_comp); +// +void d_ocp_qp_ipm_get_stat(struct d_ocp_qp_ipm_ws *ws, double **stat); +// +void d_ocp_qp_ipm_get_stat_m(struct d_ocp_qp_ipm_ws *ws, int *stat_m); +// +void d_ocp_qp_ipm_get_ric_Lr(struct d_ocp_qp *qp, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws, int stage, double *Lr); +// +void d_ocp_qp_ipm_get_ric_Ls(struct d_ocp_qp *qp, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws, int stage, double *Ls); +// +void d_ocp_qp_ipm_get_ric_P(struct d_ocp_qp *qp, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws, int stage, double *P); +// +void d_ocp_qp_ipm_get_ric_lr(struct d_ocp_qp *qp, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws, int stage, double *lr); +// +void d_ocp_qp_ipm_get_ric_p(struct d_ocp_qp *qp, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws, int stage, double *p); +// feedback control gain in the form u = K x + k +void d_ocp_qp_ipm_get_ric_K(struct d_ocp_qp *qp, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws, int stage, double *K); +// feedback control gain in the form u = K x + k +void d_ocp_qp_ipm_get_ric_k(struct d_ocp_qp *qp, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws, int stage, double *k); +// +void d_ocp_qp_init_var(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws); +// +void d_ocp_qp_ipm_abs_step(int kk, struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws); +// +void d_ocp_qp_ipm_delta_step(int kk, struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws); +// +void d_ocp_qp_ipm_solve(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws); +// +void d_ocp_qp_ipm_predict(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws); +// +void d_ocp_qp_ipm_sens(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws); + + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + +#endif // HPIPM_D_OCP_QP_IPM_H_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_ocp_qp_kkt.h b/phonelibs/acados/include/hpipm/include/hpipm_d_ocp_qp_kkt.h new file mode 100644 index 0000000000..19dcec32ca --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_ocp_qp_kkt.h @@ -0,0 +1,66 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_OCP_QP_KKT_H_ +#define HPIPM_D_OCP_QP_KKT_H_ + +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + +// +void d_ocp_qp_fact_solve_kkt_unconstr(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws); +// +void d_ocp_qp_fact_solve_kkt_step(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws); +// +void d_ocp_qp_fact_lq_solve_kkt_step(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws); +// +void d_ocp_qp_solve_kkt_step(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + +#endif // HPIPM_D_OCP_QP_KKT_H_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_ocp_qp_red.h b/phonelibs/acados/include/hpipm/include/hpipm_d_ocp_qp_red.h new file mode 100644 index 0000000000..f0482b0444 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_ocp_qp_red.h @@ -0,0 +1,117 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_OCP_QP_RED_H_ +#define HPIPM_D_OCP_QP_RED_H_ + + + +#include +#include + +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_ocp_qp_reduce_eq_dof_arg + { + double lam_min; + double t_min; + int alias_unchanged; // do not keep copy unchanged stage + int comp_prim_sol; // primal solution (v) + int comp_dual_sol_eq; // dual solution equality constr (pi) + int comp_dual_sol_ineq; // dual solution inequality constr (lam t) + hpipm_size_t memsize; // memory size in bytes + }; + + + +struct d_ocp_qp_reduce_eq_dof_ws + { + struct blasfeo_dvec *tmp_nuxM; + struct blasfeo_dvec *tmp_nbgM; + int *e_imask_ux; + int *e_imask_d; + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +void d_ocp_qp_dim_reduce_eq_dof(struct d_ocp_qp_dim *dim, struct d_ocp_qp_dim *dim_red); +// +hpipm_size_t d_ocp_qp_reduce_eq_dof_arg_memsize(); +// +void d_ocp_qp_reduce_eq_dof_arg_create(struct d_ocp_qp_reduce_eq_dof_arg *arg, void *mem); +// +void d_ocp_qp_reduce_eq_dof_arg_set_default(struct d_ocp_qp_reduce_eq_dof_arg *arg); +// +void d_ocp_qp_reduce_eq_dof_arg_set_alias_unchanged(struct d_ocp_qp_reduce_eq_dof_arg *arg, int value); +// +void d_ocp_qp_reduce_eq_dof_arg_set_comp_prim_sol(struct d_ocp_qp_reduce_eq_dof_arg *arg, int value); +// +void d_ocp_qp_reduce_eq_dof_arg_set_comp_dual_sol_eq(struct d_ocp_qp_reduce_eq_dof_arg *arg, int value); +// +void d_ocp_qp_reduce_eq_dof_arg_set_comp_dual_sol_ineq(struct d_ocp_qp_reduce_eq_dof_arg *arg, int value); +// +hpipm_size_t d_ocp_qp_reduce_eq_dof_ws_memsize(struct d_ocp_qp_dim *dim); +// +void d_ocp_qp_reduce_eq_dof_ws_create(struct d_ocp_qp_dim *dim, struct d_ocp_qp_reduce_eq_dof_ws *work, void *mem); +// +void d_ocp_qp_reduce_eq_dof(struct d_ocp_qp *qp, struct d_ocp_qp *qp_red, struct d_ocp_qp_reduce_eq_dof_arg *arg, struct d_ocp_qp_reduce_eq_dof_ws *work); +// +void d_ocp_qp_reduce_eq_dof_lhs(struct d_ocp_qp *qp, struct d_ocp_qp *qp_red, struct d_ocp_qp_reduce_eq_dof_arg *arg, struct d_ocp_qp_reduce_eq_dof_ws *work); +// +void d_ocp_qp_reduce_eq_dof_rhs(struct d_ocp_qp *qp, struct d_ocp_qp *qp_red, struct d_ocp_qp_reduce_eq_dof_arg *arg, struct d_ocp_qp_reduce_eq_dof_ws *work); +// +void d_ocp_qp_restore_eq_dof(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol_red, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_reduce_eq_dof_arg *arg, struct d_ocp_qp_reduce_eq_dof_ws *work); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_D_OCP_QP_RED_H_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_ocp_qp_res.h b/phonelibs/acados/include/hpipm/include/hpipm_d_ocp_qp_res.h new file mode 100644 index 0000000000..061c5729cd --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_ocp_qp_res.h @@ -0,0 +1,112 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_OCP_QP_RES_H_ +#define HPIPM_D_OCP_QP_RES_H_ + + + +#include +#include + +#include +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_ocp_qp_res + { + struct d_ocp_qp_dim *dim; + struct blasfeo_dvec *res_g; // q-residuals + struct blasfeo_dvec *res_b; // b-residuals + struct blasfeo_dvec *res_d; // d-residuals + struct blasfeo_dvec *res_m; // m-residuals + double res_max[4]; // max of residuals + double res_mu; // mu-residual + hpipm_size_t memsize; + }; + + + +struct d_ocp_qp_res_ws + { + struct blasfeo_dvec *tmp_nbgM; // work space of size nbM+ngM + struct blasfeo_dvec *tmp_nsM; // work space of size nsM + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_ocp_qp_res_memsize(struct d_ocp_qp_dim *ocp_dim); +// +void d_ocp_qp_res_create(struct d_ocp_qp_dim *ocp_dim, struct d_ocp_qp_res *res, void *mem); +// +hpipm_size_t d_ocp_qp_res_ws_memsize(struct d_ocp_qp_dim *ocp_dim); +// +void d_ocp_qp_res_ws_create(struct d_ocp_qp_dim *ocp_dim, struct d_ocp_qp_res_ws *workspace, void *mem); +// +void d_ocp_qp_res_compute(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_res *res, struct d_ocp_qp_res_ws *ws); +// +void d_ocp_qp_res_compute_lin(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_sol *qp_step, struct d_ocp_qp_res *res, struct d_ocp_qp_res_ws *ws); +// +void d_ocp_qp_res_compute_inf_norm(struct d_ocp_qp_res *res); +// +void d_ocp_qp_res_get_all(struct d_ocp_qp_res *res, double **res_r, double **res_q, double **res_ls, double **res_us, double **res_b, double **res_d_lb, double **res_d_ub, double **res_d_lg, double **res_d_ug, double **res_d_ls, double **res_d_us, double **res_m_lb, double **res_m_ub, double **res_m_lg, double **res_m_ug, double **res_m_ls, double **res_m_us); +// +void d_ocp_qp_res_get_max_res_stat(struct d_ocp_qp_res *res, double *value); +// +void d_ocp_qp_res_get_max_res_eq(struct d_ocp_qp_res *res, double *value); +// +void d_ocp_qp_res_get_max_res_ineq(struct d_ocp_qp_res *res, double *value); +// +void d_ocp_qp_res_get_max_res_comp(struct d_ocp_qp_res *res, double *value); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + +#endif // HPIPM_D_OCP_QP_RES_H_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_ocp_qp_sol.h b/phonelibs/acados/include/hpipm/include/hpipm_d_ocp_qp_sol.h new file mode 100644 index 0000000000..eb91f3a7b1 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_ocp_qp_sol.h @@ -0,0 +1,128 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_OCP_QP_SOL_H_ +#define HPIPM_D_OCP_QP_SOL_H_ + + + +#include +#include + +#include "hpipm_d_ocp_qp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_ocp_qp_sol + { + struct d_ocp_qp_dim *dim; + struct blasfeo_dvec *ux; + struct blasfeo_dvec *pi; + struct blasfeo_dvec *lam; + struct blasfeo_dvec *t; + void *misc; + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +hpipm_size_t d_ocp_qp_sol_strsize(); +// +hpipm_size_t d_ocp_qp_sol_memsize(struct d_ocp_qp_dim *dim); +// +void d_ocp_qp_sol_create(struct d_ocp_qp_dim *dim, struct d_ocp_qp_sol *qp_sol, void *memory); +// +void d_ocp_qp_sol_copy_all(struct d_ocp_qp_sol *qp_sol_orig, struct d_ocp_qp_sol *qp_sol_dest); +// +void d_ocp_qp_sol_get_all(struct d_ocp_qp_sol *qp_sol, double **u, double **x, double **ls, double **us, double **pi, double **lam_lb, double **lam_ub, double **lam_lg, double **lam_ug, double **lam_ls, double **lam_us); +// +void d_ocp_qp_sol_get_all_rowmaj(struct d_ocp_qp_sol *qp_sol, double **u, double **x, double **ls, double **us, double **pi, double **lam_lb, double **lam_ub, double **lam_lg, double **lam_ug, double **lam_ls, double **lam_us); +// +void d_ocp_qp_sol_set_all(double **u, double **x, double **ls, double **us, double **pi, double **lam_lb, double **lam_ub, double **lam_lg, double **lam_ug, double **lam_ls, double **lam_us, struct d_ocp_qp_sol *qp_sol); +// +void d_ocp_qp_sol_get(char *field, int stage, struct d_ocp_qp_sol *qp_sol, double *vec); +// +void d_ocp_qp_sol_get_u(int stage, struct d_ocp_qp_sol *qp_sol, double *vec); +// +void d_ocp_qp_sol_get_x(int stage, struct d_ocp_qp_sol *qp_sol, double *vec); +// +void d_ocp_qp_sol_get_sl(int stage, struct d_ocp_qp_sol *qp_sol, double *vec); +// +void d_ocp_qp_sol_get_su(int stage, struct d_ocp_qp_sol *qp_sol, double *vec); +// +void d_ocp_qp_sol_get_pi(int stage, struct d_ocp_qp_sol *qp_sol, double *vec); +// +void d_ocp_qp_sol_get_lam_lb(int stage, struct d_ocp_qp_sol *qp_sol, double *vec); +// +void d_ocp_qp_sol_get_lam_lbu(int stage, struct d_ocp_qp_sol *qp_sol, double *vec); +// +void d_ocp_qp_sol_get_lam_lbx(int stage, struct d_ocp_qp_sol *qp_sol, double *vec); +// +void d_ocp_qp_sol_get_lam_ub(int stage, struct d_ocp_qp_sol *qp_sol, double *vec); +// +void d_ocp_qp_sol_get_lam_ubu(int stage, struct d_ocp_qp_sol *qp_sol, double *vec); +// +void d_ocp_qp_sol_get_lam_ubx(int stage, struct d_ocp_qp_sol *qp_sol, double *vec); +// +void d_ocp_qp_sol_get_lam_lg(int stage, struct d_ocp_qp_sol *qp_sol, double *vec); +// +void d_ocp_qp_sol_get_lam_ug(int stage, struct d_ocp_qp_sol *qp_sol, double *vec); +// +void d_ocp_qp_sol_set(char *field, int stage, double *vec, struct d_ocp_qp_sol *qp_sol); +// +void d_ocp_qp_sol_set_u(int stage, double *vec, struct d_ocp_qp_sol *qp_sol); +// +void d_ocp_qp_sol_set_x(int stage, double *vec, struct d_ocp_qp_sol *qp_sol); +// +void d_ocp_qp_sol_set_sl(int stage, double *vec, struct d_ocp_qp_sol *qp_sol); +// +void d_ocp_qp_sol_set_su(int stage, double *vec, struct d_ocp_qp_sol *qp_sol); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_D_OCP_QP_SOL_H_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_ocp_qp_utils.h b/phonelibs/acados/include/hpipm/include/hpipm_d_ocp_qp_utils.h new file mode 100644 index 0000000000..217e9d94ae --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_ocp_qp_utils.h @@ -0,0 +1,82 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_OCP_QP_UTILS_H_ +#define HPIPM_D_OCP_QP_UTILS_H_ + + + +#include +#include + +#include "hpipm_d_ocp_qp_dim.h" +#include "hpipm_d_ocp_qp.h" +#include "hpipm_d_ocp_qp_sol.h" +#include "hpipm_d_ocp_qp_ipm.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +// +void d_ocp_qp_dim_print(struct d_ocp_qp_dim *qp_dim); +// +void d_ocp_qp_dim_codegen(char *file_name, char *mode, struct d_ocp_qp_dim *qp_dim); +// +void d_ocp_qp_print(struct d_ocp_qp_dim *qp_dim, struct d_ocp_qp *qp); +// +void d_ocp_qp_codegen(char *file_name, char *mode, struct d_ocp_qp_dim *qp_dim, struct d_ocp_qp *qp); +// +void d_ocp_qp_sol_print(struct d_ocp_qp_dim *qp_dim, struct d_ocp_qp_sol *ocp_qp_sol); +// +void d_ocp_qp_ipm_arg_print(struct d_ocp_qp_dim *qp_dim, struct d_ocp_qp_ipm_arg *arg); +// +void d_ocp_qp_ipm_arg_codegen(char *file_name, char *mode, struct d_ocp_qp_dim *qp_dim, struct d_ocp_qp_ipm_arg *arg); +// +void d_ocp_qp_res_print(struct d_ocp_qp_dim *qp_dim, struct d_ocp_qp_res *ocp_qp_res); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_D_OCP_QP_UTILS_H_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_part_cond.h b/phonelibs/acados/include/hpipm/include/hpipm_d_part_cond.h new file mode 100644 index 0000000000..1c5f785035 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_part_cond.h @@ -0,0 +1,115 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_D_PART_COND_H_ +#define HPIPM_D_PART_COND_H_ + + + +#include +#include + +#include "hpipm_common.h" +#include "hpipm_d_cond.h" + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_part_cond_qp_arg + { + struct d_cond_qp_arg *cond_arg; + int N2; + hpipm_size_t memsize; + }; + + + +struct d_part_cond_qp_ws + { + struct d_cond_qp_ws *cond_workspace; + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_part_cond_qp_arg_memsize(int N2); +// +void d_part_cond_qp_arg_create(int N2, struct d_part_cond_qp_arg *cond_arg, void *mem); +// +void d_part_cond_qp_arg_set_default(struct d_part_cond_qp_arg *cond_arg); +// set riccati-like algorithm: 0 classical, 1 squre-root +void d_part_cond_qp_arg_set_ric_alg(int ric_alg, struct d_part_cond_qp_arg *cond_arg); +// +void d_part_cond_qp_arg_set_comp_prim_sol(int value, struct d_part_cond_qp_arg *cond_arg); +// +void d_part_cond_qp_arg_set_comp_dual_sol_eq(int value, struct d_part_cond_qp_arg *cond_arg); +// +void d_part_cond_qp_arg_set_comp_dual_sol_ineq(int value, struct d_part_cond_qp_arg *cond_arg); + +// +void d_part_cond_qp_compute_block_size(int N, int N2, int *block_size); +// +void d_part_cond_qp_compute_dim(struct d_ocp_qp_dim *ocp_dim, int *block_size, struct d_ocp_qp_dim *part_dense_dim); +// +hpipm_size_t d_part_cond_qp_ws_memsize(struct d_ocp_qp_dim *ocp_dim, int *block_size, struct d_ocp_qp_dim *part_dense_dim, struct d_part_cond_qp_arg *cond_arg); +// +void d_part_cond_qp_ws_create(struct d_ocp_qp_dim *ocp_dim, int *block_size, struct d_ocp_qp_dim *part_dense_dim, struct d_part_cond_qp_arg *cond_arg, struct d_part_cond_qp_ws *cond_ws, void *mem); +// +void d_part_cond_qp_cond(struct d_ocp_qp *ocp_qp, struct d_ocp_qp *part_dense_qp, struct d_part_cond_qp_arg *cond_arg, struct d_part_cond_qp_ws *cond_ws); +// +void d_part_cond_qp_cond_lhs(struct d_ocp_qp *ocp_qp, struct d_ocp_qp *part_dense_qp, struct d_part_cond_qp_arg *cond_arg, struct d_part_cond_qp_ws *cond_ws); +// +void d_part_cond_qp_cond_rhs(struct d_ocp_qp *ocp_qp, struct d_ocp_qp *part_dense_qp, struct d_part_cond_qp_arg *cond_arg, struct d_part_cond_qp_ws *cond_ws); +// +void d_part_cond_qp_expand_sol(struct d_ocp_qp *ocp_qp, struct d_ocp_qp *part_dense_qp, struct d_ocp_qp_sol *part_dense_qp_sol, struct d_ocp_qp_sol *ocp_qp_sol, struct d_part_cond_qp_arg *cond_arg, struct d_part_cond_qp_ws *cond_ws); + +// +void d_part_cond_qp_update(int *idxc, struct d_ocp_qp *ocp_qp, struct d_ocp_qp *part_dense_qp, struct d_part_cond_qp_arg *cond_arg, struct d_part_cond_qp_ws *cond_ws); + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_D_PART_COND_H_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_part_cond_qcqp.h b/phonelibs/acados/include/hpipm/include/hpipm_d_part_cond_qcqp.h new file mode 100644 index 0000000000..2d6624e5bb --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_part_cond_qcqp.h @@ -0,0 +1,106 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_D_PART_COND_QCQP_H_ +#define HPIPM_D_PART_COND_QCQP_H_ + + + +#include +#include + +#include "hpipm_d_cond_qcqp.h" + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_part_cond_qcqp_arg + { + struct d_cond_qcqp_arg *cond_arg; + int N2; + hpipm_size_t memsize; + }; + + + +struct d_part_cond_qcqp_ws + { + struct d_cond_qcqp_ws *cond_ws; + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_part_cond_qcqp_arg_memsize(int N2); +// +void d_part_cond_qcqp_arg_create(int N2, struct d_part_cond_qcqp_arg *cond_arg, void *mem); +// +void d_part_cond_qcqp_arg_set_default(struct d_part_cond_qcqp_arg *cond_arg); +// set riccati-like algorithm: 0 classical, 1 squre-root +void d_part_cond_qcqp_arg_set_ric_alg(int ric_alg, struct d_part_cond_qcqp_arg *cond_arg); + +// +void d_part_cond_qcqp_compute_block_size(int N, int N2, int *block_size); +// +void d_part_cond_qcqp_compute_dim(struct d_ocp_qcqp_dim *ocp_dim, int *block_size, struct d_ocp_qcqp_dim *part_dense_dim); +// +hpipm_size_t d_part_cond_qcqp_ws_memsize(struct d_ocp_qcqp_dim *ocp_dim, int *block_size, struct d_ocp_qcqp_dim *part_dense_dim, struct d_part_cond_qcqp_arg *cond_arg); +// +void d_part_cond_qcqp_ws_create(struct d_ocp_qcqp_dim *ocp_dim, int *block_size, struct d_ocp_qcqp_dim *part_dense_dim, struct d_part_cond_qcqp_arg *cond_arg, struct d_part_cond_qcqp_ws *cond_ws, void *mem); +// +void d_part_cond_qcqp_cond(struct d_ocp_qcqp *ocp_qp, struct d_ocp_qcqp *part_dense_qp, struct d_part_cond_qcqp_arg *cond_arg, struct d_part_cond_qcqp_ws *cond_ws); +// +void d_part_cond_qcqp_cond_lhs(struct d_ocp_qcqp *ocp_qp, struct d_ocp_qcqp *part_dense_qp, struct d_part_cond_qcqp_arg *cond_arg, struct d_part_cond_qcqp_ws *cond_ws); +// +void d_part_cond_qcqp_cond_rhs(struct d_ocp_qcqp *ocp_qp, struct d_ocp_qcqp *part_dense_qp, struct d_part_cond_qcqp_arg *cond_arg, struct d_part_cond_qcqp_ws *cond_ws); +// +void d_part_cond_qcqp_expand_sol(struct d_ocp_qcqp *ocp_qp, struct d_ocp_qcqp *part_dense_qp, struct d_ocp_qcqp_sol *part_dense_qp_sol, struct d_ocp_qcqp_sol *ocp_qp_sol, struct d_part_cond_qcqp_arg *cond_arg, struct d_part_cond_qcqp_ws *cond_ws); + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_D_PART_COND_H_ + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_sim_erk.h b/phonelibs/acados/include/hpipm/include/hpipm_d_sim_erk.h new file mode 100644 index 0000000000..320b66bb28 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_sim_erk.h @@ -0,0 +1,122 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_SIM_ERK_H_ +#define HPIPM_D_SIM_ERK_H_ + +#include "hpipm_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +struct d_sim_erk_arg + { + struct d_sim_rk_data *rk_data; // integrator data + double h; // step size + int steps; // number of steps +// int for_sens; // compute adjoint sensitivities +// int adj_sens; // compute adjoint sensitivities + hpipm_size_t memsize; + }; + + + +struct d_sim_erk_ws + { + void (*ode)(int t, double *x, double *p, void *ode_args, double *xdot); // function pointer to ode + void (*vde_for)(int t, double *x, double *p, void *ode_args, double *xdot); // function pointer to forward vde + void (*vde_adj)(int t, double *adj_in, void *ode_args, double *adj_out); // function pointer to adjoint vde + void *ode_args; // pointer to ode args + struct d_sim_erk_arg *erk_arg; // erk arg + double *K; // internal variables + double *x_for; // states and forward sensitivities + double *x_traj; // states at all steps + double *l; // adjoint sensitivities + double *p; // parameter + double *x_tmp; // temporary states and forward sensitivities + double *adj_in; + double *adj_tmp; + int nx; // number of states + int np; // number of parameters + int nf; // number of forward sensitivities + int na; // number of adjoint sensitivities + int nf_max; // max number of forward sensitivities + int na_max; // max number of adjoint sensitivities + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_sim_erk_arg_memsize(); +// +void d_sim_erk_arg_create(struct d_sim_erk_arg *erk_arg, void *mem); +// +void d_sim_erk_arg_set_all(struct d_sim_rk_data *rk_data, double h, int steps, struct d_sim_erk_arg *erk_arg); + +// +hpipm_size_t d_sim_erk_ws_memsize(struct d_sim_erk_arg *erk_arg, int nx, int np, int nf_max, int na_max); +// +void d_sim_erk_ws_create(struct d_sim_erk_arg *erk_arg, int nx, int np, int nf_max, int na_max, struct d_sim_erk_ws *work, void *memory); +// +void d_sim_erk_ws_set_all(int nf, int na, double *x, double *fs, double *bs, double *p, void (*ode)(int t, double *x, double *p, void *ode_args, double *xdot), void (*vde_for)(int t, double *x, double *p, void *ode_args, double *xdot), void (*vde_adj)(int t, double *adj_in, void *ode_args, double *adj_out), void *ode_args, struct d_sim_erk_ws *work); +// number of directions for forward sensitivities +void d_sim_erk_ws_set_nf(int *nf, struct d_sim_erk_ws *work); +// parameters (e.g. inputs) +void d_sim_erk_ws_set_p(double *p, struct d_sim_erk_ws *work); +// state +void d_sim_erk_ws_set_x(double *x, struct d_sim_erk_ws *work); +// forward sensitivities +void d_sim_erk_ws_set_fs(double *fs, struct d_sim_erk_ws *work); +// ode funtion +void d_sim_erk_ws_set_ode(void (*ode)(int t, double *x, double *p, void *ode_args, double *xdot), struct d_sim_erk_ws *work); +// forward vde function +void d_sim_erk_ws_set_vde_for(void (*ode)(int t, double *x, double *p, void *ode_args, double *xdot), struct d_sim_erk_ws *work); +// ode_args, passed straight to the ode/vde_for/vde_adj functions +void d_sim_erk_ws_set_ode_args(void *ode_args, struct d_sim_erk_ws *work); +// state +void d_sim_erk_ws_get_x(struct d_sim_erk_ws *work, double *x); +// forward sensitivities +void d_sim_erk_ws_get_fs(struct d_sim_erk_ws *work, double *fs); +// +void d_sim_erk_solve(struct d_sim_erk_arg *arg, struct d_sim_erk_ws *work); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // HPIPM_D_SIM_ERK_H_ + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_sim_rk.h b/phonelibs/acados/include/hpipm/include/hpipm_d_sim_rk.h new file mode 100644 index 0000000000..a50dcbb491 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_sim_rk.h @@ -0,0 +1,71 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_SIM_RK_H_ +#define HPIPM_D_SIM_RK_H_ + +#include "hpipm_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +struct d_sim_rk_data + { + double *A_rk; // A in butcher tableau + double *B_rk; // b in butcher tableau + double *C_rk; // c in butcher tableau + int expl; // erk vs irk + int ns; // number of stages + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_sim_rk_data_memsize(int ns); +// +void d_sim_rk_data_create(int ns, struct d_sim_rk_data *rk_data, void *memory); +// +void d_sim_rk_data_init_default(char *field, struct d_sim_rk_data *rk_data); +// +void d_sim_rk_data_set_all(int expl, double *A_rk, double *B_rk, double *C_rk, struct d_sim_rk_data *rk_data); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // HPIPM_D_SIM_RK_H_ + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp.h b/phonelibs/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp.h new file mode 100644 index 0000000000..f36b51a064 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp.h @@ -0,0 +1,213 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_TREE_OCP_QCQP_H_ +#define HPIPM_D_TREE_OCP_QCQP_H_ + + + +#include +#include + +#include "hpipm_common.h" +#include "hpipm_d_tree_ocp_qcqp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_tree_ocp_qcqp + { + struct d_tree_ocp_qcqp_dim *dim; + struct blasfeo_dmat *BAbt; // Nn-1 + struct blasfeo_dmat *RSQrq; // Nn + struct blasfeo_dmat *DCt; // Nn + struct blasfeo_dmat **Hq; // Nn + struct blasfeo_dvec *b; // Nn-1 + struct blasfeo_dvec *rqz; // Nn + struct blasfeo_dvec *d; // Nn + struct blasfeo_dvec *d_mask; // Nn + struct blasfeo_dvec *m; // Nn + struct blasfeo_dvec *Z; // Nn + int **idxb; // indices of box constrained variables within [u; x] // Nn + int **idxs_rev; // index of soft constraints + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +hpipm_size_t d_tree_ocp_qcqp_memsize(struct d_tree_ocp_qcqp_dim *dim); +// +void d_tree_ocp_qcqp_create(struct d_tree_ocp_qcqp_dim *dim, struct d_tree_ocp_qcqp *qp, void *memory); +// +void d_tree_ocp_qcqp_set_all(double **A, double **B, double **b, double **Q, double **S, double **R, double **q, double **r, int **idxb, double **d_lb, double **d_ub, double **C, double **D, double **d_lg, double **d_ug, double **Zl, double **Zu, double **zl, double **zu, int **idxs, double **d_ls, double **d_us, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set(char *field_name, int node_edge, void *value, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_A(int edge, double *mat, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_B(int edge, double *mat, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_b(int edge, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_Q(int node, double *mat, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_S(int node, double *mat, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_R(int node, double *mat, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_q(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_r(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_lb(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_lb_mask(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_ub(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_ub_mask(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_lbx(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_lbx_mask(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_ubx(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_ubx_mask(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_lbu(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_lbu_mask(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_ubu(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_ubu_mask(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_idxb(int node, int *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_idxbx(int node, int *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_Jbx(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_idxbu(int node, int *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_Jbu(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_C(int node, double *mat, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_D(int node, double *mat, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_lg(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_lg_mask(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_ug(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_ug_mask(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_Qq(int node, double *mat, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_Sq(int node, double *mat, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_Rq(int node, double *mat, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_qq(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_rq(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_uq(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_uq_mask(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_Zl(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_Zu(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_zl(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_zu(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_lls(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_lls_mask(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_lus(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_lus_mask(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_idxs(int node, int *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_idxs_rev(int node, int *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_Jsbu(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_Jsbx(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_Jsg(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_Jsq(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +//void d_tree_ocp_qcqp_set_idxe(int node, int *vec, struct d_tree_ocp_qcqp *qp); +// +//void d_tree_ocp_qcqp_set_idxbxe(int node, int *vec, struct d_tree_ocp_qcqp *qp); +// +//void d_tree_ocp_qcqp_set_idxbue(int node, int *vec, struct d_tree_ocp_qcqp *qp); +// +//void d_tree_ocp_qcqp_set_idxge(int node, int *vec, struct d_tree_ocp_qcqp *qp); +// +//void d_tree_ocp_qcqp_set_Jbxe(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +//void d_tree_ocp_qcqp_set_Jbue(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +//void d_tree_ocp_qcqp_set_Jge(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +//void d_tree_ocp_qcqp_set_diag_H_flag(int node, int *value, struct d_tree_ocp_qcqp *qp); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_D_TREE_OCP_QCQP_H_ + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_dim.h b/phonelibs/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_dim.h new file mode 100644 index 0000000000..257b82b252 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_dim.h @@ -0,0 +1,117 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_TREE_OCP_QCQP_DIM_H_ +#define HPIPM_D_TREE_OCP_QCQP_DIM_H_ + +#include "hpipm_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_tree_ocp_qcqp_dim + { + struct d_tree_ocp_qp_dim *qp_dim; // dim of qp approximation + struct tree *ttree; // tree describing node conndection + int *nx; // number of states // Nn + int *nu; // number of inputs // Nn + int *nb; // number of box constraints // Nn + int *nbx; // number of state box constraints // Nn + int *nbu; // number of input box constraints // Nn + int *ng; // number of general constraints // Nn + int *nq; // number of (upper) quadratic constraints + int *ns; // number of soft constraints // Nn + int *nsbx; // number of soft state box constraints + int *nsbu; // number of soft input box constraints + int *nsg; // number of soft general constraints + int *nsq; // number of (upper) soft quadratic constraints + int Nn; // number of nodes + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_tree_ocp_qcqp_dim_strsize(); +// +hpipm_size_t d_tree_ocp_qcqp_dim_memsize(int Nn); +// +void d_tree_ocp_qcqp_dim_create(int Nn, struct d_tree_ocp_qcqp_dim *qp_dim, void *memory); +// +void d_tree_ocp_qcqp_dim_set_tree(struct tree *ttree, struct d_tree_ocp_qcqp_dim *dim); +// +void d_tree_ocp_qcqp_dim_set(char *field, int stage, int value, struct d_tree_ocp_qcqp_dim *dim); +// +void d_tree_ocp_qcqp_dim_set_nx(int stage, int value, struct d_tree_ocp_qcqp_dim *dim); +// +void d_tree_ocp_qcqp_dim_set_nu(int stage, int value, struct d_tree_ocp_qcqp_dim *dim); +// +void d_tree_ocp_qcqp_dim_set_nbx(int stage, int value, struct d_tree_ocp_qcqp_dim *dim); +// +void d_tree_ocp_qcqp_dim_set_nbu(int stage, int value, struct d_tree_ocp_qcqp_dim *dim); +// +void d_tree_ocp_qcqp_dim_set_ng(int stage, int value, struct d_tree_ocp_qcqp_dim *dim); +// +void d_tree_ocp_qcqp_dim_set_nq(int stage, int value, struct d_tree_ocp_qcqp_dim *dim); +// +void d_tree_ocp_qcqp_dim_set_ns(int stage, int value, struct d_tree_ocp_qcqp_dim *dim); +// +void d_tree_ocp_qcqp_dim_set_nsbx(int stage, int value, struct d_tree_ocp_qcqp_dim *dim); +// +void d_tree_ocp_qcqp_dim_set_nsbu(int stage, int value, struct d_tree_ocp_qcqp_dim *dim); +// +void d_tree_ocp_qcqp_dim_set_nsg(int stage, int value, struct d_tree_ocp_qcqp_dim *dim); +// +void d_tree_ocp_qcqp_dim_set_nsq(int stage, int value, struct d_tree_ocp_qcqp_dim *dim); +// +//void d_tree_ocp_qcqp_dim_set_nbxe(int stage, int value, struct d_tree_ocp_qcqp_dim *dim); +// +//void d_tree_ocp_qcqp_dim_set_nbue(int stage, int value, struct d_tree_ocp_qcqp_dim *dim); +// +//void d_tree_ocp_qcqp_dim_set_nge(int stage, int value, struct d_tree_ocp_qcqp_dim *dim); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_D_TREE_OCP_QCQP_DIM_H_ + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_ipm.h b/phonelibs/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_ipm.h new file mode 100644 index 0000000000..4a0fed1067 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_ipm.h @@ -0,0 +1,191 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_TREE_OCP_QCQP_IPM_H_ +#define HPIPM_D_TREE_OCP_QCQP_IPM_H_ + + + +#include +#include + +#include +#include +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_tree_ocp_qcqp_ipm_arg + { + struct d_tree_ocp_qp_ipm_arg *qp_arg; + double mu0; // initial value for complementarity slackness + double alpha_min; // exit cond on step length + double res_g_max; // exit cond on inf norm of residuals + double res_b_max; // exit cond on inf norm of residuals + double res_d_max; // exit cond on inf norm of residuals + double res_m_max; // exit cond on inf norm of residuals + double reg_prim; // reg of primal hessian + double lam_min; // min value in lam vector + double t_min; // min value in t vector + int iter_max; // exit cond in iter number + int stat_max; // iterations saved in stat + int pred_corr; // use Mehrotra's predictor-corrector IPM algirthm + int cond_pred_corr; // conditional Mehrotra's predictor-corrector + int itref_pred_max; // max number of iterative refinement steps for predictor step + int itref_corr_max; // max number of iterative refinement steps for corrector step + int warm_start; // 0 no warm start, 1 warm start primal sol, 2 warm start primal and dual sol +// int square_root_alg; // 0 classical Riccati, 1 square-root Riccati + int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq (for square_root_alg==1) + int abs_form; // absolute IPM formulation + int comp_dual_sol_eq; // dual solution of equality constrains (only for abs_form==1) + int comp_res_exit; // compute residuals on exit (only for abs_form==1 and comp_dual_sol_eq==1) +// int comp_res_pred; // compute residuals of prediction + int split_step; // use different step for primal and dual variables + int t_lam_min; // clip t and lam: 0 no, 1 in Gamma computation, 2 in solution + int mode; + hpipm_size_t memsize; + }; + + + +struct d_tree_ocp_qcqp_ipm_ws + { + struct d_tree_ocp_qp_ipm_ws *qp_ws; + struct d_tree_ocp_qp *qp; + struct d_tree_ocp_qp_sol *qp_sol; + struct d_tree_ocp_qcqp_res_ws *qcqp_res_ws; + struct d_tree_ocp_qcqp_res *qcqp_res; + struct blasfeo_dvec *tmp_nuxM; + int iter; // iteration number + int status; + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_tree_ocp_qcqp_ipm_arg_strsize(); +// +hpipm_size_t d_tree_ocp_qcqp_ipm_arg_memsize(struct d_tree_ocp_qcqp_dim *ocp_dim); +// +void d_tree_ocp_qcqp_ipm_arg_create(struct d_tree_ocp_qcqp_dim *ocp_dim, struct d_tree_ocp_qcqp_ipm_arg *arg, void *mem); +// +void d_tree_ocp_qcqp_ipm_arg_set_default(enum hpipm_mode mode, struct d_tree_ocp_qcqp_ipm_arg *arg); +// +void d_tree_ocp_qcqp_ipm_arg_set(char *field, void *value, struct d_tree_ocp_qcqp_ipm_arg *arg); +// set maximum number of iterations +void d_tree_ocp_qcqp_ipm_arg_set_iter_max(int *value, struct d_tree_ocp_qcqp_ipm_arg *arg); +// set minimum step lenght +void d_tree_ocp_qcqp_ipm_arg_set_alpha_min(double *value, struct d_tree_ocp_qcqp_ipm_arg *arg); +// set initial value of barrier parameter +void d_tree_ocp_qcqp_ipm_arg_set_mu0(double *value, struct d_tree_ocp_qcqp_ipm_arg *arg); +// set exit tolerance on stationarity condition +void d_tree_ocp_qcqp_ipm_arg_set_tol_stat(double *value, struct d_tree_ocp_qcqp_ipm_arg *arg); +// set exit tolerance on equality constr +void d_tree_ocp_qcqp_ipm_arg_set_tol_eq(double *value, struct d_tree_ocp_qcqp_ipm_arg *arg); +// set exit tolerance on inequality constr +void d_tree_ocp_qcqp_ipm_arg_set_tol_ineq(double *value, struct d_tree_ocp_qcqp_ipm_arg *arg); +// set exit tolerance on complementarity condition +void d_tree_ocp_qcqp_ipm_arg_set_tol_comp(double *value, struct d_tree_ocp_qcqp_ipm_arg *arg); +// set regularization of primal variables +void d_tree_ocp_qcqp_ipm_arg_set_reg_prim(double *value, struct d_tree_ocp_qcqp_ipm_arg *arg); +// set warm start: 0 no warm start, 1 primal var +void d_tree_ocp_qcqp_ipm_arg_set_warm_start(int *value, struct d_tree_ocp_qcqp_ipm_arg *arg); +// Mehrotra's predictor-corrector IPM algorithm: 0 no predictor-corrector, 1 use predictor-corrector +void d_tree_ocp_qcqp_ipm_arg_set_pred_corr(int *value, struct d_tree_ocp_qcqp_ipm_arg *arg); +// conditional predictor-corrector: 0 no conditinal predictor-corrector, 1 conditional predictor-corrector +void d_tree_ocp_qcqp_ipm_arg_set_cond_pred_corr(int *value, struct d_tree_ocp_qcqp_ipm_arg *arg); +// set riccati algorithm: 0 classic, 1 square-root +//void d_tree_ocp_qcqp_ipm_arg_set_ric_alg(int *value, struct d_tree_ocp_qcqp_ipm_arg *arg); +// compute residuals after solution +void d_tree_ocp_qcqp_ipm_arg_set_comp_res_exit(int *value, struct d_tree_ocp_qcqp_ipm_arg *arg); +// compute residuals of prediction +//void d_tree_ocp_qcqp_ipm_arg_set_comp_res_pred(int *value, struct d_tree_ocp_qcqp_ipm_arg *arg); +// min value of lam in the solution +void d_tree_ocp_qcqp_ipm_arg_set_lam_min(double *value, struct d_tree_ocp_qcqp_ipm_arg *arg); +// min value of t in the solution +void d_tree_ocp_qcqp_ipm_arg_set_t_min(double *value, struct d_tree_ocp_qcqp_ipm_arg *arg); +// use different step for primal and dual variables +void d_tree_ocp_qcqp_ipm_arg_set_split_step(int *value, struct d_tree_ocp_qcqp_ipm_arg *arg); +// clip t and lam: 0 no, 1 in Gamma computation, 2 in solution +void d_tree_ocp_qcqp_ipm_arg_set_t_lam_min(int *value, struct d_tree_ocp_qcqp_ipm_arg *arg); + +// +hpipm_size_t d_tree_ocp_qcqp_ipm_ws_strsize(); +// +hpipm_size_t d_tree_ocp_qcqp_ipm_ws_memsize(struct d_tree_ocp_qcqp_dim *ocp_dim, struct d_tree_ocp_qcqp_ipm_arg *arg); +// +void d_tree_ocp_qcqp_ipm_ws_create(struct d_tree_ocp_qcqp_dim *ocp_dim, struct d_tree_ocp_qcqp_ipm_arg *arg, struct d_tree_ocp_qcqp_ipm_ws *ws, void *mem); +// +void d_tree_ocp_qcqp_ipm_get(char *field, struct d_tree_ocp_qcqp_ipm_ws *ws, void *value); +// +void d_tree_ocp_qcqp_ipm_get_status(struct d_tree_ocp_qcqp_ipm_ws *ws, int *status); +// +void d_tree_ocp_qcqp_ipm_get_iter(struct d_tree_ocp_qcqp_ipm_ws *ws, int *iter); +// +void d_tree_ocp_qcqp_ipm_get_max_res_stat(struct d_tree_ocp_qcqp_ipm_ws *ws, double *res_stat); +// +void d_tree_ocp_qcqp_ipm_get_max_res_eq(struct d_tree_ocp_qcqp_ipm_ws *ws, double *res_eq); +// +void d_tree_ocp_qcqp_ipm_get_max_res_ineq(struct d_tree_ocp_qcqp_ipm_ws *ws, double *res_ineq); +// +void d_tree_ocp_qcqp_ipm_get_max_res_comp(struct d_tree_ocp_qcqp_ipm_ws *ws, double *res_comp); +// +void d_tree_ocp_qcqp_ipm_get_stat(struct d_tree_ocp_qcqp_ipm_ws *ws, double **stat); +// +void d_tree_ocp_qcqp_ipm_get_stat_m(struct d_tree_ocp_qcqp_ipm_ws *ws, int *stat_m); +// +void d_tree_ocp_qcqp_init_var(struct d_tree_ocp_qcqp *qp, struct d_tree_ocp_qcqp_sol *qp_sol, struct d_tree_ocp_qcqp_ipm_arg *arg, struct d_tree_ocp_qcqp_ipm_ws *ws); +// +void d_tree_ocp_qcqp_ipm_solve(struct d_tree_ocp_qcqp *qp, struct d_tree_ocp_qcqp_sol *qp_sol, struct d_tree_ocp_qcqp_ipm_arg *arg, struct d_tree_ocp_qcqp_ipm_ws *ws); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + +#endif // HPIPM_D_TREE_OCP_QCQP_IPM_H_ + + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_res.h b/phonelibs/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_res.h new file mode 100644 index 0000000000..69eebb1ba4 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_res.h @@ -0,0 +1,108 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_TREE_OCP_QCQP_RES_H_ +#define HPIPM_D_TREE_OCP_QCQP_RES_H_ + + + +#include +#include + +#include +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_tree_ocp_qcqp_res + { + struct d_tree_ocp_qcqp_dim *dim; + struct blasfeo_dvec *res_g; // q-residuals + struct blasfeo_dvec *res_b; // b-residuals + struct blasfeo_dvec *res_d; // d-residuals + struct blasfeo_dvec *res_m; // m-residuals + double res_max[4]; // max of residuals + double res_mu; // mu-residual + hpipm_size_t memsize; + }; + + + +struct d_tree_ocp_qcqp_res_ws + { + struct blasfeo_dvec *tmp_nuxM; // work space of size nuM+nxM + struct blasfeo_dvec *tmp_nbgqM; // work space of size nbM+ngM+nqM + struct blasfeo_dvec *tmp_nsM; // work space of size nsM + struct blasfeo_dvec *q_fun; // value for evaluation of quadr constr + struct blasfeo_dvec *q_adj; // value for adjoint of quadr constr + int use_q_fun; // reuse cached value for evaluation of quadr constr + int use_q_adj; // reuse cached value for adjoint of quadr constr + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_tree_ocp_qcqp_res_memsize(struct d_tree_ocp_qcqp_dim *ocp_dim); +// +void d_tree_ocp_qcqp_res_create(struct d_tree_ocp_qcqp_dim *ocp_dim, struct d_tree_ocp_qcqp_res *res, void *mem); +// +hpipm_size_t d_tree_ocp_qcqp_res_ws_memsize(struct d_tree_ocp_qcqp_dim *ocp_dim); +// +void d_tree_ocp_qcqp_res_ws_create(struct d_tree_ocp_qcqp_dim *ocp_dim, struct d_tree_ocp_qcqp_res_ws *ws, void *mem); +// +void d_tree_ocp_qcqp_res_compute(struct d_tree_ocp_qcqp *qp, struct d_tree_ocp_qcqp_sol *qp_sol, struct d_tree_ocp_qcqp_res *res, struct d_tree_ocp_qcqp_res_ws *ws); +// +void d_tree_ocp_qcqp_res_compute_inf_norm(struct d_tree_ocp_qcqp_res *res); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + +#endif // HPIPM_D_TREE_OCP_QCQP_RES_H_ + + + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_sol.h b/phonelibs/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_sol.h new file mode 100644 index 0000000000..cde8de5c51 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_sol.h @@ -0,0 +1,99 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_TREE_OCP_QCQP_SOL_H_ +#define HPIPM_D_TREE_OCP_QCQP_SOL_H_ + + + +#include +#include + +#include "hpipm_d_tree_ocp_qcqp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + +struct d_tree_ocp_qcqp_sol + { + struct d_tree_ocp_qcqp_dim *dim; + struct blasfeo_dvec *ux; + struct blasfeo_dvec *pi; + struct blasfeo_dvec *lam; + struct blasfeo_dvec *t; + void *misc; + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +hpipm_size_t d_tree_ocp_qcqp_sol_memsize(struct d_tree_ocp_qcqp_dim *dim); +// +void d_tree_ocp_qcqp_sol_create(struct d_tree_ocp_qcqp_dim *dim, struct d_tree_ocp_qcqp_sol *qp_sol, void *memory); +// +void d_tree_ocp_qcqp_sol_get(char *field, int node_edge, struct d_tree_ocp_qcqp_sol *qp_sol, double *vec); +// +void d_tree_ocp_qcqp_sol_get_u(int node, struct d_tree_ocp_qcqp_sol *qp_sol, double *vec); +// +void d_tree_ocp_qcqp_sol_get_x(int node, struct d_tree_ocp_qcqp_sol *qp_sol, double *vec); +// +void d_tree_ocp_qcqp_sol_get_sl(int node, struct d_tree_ocp_qcqp_sol *qp_sol, double *vec); +// +void d_tree_ocp_qcqp_sol_get_su(int node, struct d_tree_ocp_qcqp_sol *qp_sol, double *vec); +// +void d_tree_ocp_qcqp_sol_get_pi(int edge, struct d_tree_ocp_qcqp_sol *qp_sol, double *vec); +// +void d_tree_ocp_qcqp_sol_get_lam_lb(int node, struct d_tree_ocp_qcqp_sol *qp_sol, double *vec); +// +void d_tree_ocp_qcqp_sol_get_lam_ub(int node, struct d_tree_ocp_qcqp_sol *qp_sol, double *vec); +// +void d_tree_ocp_qcqp_sol_get_lam_lg(int node, struct d_tree_ocp_qcqp_sol *qp_sol, double *vec); +// +void d_tree_ocp_qcqp_sol_get_lam_ug(int node, struct d_tree_ocp_qcqp_sol *qp_sol, double *vec); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_D_TREE_OCP_QCQP_SOL_H_ + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_utils.h b/phonelibs/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_utils.h new file mode 100644 index 0000000000..5ef04fcf66 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_utils.h @@ -0,0 +1,84 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_TREE_OCP_QP_UTILS_H_ +#define HPIPM_D_TREE_OCP_QP_UTILS_H_ + + + +#include +#include + +#include "hpipm_d_tree_ocp_qcqp_dim.h" +#include "hpipm_d_tree_ocp_qcqp.h" +#include "hpipm_d_tree_ocp_qcqp_sol.h" +#include "hpipm_d_tree_ocp_qcqp_ipm.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +// +void d_tree_ocp_qcqp_dim_print(struct d_tree_ocp_qcqp_dim *qp_dim); +// +//void d_tree_ocp_qcqp_dim_codegen(char *file_name, char *mode, struct d_tree_ocp_qcqp_dim *qp_dim); +// +void d_tree_ocp_qcqp_print(struct d_tree_ocp_qcqp_dim *qp_dim, struct d_tree_ocp_qcqp *qp); +// +//void d_tree_ocp_qcqp_codegen(char *file_name, char *mode, struct d_tree_ocp_qcqp_dim *qp_dim, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_sol_print(struct d_tree_ocp_qcqp_dim *qp_dim, struct d_tree_ocp_qcqp_sol *ocp_qcqp_sol); +// +void d_tree_ocp_qcqp_ipm_arg_print(struct d_tree_ocp_qcqp_dim *qp_dim, struct d_tree_ocp_qcqp_ipm_arg *arg); +// +//void d_tree_ocp_qcqp_ipm_arg_codegen(char *file_name, char *mode, struct d_tree_ocp_qcqp_dim *qp_dim, struct d_tree_ocp_qcqp_ipm_arg *arg); +// +void d_tree_ocp_qcqp_res_print(struct d_tree_ocp_qcqp_dim *qp_dim, struct d_tree_ocp_qcqp_res *ocp_qcqp_res); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_D_TREE_OCP_QP_UTILS_H_ + + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_tree_ocp_qp.h b/phonelibs/acados/include/hpipm/include/hpipm_d_tree_ocp_qp.h new file mode 100644 index 0000000000..1666ad7c71 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_tree_ocp_qp.h @@ -0,0 +1,195 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_TREE_OCP_QP_H_ +#define HPIPM_D_TREE_OCP_QP_H_ + + + +#include +#include + +#include "hpipm_d_tree_ocp_qp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_tree_ocp_qp + { + struct d_tree_ocp_qp_dim *dim; + struct blasfeo_dmat *BAbt; // Nn-1 + struct blasfeo_dmat *RSQrq; // Nn + struct blasfeo_dmat *DCt; // Nn + struct blasfeo_dvec *b; // Nn-1 + struct blasfeo_dvec *rqz; // Nn + struct blasfeo_dvec *d; // Nn + struct blasfeo_dvec *d_mask; // Nn + struct blasfeo_dvec *m; // Nn + struct blasfeo_dvec *Z; // Nn + int **idxb; // indices of box constrained variables within [u; x] // Nn +// int **idxs; // index of soft constraints + int **idxs_rev; // index of soft constraints + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +hpipm_size_t d_tree_ocp_qp_memsize(struct d_tree_ocp_qp_dim *dim); +// +void d_tree_ocp_qp_create(struct d_tree_ocp_qp_dim *dim, struct d_tree_ocp_qp *qp, void *memory); +// +void d_tree_ocp_qp_set_all(double **A, double **B, double **b, double **Q, double **S, double **R, double **q, double **r, int **idxb, double **d_lb, double **d_ub, double **C, double **D, double **d_lg, double **d_ug, double **Zl, double **Zu, double **zl, double **zu, int **idxs, double **d_ls, double **d_us, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set(char *field_name, int node_edge, void *value, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_A(int edge, double *mat, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_B(int edge, double *mat, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_b(int edge, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_Q(int node, double *mat, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_S(int node, double *mat, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_R(int node, double *mat, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_q(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_r(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_lb(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_lb_mask(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_ub(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_ub_mask(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_lbx(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_lbx_mask(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_ubx(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_ubx_mask(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_lbu(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_lbu_mask(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_ubu(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_ubu_mask(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_idxb(int node, int *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_idxbx(int node, int *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_Jbx(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_idxbu(int node, int *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_Jbu(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_C(int node, double *mat, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_D(int node, double *mat, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_lg(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_lg_mask(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_ug(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_ug_mask(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_Zl(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_Zu(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_zl(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_zu(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_lls(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_lls_mask(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_lus(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_lus_mask(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_idxs(int node, int *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_idxs_rev(int node, int *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_Jsbu(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_Jsbx(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_Jsg(int node, double *vec, struct d_tree_ocp_qp *qp); +// +//void d_tree_ocp_qp_set_idxe(int node, int *vec, struct d_tree_ocp_qp *qp); +// +//void d_tree_ocp_qp_set_idxbxe(int node, int *vec, struct d_tree_ocp_qp *qp); +// +//void d_tree_ocp_qp_set_idxbue(int node, int *vec, struct d_tree_ocp_qp *qp); +// +//void d_tree_ocp_qp_set_idxge(int node, int *vec, struct d_tree_ocp_qp *qp); +// +//void d_tree_ocp_qp_set_Jbxe(int node, double *vec, struct d_tree_ocp_qp *qp); +// +//void d_tree_ocp_qp_set_Jbue(int node, double *vec, struct d_tree_ocp_qp *qp); +// +//void d_tree_ocp_qp_set_Jge(int node, double *vec, struct d_tree_ocp_qp *qp); +// +//void d_tree_ocp_qp_set_diag_H_flag(int node, int *value, struct d_tree_ocp_qp *qp); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_D_TREE_OCP_QP_H_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_dim.h b/phonelibs/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_dim.h new file mode 100644 index 0000000000..659b664594 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_dim.h @@ -0,0 +1,111 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_TREE_OCP_QP_DIM_H_ +#define HPIPM_D_TREE_OCP_QP_DIM_H_ + +#include "hpipm_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_tree_ocp_qp_dim + { + struct tree *ttree; // tree describing node conndection + int *nx; // number of states // Nn + int *nu; // number of inputs // Nn + int *nb; // number of box constraints // Nn + int *nbx; // number of state box constraints // Nn + int *nbu; // number of input box constraints // Nn + int *ng; // number of general constraints // Nn + int *ns; // number of soft constraints // Nn + int *nsbx; // number of soft state box constraints + int *nsbu; // number of soft input box constraints + int *nsg; // number of soft general constraints + int Nn; // number of nodes + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_tree_ocp_qp_dim_strsize(); +// +hpipm_size_t d_tree_ocp_qp_dim_memsize(int Nn); +// +void d_tree_ocp_qp_dim_create(int Nn, struct d_tree_ocp_qp_dim *qp_dim, void *memory); +// +void d_tree_ocp_qp_dim_set_all(struct tree *ttree, int *nx, int *nu, int *nbx, int *nbu, int *ng, int *nsbx, int *nsbu, int *nsg, struct d_tree_ocp_qp_dim *dim); +// +void d_tree_ocp_qp_dim_set_tree(struct tree *ttree, struct d_tree_ocp_qp_dim *dim); +// +void d_tree_ocp_qp_dim_set(char *field, int stage, int value, struct d_tree_ocp_qp_dim *dim); +// +void d_tree_ocp_qp_dim_set_nx(int stage, int value, struct d_tree_ocp_qp_dim *dim); +// +void d_tree_ocp_qp_dim_set_nu(int stage, int value, struct d_tree_ocp_qp_dim *dim); +// +void d_tree_ocp_qp_dim_set_nbx(int stage, int value, struct d_tree_ocp_qp_dim *dim); +// +void d_tree_ocp_qp_dim_set_nbu(int stage, int value, struct d_tree_ocp_qp_dim *dim); +// +void d_tree_ocp_qp_dim_set_ng(int stage, int value, struct d_tree_ocp_qp_dim *dim); +// +void d_tree_ocp_qp_dim_set_ns(int stage, int value, struct d_tree_ocp_qp_dim *dim); +// +void d_tree_ocp_qp_dim_set_nsbx(int stage, int value, struct d_tree_ocp_qp_dim *dim); +// +void d_tree_ocp_qp_dim_set_nsbu(int stage, int value, struct d_tree_ocp_qp_dim *dim); +// +void d_tree_ocp_qp_dim_set_nsg(int stage, int value, struct d_tree_ocp_qp_dim *dim); +// +void d_tree_ocp_qp_dim_set_nbxe(int stage, int value, struct d_tree_ocp_qp_dim *dim); +// +void d_tree_ocp_qp_dim_set_nbue(int stage, int value, struct d_tree_ocp_qp_dim *dim); +// +void d_tree_ocp_qp_dim_set_nge(int stage, int value, struct d_tree_ocp_qp_dim *dim); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_D_TREE_OCP_QP_DIM_H_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_ipm.h b/phonelibs/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_ipm.h new file mode 100644 index 0000000000..fb634208ce --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_ipm.h @@ -0,0 +1,209 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_D_TREE_OCP_QP_IPM_H_ +#define HPIPM_D_TREE_OCP_QP_IPM_H_ + + + +#include +#include + +#include +#include +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_tree_ocp_qp_ipm_arg + { + double mu0; // initial value for duality measure + double alpha_min; // exit cond on step length + double res_g_max; // exit cond on inf norm of residuals + double res_b_max; // exit cond on inf norm of residuals + double res_d_max; // exit cond on inf norm of residuals + double res_m_max; // exit cond on inf norm of residuals + double reg_prim; // reg of primal hessian + double lam_min; // min value in lam vector + double t_min; // min value in t vector + double tau_min; // min value of barrier parameter + int iter_max; // exit cond in iter number + int stat_max; // iterations saved in stat + int pred_corr; // use Mehrotra's predictor-corrector IPM algirthm + int cond_pred_corr; // conditional Mehrotra's predictor-corrector + int itref_pred_max; // max number of iterative refinement steps for predictor step + int itref_corr_max; // max number of iterative refinement steps for corrector step + int warm_start; // 0 no warm start, 1 warm start primal sol, 2 warm start primal and dual sol + int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq + int abs_form; // absolute IPM formulation + int comp_dual_sol_eq; // dual solution (only for abs_form==1) + int comp_res_exit; // compute residuals on exit (only for abs_form==1 and comp_dual_sol==1) + int split_step; // use different steps for primal and dual variables + int t_lam_min; // clip t and lam: 0 no, 1 in Gamma computation, 2 in solution + int mode; + hpipm_size_t memsize; + }; + + + +struct d_tree_ocp_qp_ipm_ws + { + struct d_core_qp_ipm_workspace *core_workspace; + struct d_tree_ocp_qp_res_ws *res_workspace; + struct d_tree_ocp_qp_sol *sol_step; + struct d_tree_ocp_qp_sol *sol_itref; + struct d_tree_ocp_qp *qp_step; + struct d_tree_ocp_qp *qp_itref; + struct d_tree_ocp_qp_res *res_itref; + struct d_tree_ocp_qp_res *res; + struct blasfeo_dvec *Gamma; // hessian update + struct blasfeo_dvec *gamma; // hessian update + struct blasfeo_dvec *tmp_nxM; // work space of size nxM + struct blasfeo_dvec *tmp_nbgM; // work space of size nbgM + struct blasfeo_dvec *tmp_nsM; // work space of size nsM + struct blasfeo_dvec *Pb; // Pb + struct blasfeo_dvec *Zs_inv; + struct blasfeo_dmat *L; + struct blasfeo_dmat *Lh; + struct blasfeo_dmat *AL; + struct blasfeo_dmat *lq0; + struct blasfeo_dvec *tmp_m; + double *stat; // convergence statistics + int *use_hess_fact; + void *lq_work0; + double qp_res[4]; // infinity norm of residuals + int iter; // iteration number + int stat_max; // iterations saved in stat + int stat_m; // number of recorded stat per IPM iter + int use_Pb; + int status; // solver status + int lq_fact; // cache from arg + int mask_constr; // use constr mask + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_tree_ocp_qp_ipm_arg_memsize(struct d_tree_ocp_qp_dim *dim); +// +void d_tree_ocp_qp_ipm_arg_create(struct d_tree_ocp_qp_dim *dim, struct d_tree_ocp_qp_ipm_arg *arg, void *mem); +// +void d_tree_ocp_qp_ipm_arg_set_default(enum hpipm_mode mode, struct d_tree_ocp_qp_ipm_arg *arg); +// +void d_tree_ocp_qp_ipm_arg_set_iter_max(int *iter_max, struct d_tree_ocp_qp_ipm_arg *arg); +// +void d_tree_ocp_qp_ipm_arg_set_alpha_min(double *alpha_min, struct d_tree_ocp_qp_ipm_arg *arg); +// +void d_tree_ocp_qp_ipm_arg_set_mu0(double *mu0, struct d_tree_ocp_qp_ipm_arg *arg); +// +void d_tree_ocp_qp_ipm_arg_set_tol_stat(double *tol_stat, struct d_tree_ocp_qp_ipm_arg *arg); +// +void d_tree_ocp_qp_ipm_arg_set_tol_eq(double *tol_eq, struct d_tree_ocp_qp_ipm_arg *arg); +// +void d_tree_ocp_qp_ipm_arg_set_tol_ineq(double *tol_ineq, struct d_tree_ocp_qp_ipm_arg *arg); +// +void d_tree_ocp_qp_ipm_arg_set_tol_comp(double *tol_comp, struct d_tree_ocp_qp_ipm_arg *arg); +// +void d_tree_ocp_qp_ipm_arg_set_reg_prim(double *reg, struct d_tree_ocp_qp_ipm_arg *arg); +// +void d_tree_ocp_qp_ipm_arg_set_warm_start(int *warm_start, struct d_tree_ocp_qp_ipm_arg *arg); +// +void d_tree_ocp_qp_ipm_arg_set_pred_corr(int *pred_corr, struct d_tree_ocp_qp_ipm_arg *arg); +// +void d_tree_ocp_qp_ipm_arg_set_cond_pred_corr(int *value, struct d_tree_ocp_qp_ipm_arg *arg); +// +void d_tree_ocp_qp_ipm_arg_set_comp_dual_sol_eq(int *value, struct d_tree_ocp_qp_ipm_arg *arg); +// +void d_tree_ocp_qp_ipm_arg_set_comp_res_exit(int *value, struct d_tree_ocp_qp_ipm_arg *arg); +// +void d_tree_ocp_qp_ipm_arg_set_lam_min(double *value, struct d_tree_ocp_qp_ipm_arg *arg); +// +void d_tree_ocp_qp_ipm_arg_set_t_min(double *value, struct d_tree_ocp_qp_ipm_arg *arg); +// +void d_tree_ocp_qp_ipm_arg_set_tau_min(double *value, struct d_tree_ocp_qp_ipm_arg *arg); +// +void d_tree_ocp_qp_ipm_arg_set_split_step(int *value, struct d_tree_ocp_qp_ipm_arg *arg); +// +void d_tree_ocp_qp_ipm_arg_set_t_lam_min(int *value, struct d_tree_ocp_qp_ipm_arg *arg); + +// +hpipm_size_t d_tree_ocp_qp_ipm_ws_memsize(struct d_tree_ocp_qp_dim *dim, struct d_tree_ocp_qp_ipm_arg *arg); +// +void d_tree_ocp_qp_ipm_ws_create(struct d_tree_ocp_qp_dim *dim, struct d_tree_ocp_qp_ipm_arg *arg, struct d_tree_ocp_qp_ipm_ws *ws, void *mem); +// +void d_tree_ocp_qp_ipm_get_status(struct d_tree_ocp_qp_ipm_ws *ws, int *status); +// +void d_tree_ocp_qp_ipm_get_iter(struct d_tree_ocp_qp_ipm_ws *ws, int *iter); +// +void d_tree_ocp_qp_ipm_get_max_res_stat(struct d_tree_ocp_qp_ipm_ws *ws, double *res_stat); +// +void d_tree_ocp_qp_ipm_get_max_res_eq(struct d_tree_ocp_qp_ipm_ws *ws, double *res_eq); +// +void d_tree_ocp_qp_ipm_get_max_res_ineq(struct d_tree_ocp_qp_ipm_ws *ws, double *res_ineq); +// +void d_tree_ocp_qp_ipm_get_max_res_comp(struct d_tree_ocp_qp_ipm_ws *ws, double *res_comp); +// +void d_tree_ocp_qp_ipm_get_stat(struct d_tree_ocp_qp_ipm_ws *ws, double **stat); +// +void d_tree_ocp_qp_ipm_get_stat_m(struct d_tree_ocp_qp_ipm_ws *ws, int *stat_m); +// +void d_tree_ocp_qp_init_var(struct d_tree_ocp_qp *qp, struct d_tree_ocp_qp_sol *qp_sol, struct d_tree_ocp_qp_ipm_arg *arg, struct d_tree_ocp_qp_ipm_ws *ws); +// +void d_tree_ocp_qp_ipm_abs_step(int kk, struct d_tree_ocp_qp *qp, struct d_tree_ocp_qp_sol *qp_sol, struct d_tree_ocp_qp_ipm_arg *arg, struct d_tree_ocp_qp_ipm_ws *ws); +// +void d_tree_ocp_qp_ipm_delta_step(int kk, struct d_tree_ocp_qp *qp, struct d_tree_ocp_qp_sol *qp_sol, struct d_tree_ocp_qp_ipm_arg *arg, struct d_tree_ocp_qp_ipm_ws *ws); +// +void d_tree_ocp_qp_ipm_solve(struct d_tree_ocp_qp *qp, struct d_tree_ocp_qp_sol *qp_sol, struct d_tree_ocp_qp_ipm_arg *arg, struct d_tree_ocp_qp_ipm_ws *ws); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_D_TREE_OCP_QP_IPM_H_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_kkt.h b/phonelibs/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_kkt.h new file mode 100644 index 0000000000..4afd52fe4b --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_kkt.h @@ -0,0 +1,52 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifdef __cplusplus +extern "C" { +#endif + + +// +void d_tree_ocp_qp_fact_solve_kkt_unconstr(struct d_tree_ocp_qp *qp, struct d_tree_ocp_qp_sol *qp_sol, struct d_tree_ocp_qp_ipm_arg *arg, struct d_tree_ocp_qp_ipm_ws *ws); +// +void d_tree_ocp_qp_fact_solve_kkt_step(struct d_tree_ocp_qp *qp, struct d_tree_ocp_qp_sol *qp_sol, struct d_tree_ocp_qp_ipm_arg *arg, struct d_tree_ocp_qp_ipm_ws *ws); +// +void d_tree_ocp_qp_fact_lq_solve_kkt_step(struct d_tree_ocp_qp *qp, struct d_tree_ocp_qp_sol *qp_sol, struct d_tree_ocp_qp_ipm_arg *arg, struct d_tree_ocp_qp_ipm_ws *ws); +// +void d_tree_ocp_qp_solve_kkt_step(struct d_tree_ocp_qp *qp, struct d_tree_ocp_qp_sol *qp_sol, struct d_tree_ocp_qp_ipm_arg *arg, struct d_tree_ocp_qp_ipm_ws *ws); + +#ifdef __cplusplus +} /* extern "C" */ +#endif diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_res.h b/phonelibs/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_res.h new file mode 100644 index 0000000000..fe499080ea --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_res.h @@ -0,0 +1,106 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_TREE_OCP_QP_RES_H_ +#define HPIPM_D_TREE_OCP_QP_RES_H_ + + + +#include +#include + +#include +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_tree_ocp_qp_res + { + struct d_tree_ocp_qp_dim *dim; + struct blasfeo_dvec *res_g; // q-residuals + struct blasfeo_dvec *res_b; // b-residuals + struct blasfeo_dvec *res_d; // d-residuals + struct blasfeo_dvec *res_m; // m-residuals + double res_max[4]; // max of residuals + double res_mu; // mu-residual + hpipm_size_t memsize; + }; + + + +struct d_tree_ocp_qp_res_ws + { + struct blasfeo_dvec *tmp_nbgM; // work space of size nbM+ngM + struct blasfeo_dvec *tmp_nsM; // work space of size nsM + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_tree_ocp_qp_res_memsize(struct d_tree_ocp_qp_dim *ocp_dim); +// +void d_tree_ocp_qp_res_create(struct d_tree_ocp_qp_dim *ocp_dim, struct d_tree_ocp_qp_res *res, void *mem); +// +hpipm_size_t d_tree_ocp_qp_res_ws_memsize(struct d_tree_ocp_qp_dim *ocp_dim); +// +void d_tree_ocp_qp_res_ws_create(struct d_tree_ocp_qp_dim *ocp_dim, struct d_tree_ocp_qp_res_ws *ws, void *mem); +// +void d_tree_ocp_qp_res_get_all(struct d_tree_ocp_qp_res *res, double **res_r, double **res_q, double **res_ls, double **res_us, double **res_b, double **res_d_lb, double **res_d_ub, double **res_d_lg, double **res_d_ug, double **res_d_ls, double **res_d_us, double **res_m_lb, double **res_m_ub, double **res_m_lg, double **res_m_ug, double **res_m_ls, double **res_m_us); +// +void d_tree_ocp_qp_res_compute(struct d_tree_ocp_qp *qp, struct d_tree_ocp_qp_sol *qp_sol, struct d_tree_ocp_qp_res *res, struct d_tree_ocp_qp_res_ws *ws); +// +void d_tree_ocp_qp_res_compute_lin(struct d_tree_ocp_qp *qp, struct d_tree_ocp_qp_sol *qp_sol, struct d_tree_ocp_qp_sol *qp_step, struct d_tree_ocp_qp_res *res, struct d_tree_ocp_qp_res_ws *ws); +// +void d_tree_ocp_qp_res_compute_inf_norm(struct d_tree_ocp_qp_res *res); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + +#endif // HPIPM_D_TREE_OCP_QP_RES_H_ + + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_sol.h b/phonelibs/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_sol.h new file mode 100644 index 0000000000..343d5e8ca6 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_sol.h @@ -0,0 +1,100 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_TREE_OCP_QP_SOL_H_ +#define HPIPM_D_TREE_OCP_QP_SOL_H_ + + + +#include +#include + +#include "hpipm_d_tree_ocp_qp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + +struct d_tree_ocp_qp_sol + { + struct d_tree_ocp_qp_dim *dim; + struct blasfeo_dvec *ux; + struct blasfeo_dvec *pi; + struct blasfeo_dvec *lam; + struct blasfeo_dvec *t; + void *misc; + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +hpipm_size_t d_tree_ocp_qp_sol_memsize(struct d_tree_ocp_qp_dim *dim); +// +void d_tree_ocp_qp_sol_create(struct d_tree_ocp_qp_dim *dim, struct d_tree_ocp_qp_sol *qp_sol, void *memory); +// +void d_tree_ocp_qp_sol_get_all(struct d_tree_ocp_qp *qp, struct d_tree_ocp_qp_sol *qp_sol, double **u, double **x, double **ls, double **us, double **pi, double **lam_lb, double **lam_ub, double **lam_lg, double **lam_ug, double **lam_ls, double **lam_us); +// +void d_tree_ocp_qp_sol_get(char *field, int node_edge, struct d_tree_ocp_qp_sol *qp_sol, double *vec); +// +void d_tree_ocp_qp_sol_get_u(int node, struct d_tree_ocp_qp_sol *qp_sol, double *vec); +// +void d_tree_ocp_qp_sol_get_x(int node, struct d_tree_ocp_qp_sol *qp_sol, double *vec); +// +void d_tree_ocp_qp_sol_get_sl(int node, struct d_tree_ocp_qp_sol *qp_sol, double *vec); +// +void d_tree_ocp_qp_sol_get_su(int node, struct d_tree_ocp_qp_sol *qp_sol, double *vec); +// +void d_tree_ocp_qp_sol_get_pi(int edge, struct d_tree_ocp_qp_sol *qp_sol, double *vec); +// +void d_tree_ocp_qp_sol_get_lam_lb(int node, struct d_tree_ocp_qp_sol *qp_sol, double *vec); +// +void d_tree_ocp_qp_sol_get_lam_ub(int node, struct d_tree_ocp_qp_sol *qp_sol, double *vec); +// +void d_tree_ocp_qp_sol_get_lam_lg(int node, struct d_tree_ocp_qp_sol *qp_sol, double *vec); +// +void d_tree_ocp_qp_sol_get_lam_ug(int node, struct d_tree_ocp_qp_sol *qp_sol, double *vec); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_D_TREE_OCP_QP_SOL_H_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_utils.h b/phonelibs/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_utils.h new file mode 100644 index 0000000000..b689fdc0fa --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_utils.h @@ -0,0 +1,83 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_TREE_OCP_QP_UTILS_H_ +#define HPIPM_D_TREE_OCP_QP_UTILS_H_ + + + +#include +#include + +#include "hpipm_d_tree_ocp_qp_dim.h" +#include "hpipm_d_tree_ocp_qp.h" +#include "hpipm_d_tree_ocp_qp_sol.h" +#include "hpipm_d_tree_ocp_qp_ipm.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +// +void d_tree_ocp_qp_dim_print(struct d_tree_ocp_qp_dim *qp_dim); +// +//void d_tree_ocp_qp_dim_codegen(char *file_name, char *mode, struct d_tree_ocp_qp_dim *qp_dim); +// +void d_tree_ocp_qp_print(struct d_tree_ocp_qp_dim *qp_dim, struct d_tree_ocp_qp *qp); +// +//void d_tree_ocp_qp_codegen(char *file_name, char *mode, struct d_tree_ocp_qp_dim *qp_dim, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_sol_print(struct d_tree_ocp_qp_dim *qp_dim, struct d_tree_ocp_qp_sol *ocp_qp_sol); +// +void d_tree_ocp_qp_ipm_arg_print(struct d_tree_ocp_qp_dim *qp_dim, struct d_tree_ocp_qp_ipm_arg *arg); +// +//void d_tree_ocp_qp_ipm_arg_codegen(char *file_name, char *mode, struct d_tree_ocp_qp_dim *qp_dim, struct d_tree_ocp_qp_ipm_arg *arg); +// +void d_tree_ocp_qp_res_print(struct d_tree_ocp_qp_dim *qp_dim, struct d_tree_ocp_qp_res *ocp_qp_res); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_D_TREE_OCP_QP_UTILS_H_ + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_m_dense_qp.h b/phonelibs/acados/include/hpipm/include/hpipm_m_dense_qp.h new file mode 100644 index 0000000000..8bc101003e --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_m_dense_qp.h @@ -0,0 +1,68 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + + +#ifndef HPIPM_M_DENSE_QP_H_ +#define HPIPM_M_DENSE_QP_H_ + + + +#include +#include + +#include "hpipm_d_dense_qp.h" +#include "hpipm_s_dense_qp.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +void cvt_d2s_dense_qp(struct d_dense_qp *qpd, struct s_dense_qp *qps); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_M_DENSE_QP_H_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_m_dense_qp_dim.h b/phonelibs/acados/include/hpipm/include/hpipm_m_dense_qp_dim.h new file mode 100644 index 0000000000..4610321b97 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_m_dense_qp_dim.h @@ -0,0 +1,68 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + + +#ifndef HPIPM_M_DENSE_QP_DIM_H_ +#define HPIPM_M_DENSE_QP_DIM_H_ + + + +#include +#include + +#include "hpipm_d_dense_qp_dim.h" +#include "hpipm_s_dense_qp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +void cvt_d2s_dense_qp_dim(struct d_dense_qp_dim *qpd, struct s_dense_qp_dim *qps); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_M_DENSE_QP_DIM_H_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_m_ocp_qp.h b/phonelibs/acados/include/hpipm/include/hpipm_m_ocp_qp.h new file mode 100644 index 0000000000..95c3dad1aa --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_m_ocp_qp.h @@ -0,0 +1,49 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +void m_cvt_d_ocp_qp_to_s_ocp_qp(struct d_ocp_qp *d_qp, struct s_ocp_qp *s_qp); + +#ifdef __cplusplus +} /* extern "C" */ +#endif diff --git a/phonelibs/acados/include/hpipm/include/hpipm_m_ocp_qp_ipm_hard.h b/phonelibs/acados/include/hpipm/include/hpipm_m_ocp_qp_ipm_hard.h new file mode 100644 index 0000000000..1c44acee2e --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_m_ocp_qp_ipm_hard.h @@ -0,0 +1,115 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + + +struct m_ipm_hard_ocp_qp_workspace + { + struct d_ipm_hard_core_qp_workspace *core_workspace; + struct blasfeo_dvec *dux; + struct blasfeo_dvec *dpi; + struct blasfeo_dvec *dt_lb; + struct blasfeo_dvec *dt_lg; + struct blasfeo_svec *sdux; // XXX + struct blasfeo_svec *sdpi; // XXX + struct blasfeo_dvec *res_g; // q-residuals + struct blasfeo_dvec *res_b; // b-residuals + struct blasfeo_dvec *res_d; // d-residuals XXX remove ??? + struct blasfeo_dvec *res_d_lb; // d-residuals + struct blasfeo_dvec *res_d_ub; // d-residuals + struct blasfeo_dvec *res_d_lg; // d-residuals + struct blasfeo_dvec *res_d_ug; // d-residuals + struct blasfeo_dvec *res_m; // m-residuals + struct blasfeo_dvec *res_m_lb; // m-residuals + struct blasfeo_dvec *res_m_ub; // m-residuals + struct blasfeo_dvec *res_m_lg; // m-residuals + struct blasfeo_dvec *res_m_ug; // m-residuals + struct blasfeo_svec *sres_g; // q-residuals // XXX + struct blasfeo_svec *sres_b; // b-residuals // XXX + struct blasfeo_dvec *Qx_lb; // hessian update + struct blasfeo_dvec *Qx_lg; // hessian update + struct blasfeo_dvec *qx_lb; // gradient update + struct blasfeo_dvec *qx_lg; // gradient update + struct blasfeo_svec *sQx_lb; // hessian update // XXX + struct blasfeo_svec *sQx_lg; // hessian update // XXX + struct blasfeo_svec *sqx_lb; // gradient update // XXX + struct blasfeo_svec *sqx_lg; // gradient update // XXX + struct blasfeo_dvec *tmp_nbM; // work space of size nbM + struct blasfeo_svec *tmp_nxM; // work space of size nxM // XXX + struct blasfeo_dvec *tmp_ngM; // work space of size ngM + struct blasfeo_svec *Pb; // Pb // XXX + struct blasfeo_smat *L; // XXX + struct blasfeo_smat *AL; // XXX + struct blasfeo_svec *sSx; // scaling + struct blasfeo_svec *sSi; // scaling inverted + double *stat; // convergence statistics + double res_mu; // mu-residual + int iter; // iteration number + int compute_Pb; + int scale; + }; + + + +struct m_ipm_hard_ocp_qp_arg + { + double alpha_min; // exit cond on step length + double mu_max; // exit cond on duality measure + double mu0; // initial value for duality measure + int iter_max; // exit cond in iter number + }; + + + +// +hpipm_size_t m_memsize_ipm_hard_ocp_qp(struct d_ocp_qp *d_qp, struct s_ocp_qp *s_qp, struct m_ipm_hard_ocp_qp_arg *arg); +// +void m_create_ipm_hard_ocp_qp(struct d_ocp_qp *d_qp, struct s_ocp_qp *s_qp, struct m_ipm_hard_ocp_qp_arg *arg, struct m_ipm_hard_ocp_qp_workspace *ws, void *mem); +// +void m_solve_ipm_hard_ocp_qp(struct d_ocp_qp *d_qp, struct s_ocp_qp *s_qp, struct d_ocp_qp_sol *qp_sol, struct m_ipm_hard_ocp_qp_workspace *ws); +// +void m_solve_ipm2_hard_ocp_qp(struct d_ocp_qp *d_qp, struct s_ocp_qp *s_qp, struct d_ocp_qp_sol *qp_sol, struct m_ipm_hard_ocp_qp_workspace *ws); + +#ifdef __cplusplus +} /* extern "C" */ +#endif diff --git a/phonelibs/acados/include/hpipm/include/hpipm_m_ocp_qp_kkt.h b/phonelibs/acados/include/hpipm/include/hpipm_m_ocp_qp_kkt.h new file mode 100644 index 0000000000..032fe95b1f --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_m_ocp_qp_kkt.h @@ -0,0 +1,45 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifdef __cplusplus +extern "C" { +#endif + +void m_fact_solve_kkt_step_hard_ocp_qp(struct d_ocp_qp *d_qp, struct s_ocp_qp *s_qp, struct m_ipm_hard_ocp_qp_workspace *ws); +void m_solve_kkt_step_hard_ocp_qp(struct d_ocp_qp *d_qp, struct s_ocp_qp *s_qp, struct m_ipm_hard_ocp_qp_workspace *ws); + +#ifdef __cplusplus +} /* extern "C" */ +#endif diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_cast_qcqp.h b/phonelibs/acados/include/hpipm/include/hpipm_s_cast_qcqp.h new file mode 100644 index 0000000000..ba01ecb3bd --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_cast_qcqp.h @@ -0,0 +1,72 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_S_CAST_QCQP_H_ +#define HPIPM_S_CAST_QCQP_H_ + + + +#include +#include + +#include "hpipm_s_dense_qcqp.h" +#include "hpipm_s_dense_qcqp_sol.h" +#include "hpipm_s_ocp_qcqp.h" +#include "hpipm_s_ocp_qcqp_dim.h" +#include "hpipm_s_ocp_qcqp_sol.h" + +#ifdef __cplusplus +extern "C" { +#endif + + + +// +void s_cast_qcqp_compute_dim(struct s_ocp_qcqp_dim *ocp_dim, struct s_dense_qcqp_dim *dense_dim); +// +void s_cast_qcqp_cond(struct s_ocp_qcqp *ocp_qp, struct s_dense_qcqp *dense_qp); + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_S_CAST_QCQP_H_ + + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_cond.h b/phonelibs/acados/include/hpipm/include/hpipm_s_cond.h new file mode 100644 index 0000000000..30116798b7 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_cond.h @@ -0,0 +1,137 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_S_COND_H_ +#define HPIPM_S_COND_H_ + + + +#include +#include + +#include "hpipm_s_dense_qp.h" +#include "hpipm_s_dense_qp_sol.h" +#include "hpipm_s_ocp_qp.h" +#include "hpipm_s_ocp_qp_dim.h" +#include "hpipm_s_ocp_qp_sol.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_cond_qp_arg + { + int cond_last_stage; // condense last stage + int cond_alg; // condensing algorithm: 0 N2-nx3, 1 N3-nx2 + int comp_prim_sol; // primal solution (v) + int comp_dual_sol_eq; // dual solution equality constr (pi) + int comp_dual_sol_ineq; // dual solution inequality constr (lam t) + int square_root_alg; // square root algorithm (faster but requires RSQ>0) + hpipm_size_t memsize; + }; + + + +struct s_cond_qp_ws + { + struct blasfeo_smat *Gamma; + struct blasfeo_smat *GammaQ; + struct blasfeo_smat *L; + struct blasfeo_smat *Lx; + struct blasfeo_smat *AL; + struct blasfeo_svec *Gammab; + struct blasfeo_svec *l; + struct blasfeo_svec *tmp_nbgM; + struct blasfeo_svec *tmp_nuxM; + int bs; // block size + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_cond_qp_arg_memsize(); +// +void s_cond_qp_arg_create(struct s_cond_qp_arg *cond_arg, void *mem); +// +void s_cond_qp_arg_set_default(struct s_cond_qp_arg *cond_arg); +// condensing algorithm: 0 N2-nx3, 1 N3-nx2 +void s_cond_qp_arg_set_cond_alg(int cond_alg, struct s_cond_qp_arg *cond_arg); +// set riccati-like algorithm: 0 classical, 1 square-root +void s_cond_qp_arg_set_ric_alg(int ric_alg, struct s_cond_qp_arg *cond_arg); +// condense last stage: 0 last stage disregarded, 1 last stage condensed too +void s_cond_qp_arg_set_cond_last_stage(int cond_last_stage, struct s_cond_qp_arg *cond_arg); +// +void s_cond_qp_arg_set_comp_prim_sol(int value, struct s_cond_qp_arg *cond_arg); +// +void s_cond_qp_arg_set_comp_dual_sol_eq(int value, struct s_cond_qp_arg *cond_arg); +// +void s_cond_qp_arg_set_comp_dual_sol_ineq(int value, struct s_cond_qp_arg *cond_arg); + +// +void s_cond_qp_compute_dim(struct s_ocp_qp_dim *ocp_dim, struct s_dense_qp_dim *dense_dim); +// +hpipm_size_t s_cond_qp_ws_memsize(struct s_ocp_qp_dim *ocp_dim, struct s_cond_qp_arg *cond_arg); +// +void s_cond_qp_ws_create(struct s_ocp_qp_dim *ocp_dim, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws, void *mem); +// +void s_cond_qp_cond(struct s_ocp_qp *ocp_qp, struct s_dense_qp *dense_qp, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws); +// +void s_cond_qp_cond_lhs(struct s_ocp_qp *ocp_qp, struct s_dense_qp *dense_qp, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws); +// +void s_cond_qp_cond_rhs(struct s_ocp_qp *ocp_qp, struct s_dense_qp *dense_qp, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws); +// +void s_cond_qp_expand_sol(struct s_ocp_qp *ocp_qp, struct s_dense_qp_sol *dense_qp_sol, struct s_ocp_qp_sol *ocp_qp_sol, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws); +// TODO remove +void s_cond_qp_expand_primal_sol(struct s_ocp_qp *ocp_qp, struct s_dense_qp_sol *dense_qp_sol, struct s_ocp_qp_sol *ocp_qp_sol, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws); + +// +void s_cond_qp_update(int *idxc, struct s_ocp_qp *ocp_qp, struct s_dense_qp *dense_qp, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_S_COND_H_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_cond_aux.h b/phonelibs/acados/include/hpipm/include/hpipm_s_cond_aux.h new file mode 100644 index 0000000000..003472ab9f --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_cond_aux.h @@ -0,0 +1,92 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_S_COND_AUX_H_ +#define HPIPM_S_COND_AUX_H_ + + + +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +// +void s_cond_BAbt(struct s_ocp_qp *ocp_qp, struct blasfeo_smat *BAbt2, struct blasfeo_svec *b2, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws); +// +void s_cond_BAt(struct s_ocp_qp *ocp_qp, struct blasfeo_smat *BAbt2, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws); +// +void s_cond_b(struct s_ocp_qp *ocp_qp, struct blasfeo_svec *b2, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws); +// +void s_cond_RSQrq(struct s_ocp_qp *ocp_qp, struct blasfeo_smat *RSQrq2, struct blasfeo_svec *rq2, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws); +// +void s_cond_RSQ(struct s_ocp_qp *ocp_qp, struct blasfeo_smat *RSQrq2, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws); +// +void s_cond_rq(struct s_ocp_qp *ocp_qp, struct blasfeo_svec *rq2, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws); +// +void s_cond_DCtd(struct s_ocp_qp *ocp_qp, int *idxb2, struct blasfeo_smat *DCt2, struct blasfeo_svec *d2, struct blasfeo_svec *d_mask2, int *idxs_rev2, struct blasfeo_svec *Z2, struct blasfeo_svec *z2, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws); +// +void s_cond_DCt(struct s_ocp_qp *ocp_qp, int *idxb2, struct blasfeo_smat *DCt2, int *idxs_rev2, struct blasfeo_svec *Z2, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws); +// +void s_cond_d(struct s_ocp_qp *ocp_qp, struct blasfeo_svec *d2, struct blasfeo_svec *d_mask2, struct blasfeo_svec *z2, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws); +// +void s_expand_sol(struct s_ocp_qp *ocp_qp, struct s_dense_qp_sol *dense_qp_sol, struct s_ocp_qp_sol *ocp_qp_sol, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws); +// +void s_expand_primal_sol(struct s_ocp_qp *ocp_qp, struct s_dense_qp_sol *dense_qp_sol, struct s_ocp_qp_sol *ocp_qp_sol, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws); + +// +void s_update_cond_BAbt(int *idxc, struct s_ocp_qp *ocp_qp, struct blasfeo_smat *BAbt2, struct blasfeo_svec *b2, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws); +// +void s_update_cond_RSQrq_N2nx3(int *idxc, struct s_ocp_qp *ocp_qp, struct blasfeo_smat *RSQrq2, struct blasfeo_svec *rq2, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws); +// +void s_update_cond_DCtd(int *idxc, struct s_ocp_qp *ocp_qp, int *idxb2, struct blasfeo_smat *DCt2, struct blasfeo_svec *d2, int *idxs2, struct blasfeo_svec *Z2, struct blasfeo_svec *z2, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_S_COND_AUX_H_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_cond_qcqp.h b/phonelibs/acados/include/hpipm/include/hpipm_s_cond_qcqp.h new file mode 100644 index 0000000000..c36678abd5 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_cond_qcqp.h @@ -0,0 +1,130 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_S_COND_QCQP_H_ +#define HPIPM_S_COND_QCQP_H_ + + + +#include +#include + +#include "hpipm_s_dense_qcqp.h" +#include "hpipm_s_dense_qcqp_sol.h" +#include "hpipm_s_ocp_qcqp.h" +#include "hpipm_s_ocp_qcqp_dim.h" +#include "hpipm_s_ocp_qcqp_sol.h" + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_cond_qcqp_arg + { + struct s_cond_qp_arg *qp_arg; + int cond_last_stage; // condense last stage +// int cond_variant; // TODO + int comp_prim_sol; // primal solution (v) + int comp_dual_sol_eq; // dual solution equality constr (pi) + int comp_dual_sol_ineq; // dual solution equality constr (lam t) + int square_root_alg; // square root algorithm (faster but requires RSQ>0) + hpipm_size_t memsize; + }; + + + +struct s_cond_qcqp_ws + { + struct s_cond_qp_ws *qp_ws; + struct blasfeo_smat *hess_array; // TODO remove + struct blasfeo_smat *zero_hess; // TODO remove + struct blasfeo_svec *zero_grad; // TODO remove + struct blasfeo_svec *grad_array; // TODO remove + struct blasfeo_svec *tmp_nvc; + struct blasfeo_svec *tmp_nuxM; + struct blasfeo_smat *GammaQ; + struct blasfeo_smat *tmp_DCt; + struct blasfeo_smat *tmp_nuM_nxM; +// struct blasfeo_svec *d_qp; +// struct blasfeo_svec *d_mask_qp; + hpipm_size_t memsize; + }; + + +// +hpipm_size_t s_cond_qcqp_arg_memsize(); +// +void s_cond_qcqp_arg_create(struct s_cond_qcqp_arg *cond_arg, void *mem); +// +void s_cond_qcqp_arg_set_default(struct s_cond_qcqp_arg *cond_arg); +// set riccati-like algorithm: 0 classical, 1 square-root +void s_cond_qcqp_arg_set_ric_alg(int ric_alg, struct s_cond_qcqp_arg *cond_arg); +// condense last stage: 0 last stage disregarded, 1 last stage condensed too +void s_cond_qcqp_arg_set_cond_last_stage(int cond_last_stage, struct s_cond_qcqp_arg *cond_arg); + +// +void s_cond_qcqp_compute_dim(struct s_ocp_qcqp_dim *ocp_dim, struct s_dense_qcqp_dim *dense_dim); +// +hpipm_size_t s_cond_qcqp_ws_memsize(struct s_ocp_qcqp_dim *ocp_dim, struct s_cond_qcqp_arg *cond_arg); +// +void s_cond_qcqp_ws_create(struct s_ocp_qcqp_dim *ocp_dim, struct s_cond_qcqp_arg *cond_arg, struct s_cond_qcqp_ws *cond_ws, void *mem); +// +void s_cond_qcqp_qc(struct s_ocp_qcqp *ocp_qp, struct blasfeo_smat *Hq2, int *Hq_nzero2, struct blasfeo_smat *Ct2, struct blasfeo_svec *d2, struct s_cond_qcqp_arg *cond_arg, struct s_cond_qcqp_ws *cond_ws); +// +void s_cond_qcqp_qc_lhs(struct s_ocp_qcqp *ocp_qp, struct blasfeo_smat *Hq2, int *Hq_nzero2, struct blasfeo_smat *Ct2, struct s_cond_qcqp_arg *cond_arg, struct s_cond_qcqp_ws *cond_ws); +// +void s_cond_qcqp_qc_rhs(struct s_ocp_qcqp *ocp_qp, struct blasfeo_svec *d2, struct s_cond_qcqp_arg *cond_arg, struct s_cond_qcqp_ws *cond_ws); +// +void s_cond_qcqp_cond(struct s_ocp_qcqp *ocp_qp, struct s_dense_qcqp *dense_qp, struct s_cond_qcqp_arg *cond_arg, struct s_cond_qcqp_ws *cond_ws); +// +void s_cond_qcqp_cond_rhs(struct s_ocp_qcqp *ocp_qp, struct s_dense_qcqp *dense_qp, struct s_cond_qcqp_arg *cond_arg, struct s_cond_qcqp_ws *cond_ws); +// +void s_cond_qcqp_cond_lhs(struct s_ocp_qcqp *ocp_qp, struct s_dense_qcqp *dense_qp, struct s_cond_qcqp_arg *cond_arg, struct s_cond_qcqp_ws *cond_ws); +// +void s_cond_qcqp_expand_sol(struct s_ocp_qcqp *ocp_qp, struct s_dense_qcqp_sol *dense_qp_sol, struct s_ocp_qcqp_sol *ocp_qp_sol, struct s_cond_qcqp_arg *cond_arg, struct s_cond_qcqp_ws *cond_ws); + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_S_COND_QCQP_H_ + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_core_qp_ipm.h b/phonelibs/acados/include/hpipm/include/hpipm_s_core_qp_ipm.h new file mode 100644 index 0000000000..480392c7d9 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_core_qp_ipm.h @@ -0,0 +1,101 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_CORE_QP_IPM_ +#define HPIPM_S_CORE_QP_IPM_ + +#include "hpipm_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +struct s_core_qp_ipm_workspace + { + float *v; // primal variables + float *pi; // equality constraints multipliers + float *lam; // inequality constraints multipliers + float *t; // inequality constraints slacks + float *t_inv; // inverse of t + float *v_bkp; // backup of primal variables + float *pi_bkp; // backup of equality constraints multipliers + float *lam_bkp; // backup of inequality constraints multipliers + float *t_bkp; // backup of inequality constraints slacks + float *dv; // step in v + float *dpi; // step in pi + float *dlam; // step in lam + float *dt; // step in t + float *res_g; // q-residuals + float *res_b; // b-residuals + float *res_d; // d-residuals + float *res_m; // m-residuals + float *res_m_bkp; // m-residuals + float *Gamma; // Hessian update + float *gamma; // gradient update + float alpha_prim; // step length + float alpha_dual; // step length + float alpha; // step length + float sigma; // centering XXX + float mu; // duality measuere + float mu_aff; // affine duality measuere + float nc_inv; // 1.0/nc, where nc is the total number of constraints + float nc_mask_inv; // 1.0/nc_mask + float lam_min; // min value in lam vector + float t_min; // min value in t vector + float t_min_inv; // inverse of min value in t vector + float tau_min; // min value of barrier parameter + int nv; // number of primal variables + int ne; // number of equality constraints + int nc; // (twice the) number of (two-sided) inequality constraints + int nc_mask; // total number of ineq constr after masking + int split_step; // use different step for primal and dual variables + int t_lam_min; // clip t and lam also in solution, or only in Gamma computation + hpipm_size_t memsize; // memory size (in bytes) of workspace + }; + + + +// +hpipm_size_t s_memsize_core_qp_ipm(int nv, int ne, int nc); +// +void s_create_core_qp_ipm(int nv, int ne, int nc, struct s_core_qp_ipm_workspace *workspace, void *mem); +// +void s_core_qp_ipm(struct s_core_qp_ipm_workspace *workspace); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // HPIPM_S_CORE_QP_IPM_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_core_qp_ipm_aux.h b/phonelibs/acados/include/hpipm/include/hpipm_s_core_qp_ipm_aux.h new file mode 100644 index 0000000000..1ac3d7ede9 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_core_qp_ipm_aux.h @@ -0,0 +1,68 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_CORE_QP_IPM_AUX_ +#define HPIPM_S_CORE_QP_IPM_AUX_ + +#ifdef __cplusplus +extern "C" { +#endif + +// +void s_compute_Gamma_gamma_qp(float *res_d, float *res_m, struct s_core_qp_ipm_workspace *rws); +// +void s_compute_gamma_qp(float *res_d, float *res_m, struct s_core_qp_ipm_workspace *rws); +// +void s_compute_lam_t_qp(float *res_d, float *res_m, float *dlam, float *dt, struct s_core_qp_ipm_workspace *rws); +// +void s_compute_alpha_qp(struct s_core_qp_ipm_workspace *rws); +// +void s_update_var_qp(struct s_core_qp_ipm_workspace *rws); +// +void s_compute_mu_aff_qp(struct s_core_qp_ipm_workspace *rws); +// +void s_backup_res_m(struct s_core_qp_ipm_workspace *rws); +// +void s_compute_centering_correction_qp(struct s_core_qp_ipm_workspace *rws); +// +void s_compute_centering_qp(struct s_core_qp_ipm_workspace *rws); +// +void s_compute_tau_min_qp(struct s_core_qp_ipm_workspace *rws); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // HPIPM_S_CORE_QP_IPM_AUX_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_dense_qcqp.h b/phonelibs/acados/include/hpipm/include/hpipm_s_dense_qcqp.h new file mode 100644 index 0000000000..d03c065375 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_dense_qcqp.h @@ -0,0 +1,200 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_S_DENSE_QCQP_H_ +#define HPIPM_S_DENSE_QCQP_H_ + + + +#include +#include + +#include "hpipm_s_dense_qcqp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_dense_qcqp + { + struct s_dense_qcqp_dim *dim; + struct blasfeo_smat *Hv; // hessian of cost & vector work space + struct blasfeo_smat *A; // equality constraint matrix + struct blasfeo_smat *Ct; // inequality constraints matrix + struct blasfeo_smat *Hq; // hessians of quadratic constraints + struct blasfeo_svec *gz; // gradient of cost & gradient of slacks + struct blasfeo_svec *b; // equality constraint vector + struct blasfeo_svec *d; // inequality constraints vector + struct blasfeo_svec *d_mask; // inequality constraints mask vector + struct blasfeo_svec *m; // rhs of complementarity condition + struct blasfeo_svec *Z; // (diagonal) hessian of slacks + int *idxb; // indices of box constrained variables within [u; x] + int *idxs_rev; // index of soft constraints (reverse storage) + int *Hq_nzero; // for each int, the last 3 bits ...abc, {a,b,c}=0 => {R,S,Q}=0 + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +hpipm_size_t s_dense_qcqp_memsize(struct s_dense_qcqp_dim *dim); +// +void s_dense_qcqp_create(struct s_dense_qcqp_dim *dim, struct s_dense_qcqp *qp, void *memory); + +// +void s_dense_qcqp_set(char *field, void *value, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_H(float *H, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_g(float *g, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_A(float *A, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_b(float *b, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_idxb(int *idxb, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_lb(float *lb, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_lb_mask(float *lb, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_ub(float *ub, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_ub_mask(float *ub, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_C(float *C, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_lg(float *lg, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_lg_mask(float *lg, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_ug(float *ug, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_ug_mask(float *ug, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_Hq(float *Hq, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_gq(float *gq, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_uq(float *uq, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_uq_mask(float *uq, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_idxs(int *idxs, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_idxs_rev(int *idxs_rev, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_Zl(float *Zl, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_Zu(float *Zu, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_zl(float *zl, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_zu(float *zu, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_ls(float *ls, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_ls_mask(float *ls, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_us(float *us, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_us_mask(float *us, struct s_dense_qcqp *qp); + +// getters (COLMAJ) + +void s_dense_qcqp_get_H(struct s_dense_qcqp *qp, float *H); +// +void s_dense_qcqp_get_g(struct s_dense_qcqp *qp, float *g); +// +void s_dense_qcqp_get_A(struct s_dense_qcqp *qp, float *A); +// +void s_dense_qcqp_get_b(struct s_dense_qcqp *qp, float *b); +// +void s_dense_qcqp_get_idxb(struct s_dense_qcqp *qp, int *idxb); +// +void s_dense_qcqp_get_lb(struct s_dense_qcqp *qp, float *lb); +// +void s_dense_qcqp_get_lb_mask(struct s_dense_qcqp *qp, float *lb); +// +void s_dense_qcqp_get_ub(struct s_dense_qcqp *qp, float *ub); +// +void s_dense_qcqp_get_ub_mask(struct s_dense_qcqp *qp, float *ub); +// +void s_dense_qcqp_get_C(struct s_dense_qcqp *qp, float *C); +// +void s_dense_qcqp_get_lg(struct s_dense_qcqp *qp, float *lg); +// +void s_dense_qcqp_get_lg_mask(struct s_dense_qcqp *qp, float *lg); +// +void s_dense_qcqp_get_ug(struct s_dense_qcqp *qp, float *ug); +// +void s_dense_qcqp_get_ug_mask(struct s_dense_qcqp *qp, float *ug); +// +void s_dense_qcqp_get_idxs(struct s_dense_qcqp *qp, int *idxs); +// +void s_dense_qcqp_get_idxs_rev(struct s_dense_qcqp *qp, int *idxs_rev); +// +void s_dense_qcqp_get_Zl(struct s_dense_qcqp *qp, float *Zl); +// +void s_dense_qcqp_get_Zu(struct s_dense_qcqp *qp, float *Zu); +// +void s_dense_qcqp_get_zl(struct s_dense_qcqp *qp, float *zl); +// +void s_dense_qcqp_get_zu(struct s_dense_qcqp *qp, float *zu); +// +void s_dense_qcqp_get_ls(struct s_dense_qcqp *qp, float *ls); +// +void s_dense_qcqp_get_ls_mask(struct s_dense_qcqp *qp, float *ls); +// +void s_dense_qcqp_get_us(struct s_dense_qcqp *qp, float *us); +// +void s_dense_qcqp_get_us_mask(struct s_dense_qcqp *qp, float *us); + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_S_DENSE_QCQP_H_ + + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_dense_qcqp_dim.h b/phonelibs/acados/include/hpipm/include/hpipm_s_dense_qcqp_dim.h new file mode 100644 index 0000000000..04908c2c3a --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_dense_qcqp_dim.h @@ -0,0 +1,99 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_DENSE_QCQP_DIM_H_ +#define HPIPM_S_DENSE_QCQP_DIM_H_ + +#include "hpipm_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_dense_qcqp_dim + { + struct s_dense_qp_dim *qp_dim; // dim of qp approximation + int nv; // number of variables + int ne; // number of equality constraints + int nb; // number of box constraints + int ng; // number of general constraints + int nq; // number of quadratic constraints + int nsb; // number of softened box constraints + int nsg; // number of softened general constraints + int nsq; // number of softened quadratic constraints + int ns; // number of softened constraints (nsb+nsg+nsq) TODO number of slacks + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_dense_qcqp_dim_memsize(); +// +void s_dense_qcqp_dim_create(struct s_dense_qcqp_dim *dim, void *memory); +// +void s_dense_qcqp_dim_set(char *fiels_name, int value, struct s_dense_qcqp_dim *dim); +// +void s_dense_qcqp_dim_set_nv(int value, struct s_dense_qcqp_dim *dim); +// +void s_dense_qcqp_dim_set_ne(int value, struct s_dense_qcqp_dim *dim); +// +void s_dense_qcqp_dim_set_nb(int value, struct s_dense_qcqp_dim *dim); +// +void s_dense_qcqp_dim_set_ng(int value, struct s_dense_qcqp_dim *dim); +// +void s_dense_qcqp_dim_set_nq(int value, struct s_dense_qcqp_dim *dim); +// +void s_dense_qcqp_dim_set_nsb(int value, struct s_dense_qcqp_dim *dim); +// +void s_dense_qcqp_dim_set_nsg(int value, struct s_dense_qcqp_dim *dim); +// +void s_dense_qcqp_dim_set_nsq(int value, struct s_dense_qcqp_dim *dim); +// +void s_dense_qcqp_dim_set_ns(int value, struct s_dense_qcqp_dim *dim); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_S_DENSE_QCQP_DIM_H_ + + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_dense_qcqp_ipm.h b/phonelibs/acados/include/hpipm/include/hpipm_s_dense_qcqp_ipm.h new file mode 100644 index 0000000000..8f85768ee3 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_dense_qcqp_ipm.h @@ -0,0 +1,204 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_S_DENSE_QCQP_IPM_H_ +#define HPIPM_S_DENSE_QCQP_IPM_H_ + + + +#include +#include + +#include +#include +#include +#include +#include + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_dense_qcqp_ipm_arg + { + struct s_dense_qp_ipm_arg *qp_arg; + float mu0; // initial value for duality measure + float alpha_min; // exit cond on step length + float res_g_max; // exit cond on inf norm of residuals + float res_b_max; // exit cond on inf norm of residuals + float res_d_max; // exit cond on inf norm of residuals + float res_m_max; // exit cond on inf norm of residuals + float reg_prim; // reg of primal hessian + float reg_dual; // reg of dual hessian + float lam_min; // min value in lam vector + float t_min; // min value in t vector + int iter_max; // exit cond in iter number + int stat_max; // iterations saved in stat + int pred_corr; // Mehrotra's predictor-corrector IPM algirthm + int cond_pred_corr; // conditional Mehrotra's predictor-corrector + int scale; // scale hessian + int itref_pred_max; // max number of iterative refinement steps for predictor step + int itref_corr_max; // max number of iterative refinement steps for corrector step + int warm_start; // 0 no warm start, 1 warm start primal sol, 2 warm start primal and dual sol + int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq + int abs_form; // absolute IPM formulation + int comp_res_exit; // compute residuals on exit (only for abs_form==1) + int comp_res_pred; // compute residuals of prediction + int split_step; // use different step for primal and dual variables + int t_lam_min; // clip t and lam: 0 no, 1 in Gamma computation, 2 in solution + int mode; + hpipm_size_t memsize; + }; + + + +struct s_dense_qcqp_ipm_ws + { +// float qp_res[4]; // infinity norm of residuals + struct s_dense_qp_ipm_ws *qp_ws; + struct s_dense_qp *qp; + struct s_dense_qp_sol *qp_sol; + struct s_dense_qcqp_res_ws *qcqp_res_ws; + struct s_dense_qcqp_res *qcqp_res; + struct blasfeo_svec *tmp_nv; +// float *stat; // convergence statistics +// void *lq_work0; +// void *lq_work1; + int iter; // iteration number +// int stat_max; // iterations saved in stat +// int stat_m; // numer of recorded stat per ipm iter +// int scale; +// int use_hess_fact; + int status; + hpipm_size_t memsize; // memory size (in bytes) of workspace + }; + + + +// +hpipm_size_t s_dense_qcqp_ipm_arg_memsize(struct s_dense_qcqp_dim *dim); +// +void s_dense_qcqp_ipm_arg_create(struct s_dense_qcqp_dim *dim, struct s_dense_qcqp_ipm_arg *arg, void *mem); +// +void s_dense_qcqp_ipm_arg_set_default(enum hpipm_mode mode, struct s_dense_qcqp_ipm_arg *arg); +// +void s_dense_qcqp_ipm_arg_set(char *field, void *value, struct s_dense_qcqp_ipm_arg *arg); +// +void s_dense_qcqp_ipm_arg_set_iter_max(int *iter_max, struct s_dense_qcqp_ipm_arg *arg); +// +void s_dense_qcqp_ipm_arg_set_alpha_min(float *alpha_min, struct s_dense_qcqp_ipm_arg *arg); +// +void s_dense_qcqp_ipm_arg_set_mu0(float *mu0, struct s_dense_qcqp_ipm_arg *arg); +// +void s_dense_qcqp_ipm_arg_set_tol_stat(float *tol_stat, struct s_dense_qcqp_ipm_arg *arg); +// +void s_dense_qcqp_ipm_arg_set_tol_eq(float *tol_eq, struct s_dense_qcqp_ipm_arg *arg); +// +void s_dense_qcqp_ipm_arg_set_tol_ineq(float *tol_ineq, struct s_dense_qcqp_ipm_arg *arg); +// +void s_dense_qcqp_ipm_arg_set_tol_comp(float *tol_comp, struct s_dense_qcqp_ipm_arg *arg); +// +void s_dense_qcqp_ipm_arg_set_reg_prim(float *reg, struct s_dense_qcqp_ipm_arg *arg); +// +void s_dense_qcqp_ipm_arg_set_reg_dual(float *reg, struct s_dense_qcqp_ipm_arg *arg); +// +void s_dense_qcqp_ipm_arg_set_warm_start(int *warm_start, struct s_dense_qcqp_ipm_arg *arg); +// +void s_dense_qcqp_ipm_arg_set_pred_corr(int *pred_corr, struct s_dense_qcqp_ipm_arg *arg); +// +void s_dense_qcqp_ipm_arg_set_cond_pred_corr(int *cond_pred_corr, struct s_dense_qcqp_ipm_arg *arg); +// +void s_dense_qcqp_ipm_arg_set_comp_res_pred(int *comp_res_pred, struct s_dense_qcqp_ipm_arg *arg); +// +void s_dense_qcqp_ipm_arg_set_comp_res_exit(int *comp_res_exit, struct s_dense_qcqp_ipm_arg *arg); +// +void s_dense_qcqp_ipm_arg_set_lam_min(float *value, struct s_dense_qcqp_ipm_arg *arg); +// +void s_dense_qcqp_ipm_arg_set_t_min(float *value, struct s_dense_qcqp_ipm_arg *arg); +// +void s_dense_qcqp_ipm_arg_set_split_step(int *value, struct s_dense_qcqp_ipm_arg *arg); +// +void s_dense_qcqp_ipm_arg_set_t_lam_min(int *value, struct s_dense_qcqp_ipm_arg *arg); + +// +hpipm_size_t s_dense_qcqp_ipm_ws_memsize(struct s_dense_qcqp_dim *qp_dim, struct s_dense_qcqp_ipm_arg *arg); +// +void s_dense_qcqp_ipm_ws_create(struct s_dense_qcqp_dim *qp_dim, struct s_dense_qcqp_ipm_arg *arg, struct s_dense_qcqp_ipm_ws *ws, void *mem); +// +void s_dense_qcqp_ipm_get(char *field, struct s_dense_qcqp_ipm_ws *ws, void *value); +// +void s_dense_qcqp_ipm_get_status(struct s_dense_qcqp_ipm_ws *ws, int *status); +// +void s_dense_qcqp_ipm_get_iter(struct s_dense_qcqp_ipm_ws *ws, int *iter); +// +void s_dense_qcqp_ipm_get_max_res_stat(struct s_dense_qcqp_ipm_ws *ws, float *res_stat); +// +void s_dense_qcqp_ipm_get_max_res_eq(struct s_dense_qcqp_ipm_ws *ws, float *res_eq); +// +void s_dense_qcqp_ipm_get_max_res_ineq(struct s_dense_qcqp_ipm_ws *ws, float *res_ineq); +// +void s_dense_qcqp_ipm_get_max_res_comp(struct s_dense_qcqp_ipm_ws *ws, float *res_comp); +// +void s_dense_qcqp_ipm_get_stat(struct s_dense_qcqp_ipm_ws *ws, float **stat); +// +void s_dense_qcqp_ipm_get_stat_m(struct s_dense_qcqp_ipm_ws *ws, int *stat_m); +#if 0 +// +void s_dense_qcqp_init_var(struct s_dense_qcqp *qp, struct s_dense_qcqp_sol *qp_sol, struct s_dense_qcqp_ipm_arg *arg, struct s_dense_qcqp_ipm_ws *ws); +#endif +// +void s_dense_qcqp_ipm_solve(struct s_dense_qcqp *qp, struct s_dense_qcqp_sol *qp_sol, struct s_dense_qcqp_ipm_arg *arg, struct s_dense_qcqp_ipm_ws *ws); +#if 0 +// +void s_dense_qcqp_ipm_predict(struct s_dense_qcqp *qp, struct s_dense_qcqp_sol *qp_sol, struct s_dense_qcqp_ipm_arg *arg, struct s_dense_qcqp_ipm_ws *ws); +// +void s_dense_qcqp_ipm_sens(struct s_dense_qcqp *qp, struct s_dense_qcqp_sol *qp_sol, struct s_dense_qcqp_ipm_arg *arg, struct s_dense_qcqp_ipm_ws *ws); +#endif + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_S_DENSE_QCQP_IPM_H_ + + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_dense_qcqp_res.h b/phonelibs/acados/include/hpipm/include/hpipm_s_dense_qcqp_res.h new file mode 100644 index 0000000000..779658cc8f --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_dense_qcqp_res.h @@ -0,0 +1,108 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_DENSE_QCQP_RES_H_ +#define HPIPM_S_DENSE_QCQP_RES_H_ + + + +#include +#include + +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_dense_qcqp_res + { + struct s_dense_qcqp_dim *dim; + struct blasfeo_svec *res_g; // q-residuals + struct blasfeo_svec *res_b; // b-residuals + struct blasfeo_svec *res_d; // d-residuals + struct blasfeo_svec *res_m; // m-residuals + float res_max[4]; // infinity norm of residuals + float res_mu; // mu-residual + hpipm_size_t memsize; + }; + + + +struct s_dense_qcqp_res_ws + { + struct blasfeo_svec *tmp_nv; // work space of size nv + struct blasfeo_svec *tmp_nbgq; // work space of size nbM+ngM+nqM + struct blasfeo_svec *tmp_ns; // work space of size nsM + struct blasfeo_svec *q_fun; // value for evaluation of quadr constr + struct blasfeo_svec *q_adj; // value for adjoint of quadr constr + int use_q_fun; // reuse cached value for evaluation of quadr constr + int use_q_adj; // reuse cached value for adjoint of quadr constr + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_dense_qcqp_res_memsize(struct s_dense_qcqp_dim *dim); +// +void s_dense_qcqp_res_create(struct s_dense_qcqp_dim *dim, struct s_dense_qcqp_res *res, void *mem); +// +hpipm_size_t s_dense_qcqp_res_ws_memsize(struct s_dense_qcqp_dim *dim); +// +void s_dense_qcqp_res_ws_create(struct s_dense_qcqp_dim *dim, struct s_dense_qcqp_res_ws *workspace, void *mem); +// +void s_dense_qcqp_res_compute(struct s_dense_qcqp *qp, struct s_dense_qcqp_sol *qp_sol, struct s_dense_qcqp_res *res, struct s_dense_qcqp_res_ws *ws); +// +void s_dense_qcqp_res_compute_inf_norm(struct s_dense_qcqp_res *res); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + +#endif // HPIPM_S_DENSE_QCQP_RES_H_ + + + + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_dense_qcqp_sol.h b/phonelibs/acados/include/hpipm/include/hpipm_s_dense_qcqp_sol.h new file mode 100644 index 0000000000..197a690e52 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_dense_qcqp_sol.h @@ -0,0 +1,86 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_S_DENSE_QCQP_SOL_H_ +#define HPIPM_S_DENSE_QCQP_SOL_H_ + + + +#include +#include + +#include "hpipm_s_dense_qcqp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_dense_qcqp_sol + { + struct s_dense_qcqp_dim *dim; + struct blasfeo_svec *v; + struct blasfeo_svec *pi; + struct blasfeo_svec *lam; + struct blasfeo_svec *t; + void *misc; + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_dense_qcqp_sol_memsize(struct s_dense_qcqp_dim *dim); +// +void s_dense_qcqp_sol_create(struct s_dense_qcqp_dim *dim, struct s_dense_qcqp_sol *qp_sol, void *memory); +// +void s_dense_qcqp_sol_get_v(struct s_dense_qcqp_sol *qp_sol, float *v); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_S_DENSE_QCQP_SOL_H_ + + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_dense_qcqp_utils.h b/phonelibs/acados/include/hpipm/include/hpipm_s_dense_qcqp_utils.h new file mode 100644 index 0000000000..4f5aae26eb --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_dense_qcqp_utils.h @@ -0,0 +1,83 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_DENSE_QCQP_UTILS_H_ +#define HPIPM_S_DENSE_QCQP_UTILS_H_ + + + +#include +#include + +#include "hpipm_s_dense_qcqp_dim.h" +#include "hpipm_s_dense_qcqp.h" +#include "hpipm_s_dense_qcqp_sol.h" +//#include "hpipm_s_dense_qcqp_ipm.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +// +void s_dense_qcqp_dim_print(struct s_dense_qcqp_dim *qp_dim); +// +//void s_dense_qcqp_dim_codegen(char *file_name, char *mode, struct s_dense_qcqp_dim *qp_dim); +// +void s_dense_qcqp_print(struct s_dense_qcqp_dim *qp_dim, struct s_dense_qcqp *qp); +// +//void s_dense_qcqp_codegen(char *file_name, char *mode, struct s_dense_qcqp_dim *qp_dim, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_sol_print(struct s_dense_qcqp_dim *qp_dim, struct s_dense_qcqp_sol *dense_qcqp_sol); +// +//void s_dense_qcqp_ipm_arg_codegen(char *file_name, char *mode, struct s_dense_qcqp_dim *qp_dim, struct s_dense_qcqp_ipm_arg *arg); +// +void s_dense_qcqp_res_print(struct s_dense_qcqp_dim *qp_dim, struct s_dense_qcqp_res *dense_qcqp_res); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_S_DENSE_QCQP_UTILS_H_ + + + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_dense_qp.h b/phonelibs/acados/include/hpipm/include/hpipm_s_dense_qp.h new file mode 100644 index 0000000000..3c2517fe1b --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_dense_qp.h @@ -0,0 +1,207 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_S_DENSE_QP_H_ +#define HPIPM_S_DENSE_QP_H_ + + + +#include +#include + +#include "hpipm_s_dense_qp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_dense_qp + { + struct s_dense_qp_dim *dim; + struct blasfeo_smat *Hv; // hessian & gradient + struct blasfeo_smat *A; // dynamics matrix + struct blasfeo_smat *Ct; // constraints matrix + struct blasfeo_svec *gz; // gradient & gradient of slacks + struct blasfeo_svec *b; // dynamics vector + struct blasfeo_svec *d; // constraints vector + struct blasfeo_svec *d_mask; // inequality constraints mask vector + struct blasfeo_svec *m; // rhs of complementarity condition + struct blasfeo_svec *Z; // (diagonal) hessian of slacks + int *idxb; // indices of box constrained variables within [u; x] + int *idxs_rev; // index of soft constraints (reverse storage) + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +hpipm_size_t s_dense_qp_memsize(struct s_dense_qp_dim *dim); +// +void s_dense_qp_create(struct s_dense_qp_dim *dim, struct s_dense_qp *qp, void *memory); + +// setters - colmaj +// +void s_dense_qp_set_all(float *H, float *g, float *A, float *b, int *idxb, float *d_lb, float *d_ub, float *C, float *d_lg, float *d_ug, float *Zl, float *Zu, float *zl, float *zu, int *idxs, float *d_ls, float *d_us, struct s_dense_qp *qp); +// +void s_dense_qp_get_all(struct s_dense_qp *qp, float *H, float *g, float *A, float *b, int *idxb, float *d_lb, float *d_ub, float *C, float *d_lg, float *d_ug, float *Zl, float *Zu, float *zl, float *zu, int *idxs, float *d_ls, float *d_us); +// +void s_dense_qp_set(char *field, void *value, struct s_dense_qp *qp); +// +void s_dense_qp_set_H(float *H, struct s_dense_qp *qp); +// +void s_dense_qp_set_g(float *g, struct s_dense_qp *qp); +// +void s_dense_qp_set_A(float *A, struct s_dense_qp *qp); +// +void s_dense_qp_set_b(float *b, struct s_dense_qp *qp); +// +void s_dense_qp_set_idxb(int *idxb, struct s_dense_qp *qp); +// +void s_dense_qp_set_Jb(float *Jb, struct s_dense_qp *qp); +// +void s_dense_qp_set_lb(float *lb, struct s_dense_qp *qp); +// +void s_dense_qp_set_lb_mask(float *lb, struct s_dense_qp *qp); +// +void s_dense_qp_set_ub(float *ub, struct s_dense_qp *qp); +// +void s_dense_qp_set_ub_mask(float *ub, struct s_dense_qp *qp); +// +void s_dense_qp_set_C(float *C, struct s_dense_qp *qp); +// +void s_dense_qp_set_lg(float *lg, struct s_dense_qp *qp); +// +void s_dense_qp_set_lg_mask(float *lg, struct s_dense_qp *qp); +// +void s_dense_qp_set_ug(float *ug, struct s_dense_qp *qp); +// +void s_dense_qp_set_ug_mask(float *ug, struct s_dense_qp *qp); +// +void s_dense_qp_set_idxs(int *idxs, struct s_dense_qp *qp); +// +void s_dense_qp_set_idxs_rev(int *idxs_rev, struct s_dense_qp *qp); +// +void s_dense_qp_set_Jsb(float *Jsb, struct s_dense_qp *qp); +// +void s_dense_qp_set_Jsg(float *Jsg, struct s_dense_qp *qp); +// +void s_dense_qp_set_Zl(float *Zl, struct s_dense_qp *qp); +// +void s_dense_qp_set_Zu(float *Zu, struct s_dense_qp *qp); +// +void s_dense_qp_set_zl(float *zl, struct s_dense_qp *qp); +// +void s_dense_qp_set_zu(float *zu, struct s_dense_qp *qp); +// +void s_dense_qp_set_ls(float *ls, struct s_dense_qp *qp); +// +void s_dense_qp_set_ls_mask(float *ls, struct s_dense_qp *qp); +// +void s_dense_qp_set_us(float *us, struct s_dense_qp *qp); +// +void s_dense_qp_set_us_mask(float *us, struct s_dense_qp *qp); + +// getters - colmaj +// +void s_dense_qp_get_H(struct s_dense_qp *qp, float *H); +// +void s_dense_qp_get_g(struct s_dense_qp *qp, float *g); +// +void s_dense_qp_get_A(struct s_dense_qp *qp, float *A); +// +void s_dense_qp_get_b(struct s_dense_qp *qp, float *b); +// +void s_dense_qp_get_idxb(struct s_dense_qp *qp, int *idxb); +// +void s_dense_qp_get_lb(struct s_dense_qp *qp, float *lb); +// +void s_dense_qp_get_lb_mask(struct s_dense_qp *qp, float *lb); +// +void s_dense_qp_get_ub(struct s_dense_qp *qp, float *ub); +// +void s_dense_qp_get_ub_mask(struct s_dense_qp *qp, float *ub); +// +void s_dense_qp_get_C(struct s_dense_qp *qp, float *C); +// +void s_dense_qp_get_lg(struct s_dense_qp *qp, float *lg); +// +void s_dense_qp_get_lg_mask(struct s_dense_qp *qp, float *lg); +// +void s_dense_qp_get_ug(struct s_dense_qp *qp, float *ug); +// +void s_dense_qp_get_ug_mask(struct s_dense_qp *qp, float *ug); +// +void s_dense_qp_get_idxs(struct s_dense_qp *qp, int *idxs); +// +void s_dense_qp_get_idxs_rev(struct s_dense_qp *qp, int *idxs_rev); +// +void s_dense_qp_get_Zl(struct s_dense_qp *qp, float *Zl); +// +void s_dense_qp_get_Zu(struct s_dense_qp *qp, float *Zu); +// +void s_dense_qp_get_zl(struct s_dense_qp *qp, float *zl); +// +void s_dense_qp_get_zu(struct s_dense_qp *qp, float *zu); +// +void s_dense_qp_get_ls(struct s_dense_qp *qp, float *ls); +// +void s_dense_qp_get_ls_mask(struct s_dense_qp *qp, float *ls); +// +void s_dense_qp_get_us(struct s_dense_qp *qp, float *us); +// +void s_dense_qp_get_us_mask(struct s_dense_qp *qp, float *us); + +// setters - rowmaj +// +void s_dense_qp_set_all_rowmaj(float *H, float *g, float *A, float *b, int *idxb, float *d_lb, float *d_ub, float *C, float *d_lg, float *d_ug, float *Zl, float *Zu, float *zl, float *zu, int *idxs, float *d_ls, float *d_us, struct s_dense_qp *qp); + +// getters - rowmaj +// +void s_dense_qp_get_all_rowmaj(struct s_dense_qp *qp, float *H, float *g, float *A, float *b, int *idxb, float *d_lb, float *d_ub, float *C, float *d_lg, float *d_ug, float *Zl, float *Zu, float *zl, float *zu, int *idxs, float *d_ls, float *d_us); + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_S_DENSE_QP_H_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_dense_qp_dim.h b/phonelibs/acados/include/hpipm/include/hpipm_s_dense_qp_dim.h new file mode 100644 index 0000000000..b979d24432 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_dense_qp_dim.h @@ -0,0 +1,94 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_DENSE_QP_DIM_H_ +#define HPIPM_S_DENSE_QP_DIM_H_ + +#include "hpipm_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_dense_qp_dim + { + int nv; // number of variables + int ne; // number of equality constraints + int nb; // number of box constraints + int ng; // number of general constraints + int nsb; // number of softened box constraints + int nsg; // number of softened general constraints + int ns; // number of softened constraints (nsb+nsg) + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_dense_qp_dim_memsize(); +// +void s_dense_qp_dim_create(struct s_dense_qp_dim *qp_dim, void *memory); +// +void s_dense_qp_dim_set_all(int nv, int ne, int nb, int ng, int nsb, int nsg, struct s_dense_qp_dim *dim); +// +void s_dense_qp_dim_set(char *fiels_name, int value, struct s_dense_qp_dim *dim); +// +void s_dense_qp_dim_set_nv(int value, struct s_dense_qp_dim *dim); +// +void s_dense_qp_dim_set_ne(int value, struct s_dense_qp_dim *dim); +// +void s_dense_qp_dim_set_nb(int value, struct s_dense_qp_dim *dim); +// +void s_dense_qp_dim_set_ng(int value, struct s_dense_qp_dim *dim); +// +void s_dense_qp_dim_set_nsb(int value, struct s_dense_qp_dim *dim); +// +void s_dense_qp_dim_set_nsg(int value, struct s_dense_qp_dim *dim); +// +void s_dense_qp_dim_set_ns(int value, struct s_dense_qp_dim *dim); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_S_DENSE_QP_DIM_H_ + + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_dense_qp_ipm.h b/phonelibs/acados/include/hpipm/include/hpipm_s_dense_qp_ipm.h new file mode 100644 index 0000000000..f2d56d4529 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_dense_qp_ipm.h @@ -0,0 +1,260 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_S_DENSE_QP_IPM_H_ +#define HPIPM_S_DENSE_QP_IPM_H_ + + + +#include +#include + +#include +#include +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_dense_qp_ipm_arg + { + float mu0; // initial value for duality measure + float alpha_min; // exit cond on step length + float res_g_max; // exit cond on inf norm of residuals + float res_b_max; // exit cond on inf norm of residuals + float res_d_max; // exit cond on inf norm of residuals + float res_m_max; // exit cond on inf norm of residuals + float reg_prim; // reg of primal hessian + float reg_dual; // reg of dual hessian + float lam_min; // min value in lam vector + float t_min; // min value in t vector + float tau_min; // min value of barrier parameter + int iter_max; // exit cond in iter number + int stat_max; // iterations saved in stat + int pred_corr; // Mehrotra's predictor-corrector IPM algirthm + int cond_pred_corr; // conditional Mehrotra's predictor-corrector + int scale; // scale hessian + int itref_pred_max; // max number of iterative refinement steps for predictor step + int itref_corr_max; // max number of iterative refinement steps for corrector step + int warm_start; // 0 no warm start, 1 warm start primal sol, 2 warm start primal and dual sol + int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq + int abs_form; // absolute IPM formulation + int comp_res_exit; // compute residuals on exit (only for abs_form==1) + int comp_res_pred; // compute residuals of prediction + int kkt_fact_alg; // 0 null-space, 1 schur-complement + int remove_lin_dep_eq; // 0 do not, 1 do check and remove linearly dependent equality constraints + int compute_obj; // compute obj on exit + int split_step; // use different steps for primal and dual variables + int t_lam_min; // clip t and lam: 0 no, 1 in Gamma computation, 2 in solution + int mode; + hpipm_size_t memsize; + }; + + + +struct s_dense_qp_ipm_ws + { + struct s_core_qp_ipm_workspace *core_workspace; + struct s_dense_qp_res_ws *res_ws; + struct s_dense_qp_sol *sol_step; + struct s_dense_qp_sol *sol_itref; + struct s_dense_qp *qp_step; + struct s_dense_qp *qp_itref; + struct s_dense_qp_res *res; + struct s_dense_qp_res *res_itref; + struct s_dense_qp_res *res_step; + struct blasfeo_svec *Gamma; // + struct blasfeo_svec *gamma; // + struct blasfeo_svec *Zs_inv; // + struct blasfeo_smat *Lv; // + struct blasfeo_smat *AL; // + struct blasfeo_smat *Le; // + struct blasfeo_smat *Ctx; // + struct blasfeo_svec *lv; // + struct blasfeo_svec *sv; // scale for Lv + struct blasfeo_svec *se; // scale for Le + struct blasfeo_svec *tmp_nbg; // work space of size nb+ng + struct blasfeo_svec *tmp_ns; // work space of size ns + struct blasfeo_smat *lq0; + struct blasfeo_smat *lq1; + struct blasfeo_svec *tmp_m; + struct blasfeo_smat *A_LQ; + struct blasfeo_smat *A_Q; + struct blasfeo_smat *Zt; + struct blasfeo_smat *ZtH; + struct blasfeo_smat *ZtHZ; + struct blasfeo_svec *xy; + struct blasfeo_svec *Yxy; + struct blasfeo_svec *xz; + struct blasfeo_svec *tmp_nv; + struct blasfeo_svec *tmp_2ns; + struct blasfeo_svec *tmp_nv2ns; + struct blasfeo_smat *A_li; // A of linearly independent equality constraints + struct blasfeo_svec *b_li; // b of linearly independent equality constraints + struct blasfeo_smat *A_bkp; // pointer to backup A + struct blasfeo_svec *b_bkp; // pointer to backup b + struct blasfeo_smat *Ab_LU; + float *stat; // convergence statistics + int *ipiv_v; + int *ipiv_e; + int *ipiv_e1; + void *lq_work0; + void *lq_work1; + void *lq_work_null; + void *orglq_work_null; + int iter; // iteration number + int stat_max; // iterations saved in stat + int stat_m; // numer of recorded stat per ipm iter + int scale; + int use_hess_fact; + int use_A_fact; + int status; + int lq_fact; // cache from arg + int mask_constr; // use constr mask + int ne_li; // number of linearly independent equality constraints + int ne_bkp; // ne backup + hpipm_size_t memsize; // memory size (in bytes) of workspace + }; + + + +// +hpipm_size_t s_dense_qp_ipm_arg_memsize(struct s_dense_qp_dim *qp_dim); +// +void s_dense_qp_ipm_arg_create(struct s_dense_qp_dim *qp_dim, struct s_dense_qp_ipm_arg *arg, void *mem); +// +void s_dense_qp_ipm_arg_set_default(enum hpipm_mode mode, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set(char *field, void *value, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_iter_max(int *iter_max, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_alpha_min(float *alpha_min, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_mu0(float *mu0, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_tol_stat(float *tol_stat, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_tol_eq(float *tol_eq, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_tol_ineq(float *tol_ineq, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_tol_comp(float *tol_comp, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_reg_prim(float *reg, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_reg_dual(float *reg, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_warm_start(int *warm_start, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_pred_corr(int *pred_corr, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_cond_pred_corr(int *cond_pred_corr, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_comp_res_pred(int *comp_res_pred, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_comp_res_exit(int *comp_res_exit, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_lam_min(float *value, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_t_min(float *value, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_tau_min(float *value, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_kkt_fact_alg(int *value, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_remove_lin_dep_eq(int *value, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_compute_obj(int *value, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_split_step(int *value, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_t_lam_min(int *value, struct s_dense_qp_ipm_arg *arg); + +// +hpipm_size_t s_dense_qp_ipm_ws_memsize(struct s_dense_qp_dim *qp_dim, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_ws_create(struct s_dense_qp_dim *qp_dim, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws, void *mem); +// +void s_dense_qp_ipm_get(char *field, struct s_dense_qp_ipm_ws *ws, void *value); +// +void s_dense_qp_ipm_get_status(struct s_dense_qp_ipm_ws *ws, int *status); +// +void s_dense_qp_ipm_get_iter(struct s_dense_qp_ipm_ws *ws, int *iter); +// +void s_dense_qp_ipm_get_max_res_stat(struct s_dense_qp_ipm_ws *ws, float *res_stat); +// +void s_dense_qp_ipm_get_max_res_eq(struct s_dense_qp_ipm_ws *ws, float *res_eq); +// +void s_dense_qp_ipm_get_max_res_ineq(struct s_dense_qp_ipm_ws *ws, float *res_ineq); +// +void s_dense_qp_ipm_get_max_res_comp(struct s_dense_qp_ipm_ws *ws, float *res_comp); +// +void s_dense_qp_ipm_get_stat(struct s_dense_qp_ipm_ws *ws, float **stat); +// +void s_dense_qp_ipm_get_stat_m(struct s_dense_qp_ipm_ws *ws, int *stat_m); +// +void s_dense_qp_init_var(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws); +// +void s_dense_qp_ipm_abs_step(int kk, struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws); +// +void s_dense_qp_ipm_delta_step(int kk, struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws); +// +void s_dense_qp_ipm_solve(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws); +// +void s_dense_qp_ipm_predict(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws); +// +void s_dense_qp_ipm_sens(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws); +// +void s_dense_qp_compute_step_length(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_S_DENSE_QP_IPM_H_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_dense_qp_kkt.h b/phonelibs/acados/include/hpipm/include/hpipm_s_dense_qp_kkt.h new file mode 100644 index 0000000000..260dc0ab21 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_dense_qp_kkt.h @@ -0,0 +1,72 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_S_DENSE_QP_KKT_H_ +#define HPIPM_S_DENSE_QP_KKT_H_ + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +// +void s_fact_solve_kkt_unconstr_dense_qp(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws); +// +void s_fact_solve_kkt_step_dense_qp(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws); +// +void s_fact_lq_solve_kkt_step_dense_qp(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws); +// +void s_solve_kkt_step_dense_qp(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws); +// +void s_dense_qp_remove_lin_dep_eq(struct s_dense_qp *qp, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws); +// +void s_dense_qp_restore_lin_dep_eq(struct s_dense_qp *qp, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws); +// +void s_dense_qp_compute_obj(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_S_DENSE_QP_KKT_H_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_dense_qp_res.h b/phonelibs/acados/include/hpipm/include/hpipm_s_dense_qp_res.h new file mode 100644 index 0000000000..06b609c537 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_dense_qp_res.h @@ -0,0 +1,106 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_DENSE_QP_RES_H_ +#define HPIPM_S_DENSE_QP_RES_H_ + + + +#include +#include + +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_dense_qp_res + { + struct s_dense_qp_dim *dim; + struct blasfeo_svec *res_g; // q-residuals + struct blasfeo_svec *res_b; // b-residuals + struct blasfeo_svec *res_d; // d-residuals + struct blasfeo_svec *res_m; // m-residuals + float res_max[4]; // max of residuals + float res_mu; // mu-residual + hpipm_size_t memsize; + }; + + + +struct s_dense_qp_res_ws + { + struct blasfeo_svec *tmp_nbg; // work space of size nbM+ngM + struct blasfeo_svec *tmp_ns; // work space of size nsM + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_dense_qp_res_memsize(struct s_dense_qp_dim *dim); +// +void s_dense_qp_res_create(struct s_dense_qp_dim *dim, struct s_dense_qp_res *res, void *mem); +// +hpipm_size_t s_dense_qp_res_ws_memsize(struct s_dense_qp_dim *dim); +// +void s_dense_qp_res_ws_create(struct s_dense_qp_dim *dim, struct s_dense_qp_res_ws *workspace, void *mem); +// +void s_dense_qp_res_compute(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_dense_qp_res *res, struct s_dense_qp_res_ws *ws); +// +void s_dense_qp_res_compute_lin(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_dense_qp_sol *qp_step, struct s_dense_qp_res *res, struct s_dense_qp_res_ws *ws); +// +void s_dense_qp_res_compute_inf_norm(struct s_dense_qp_res *res); +// +void s_dense_qp_res_get_all(struct s_dense_qp_res *res, float *res_g, float *res_ls, float *res_us, float *res_b, float *res_d_lb, float *res_d_ub, float *res_d_lg, float *res_d_ug, float *res_d_ls, float *res_d_us, float *res_m_lb, float *res_m_ub, float *res_m_lg, float *res_m_ug, float *res_m_ls, float *res_m_us); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + +#endif // HPIPM_D_DENSE_QP_RES_H_ + + + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_dense_qp_sol.h b/phonelibs/acados/include/hpipm/include/hpipm_s_dense_qp_sol.h new file mode 100644 index 0000000000..1f40076378 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_dense_qp_sol.h @@ -0,0 +1,94 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_S_DENSE_QP_SOL_H_ +#define HPIPM_S_DENSE_QP_SOL_H_ + + + +#include +#include + +#include "hpipm_s_dense_qp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_dense_qp_sol + { + struct s_dense_qp_dim *dim; + struct blasfeo_svec *v; + struct blasfeo_svec *pi; + struct blasfeo_svec *lam; + struct blasfeo_svec *t; + void *misc; + float obj; + int valid_obj; + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_dense_qp_sol_memsize(struct s_dense_qp_dim *dim); +// +void s_dense_qp_sol_create(struct s_dense_qp_dim *dim, struct s_dense_qp_sol *qp_sol, void *memory); +// +void s_dense_qp_sol_get_all(struct s_dense_qp_sol *qp_sol, float *v, float *ls, float *us, float *pi, float *lam_lb, float *lam_ub, float *lam_lg, float *lam_ug, float *lam_ls, float *lam_us); +// +void s_dense_qp_sol_get(char *field, struct s_dense_qp_sol *sol, void *value); +// +void s_dense_qp_sol_get_v(struct s_dense_qp_sol *sol, float *v); +// +void s_dense_qp_sol_get_valid_obj(struct s_dense_qp_sol *sol, int *valid_obj); +// +void s_dense_qp_sol_get_obj(struct s_dense_qp_sol *sol, float *obj); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_S_DENSE_QP_SOL_H_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_dense_qp_utils.h b/phonelibs/acados/include/hpipm/include/hpipm_s_dense_qp_utils.h new file mode 100644 index 0000000000..3dd93259a5 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_dense_qp_utils.h @@ -0,0 +1,84 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_DENSE_QP_UTILS_H_ +#define HPIPM_S_DENSE_QP_UTILS_H_ + + + +#include +#include + +#include "hpipm_s_dense_qp_dim.h" +#include "hpipm_s_dense_qp.h" +#include "hpipm_s_dense_qp_sol.h" +#include "hpipm_s_dense_qp_ipm.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +// +void s_dense_qp_dim_print(struct s_dense_qp_dim *qp_dim); +// +//void s_dense_qp_dim_codegen(char *file_name, char *mode, struct s_dense_qp_dim *qp_dim); +// +void s_dense_qp_print(struct s_dense_qp_dim *qp_dim, struct s_dense_qp *qp); +// +//void s_dense_qp_codegen(char *file_name, char *mode, struct s_dense_qp_dim *qp_dim, struct s_dense_qp *qp); +// +void s_dense_qp_sol_print(struct s_dense_qp_dim *qp_dim, struct s_dense_qp_sol *dense_qp_sol); +// +//void s_dense_qp_ipm_arg_codegen(char *file_name, char *mode, struct s_dense_qp_dim *qp_dim, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_res_print(struct s_dense_qp_dim *qp_dim, struct s_dense_qp_res *dense_qp_res); +// +void s_dense_qp_arg_print(struct s_dense_qp_dim *qp_dim, struct s_dense_qp_ipm_arg *qp_ipm_arg); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_S_DENSE_QP_UTILS_H_ + + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_ocp_qcqp.h b/phonelibs/acados/include/hpipm/include/hpipm_s_ocp_qcqp.h new file mode 100644 index 0000000000..b90b2ac633 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_ocp_qcqp.h @@ -0,0 +1,303 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_OCP_QCQP_H_ +#define HPIPM_S_OCP_QCQP_H_ + + + +#include +#include + +#include "hpipm_s_ocp_qcqp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_ocp_qcqp + { + struct s_ocp_qcqp_dim *dim; + struct blasfeo_smat *BAbt; // dynamics matrix & vector work space + struct blasfeo_smat *RSQrq; // hessian of cost & vector work space + struct blasfeo_smat *DCt; // inequality constraints matrix + struct blasfeo_smat **Hq; // hessians of quadratic constraints + struct blasfeo_svec *b; // dynamics vector + struct blasfeo_svec *rqz; // gradient of cost & gradient of slacks + struct blasfeo_svec *d; // inequality constraints vector + struct blasfeo_svec *d_mask; // inequality constraints mask vector + struct blasfeo_svec *m; // rhs of complementarity condition + struct blasfeo_svec *Z; // (diagonal) hessian of slacks + int **idxb; // indices of box constrained variables within [u; x] + int **idxs_rev; // index of soft constraints (reverse storage) + int **Hq_nzero; // for each int, the last 3 bits ...abc, {a,b,c}=0 => {R,S,Q}=0 + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +hpipm_size_t s_ocp_qcqp_strsize(); +// +hpipm_size_t s_ocp_qcqp_memsize(struct s_ocp_qcqp_dim *dim); +// +void s_ocp_qcqp_create(struct s_ocp_qcqp_dim *dim, struct s_ocp_qcqp *qp, void *memory); +// +void s_ocp_qcqp_copy_all(struct s_ocp_qcqp *qp_orig, struct s_ocp_qcqp *qp_dest); + +// setters +// +void s_ocp_qcqp_set_all_zero(struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_rhs_zero(struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set(char *fiels_name, int stage, void *value, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_el(char *fiels_name, int stage, int index, void *value, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_A(int stage, float *mat, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_B(int stage, float *mat, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_b(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_Q(int stage, float *mat, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_S(int stage, float *mat, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_R(int stage, float *mat, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_q(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_r(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_lb(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_lb_mask(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_ub(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_ub_mask(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_lbx(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_lbx_mask(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_el_lbx(int stage, int index, float *elem, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_ubx(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_ubx_mask(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_el_ubx(int stage, int index, float *elem, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_lbu(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_lbu_mask(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_ubu(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_ubu_mask(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_idxb(int stage, int *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_idxbx(int stage, int *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_Jbx(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_idxbu(int stage, int *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_Jbu(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_C(int stage, float *mat, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_D(int stage, float *mat, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_lg(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_lg_mask(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_ug(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_ug_mask(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_Qq(int stage, float *mat, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_Sq(int stage, float *mat, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_Rq(int stage, float *mat, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_qq(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_rq(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_uq(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_uq_mask(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_Zl(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_Zu(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_zl(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_zu(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_lls(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_lls_mask(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_lus(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_lus_mask(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_idxs(int stage, int *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_idxs_rev(int stage, int *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_Jsbu(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_Jsbx(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_Jsg(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_Jsq(int stage, float *vec, struct s_ocp_qcqp *qp); + +// getters +// +void s_ocp_qcqp_get(char *field, int stage, struct s_ocp_qcqp *qp, void *value); +// +void s_ocp_qcqp_get_A(int stage, struct s_ocp_qcqp *qp, float *mat); +// +void s_ocp_qcqp_get_B(int stage, struct s_ocp_qcqp *qp, float *mat); +// +void s_ocp_qcqp_get_b(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_Q(int stage, struct s_ocp_qcqp *qp, float *mat); +// +void s_ocp_qcqp_get_S(int stage, struct s_ocp_qcqp *qp, float *mat); +// +void s_ocp_qcqp_get_R(int stage, struct s_ocp_qcqp *qp, float *mat); +// +void s_ocp_qcqp_get_q(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_r(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_ub(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_ub_mask(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_lb(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_lb_mask(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_lbx(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_lbx_mask(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_ubx(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_ubx_mask(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_lbu(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_lbu_mask(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_ubu(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_ubu_mask(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_idxb(int stage, struct s_ocp_qcqp *qp, int *vec); +// +//void s_ocp_qcqp_get_idxbx(int stage, struct s_ocp_qcqp *qp, int *vec); +// +//void s_ocp_qcqp_get_Jbx(int stage, struct s_ocp_qcqp *qp, float *vec); +// +//void s_ocp_qcqp_get_idxbu(int stage, struct s_ocp_qcqp *qp, int *vec); +// +//void s_ocp_qcqp_get_Jbu(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_C(int stage, struct s_ocp_qcqp *qp, float *mat); +// +void s_ocp_qcqp_get_D(int stage, struct s_ocp_qcqp *qp, float *mat); +// +void s_ocp_qcqp_get_lg(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_lg_mask(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_ug(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_ug_mask(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_Zl(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_Zu(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_zl(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_zu(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_lls(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_lls_mask(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_lus(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_lus_mask(int stage, struct s_ocp_qcqp *qp, float *vec); +// XXX only valid if there is one slack per softed constraint !!! +void s_ocp_qcqp_get_idxs(int stage, struct s_ocp_qcqp *qp, int *vec); +// +void s_ocp_qcqp_get_idxs_rev(int stage, struct s_ocp_qcqp *qp, int *vec); +// +//void s_ocp_qcqp_get_Jsbu(int stage, struct s_ocp_qcqp *qp, float *vec); +// +//void s_ocp_qcqp_get_Jsbx(int stage, struct s_ocp_qcqp *qp, float *vec); +// +//void s_ocp_qcqp_get_Jsg(int stage, struct s_ocp_qcqp *qp, float *vec); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_S_OCP_QCQP_H_ + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_ocp_qcqp_dim.h b/phonelibs/acados/include/hpipm/include/hpipm_s_ocp_qcqp_dim.h new file mode 100644 index 0000000000..c09903f074 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_ocp_qcqp_dim.h @@ -0,0 +1,119 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_OCP_QCQP_DIM_H_ +#define HPIPM_S_OCP_QCQP_DIM_H_ + +#include "hpipm_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_ocp_qcqp_dim + { + struct s_ocp_qp_dim *qp_dim; // dim of qp approximation + int *nx; // number of states + int *nu; // number of inputs + int *nb; // number of box constraints + int *nbx; // number of (two-sided) state box constraints + int *nbu; // number of (two-sided) input box constraints + int *ng; // number of (two-sided) general constraints + int *nq; // number of (upper) quadratic constraints + int *ns; // number of soft constraints + int *nsbx; // number of (two-sided) soft state box constraints + int *nsbu; // number of (two-sided) soft input box constraints + int *nsg; // number of (two-sided) soft general constraints + int *nsq; // number of (upper) soft quadratic constraints + int N; // horizon length + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_ocp_qcqp_dim_strsize(); +// +hpipm_size_t s_ocp_qcqp_dim_memsize(int N); +// +void s_ocp_qcqp_dim_create(int N, struct s_ocp_qcqp_dim *qp_dim, void *memory); +// +void s_ocp_qcqp_dim_copy_all(struct s_ocp_qcqp_dim *dim_orig, struct s_ocp_qcqp_dim *dim_dest); +// +void s_ocp_qcqp_dim_set(char *field, int stage, int value, struct s_ocp_qcqp_dim *dim); +// +void s_ocp_qcqp_dim_set_nx(int stage, int value, struct s_ocp_qcqp_dim *dim); +// +void s_ocp_qcqp_dim_set_nu(int stage, int value, struct s_ocp_qcqp_dim *dim); +// +void s_ocp_qcqp_dim_set_nbx(int stage, int value, struct s_ocp_qcqp_dim *dim); +// +void s_ocp_qcqp_dim_set_nbu(int stage, int value, struct s_ocp_qcqp_dim *dim); +// +void s_ocp_qcqp_dim_set_ng(int stage, int value, struct s_ocp_qcqp_dim *dim); +// +void s_ocp_qcqp_dim_set_nq(int stage, int value, struct s_ocp_qcqp_dim *dim); +// +void s_ocp_qcqp_dim_set_ns(int stage, int value, struct s_ocp_qcqp_dim *dim); +// +void s_ocp_qcqp_dim_set_nsbx(int stage, int value, struct s_ocp_qcqp_dim *dim); +// +void s_ocp_qcqp_dim_set_nsbu(int stage, int value, struct s_ocp_qcqp_dim *dim); +// +void s_ocp_qcqp_dim_set_nsg(int stage, int value, struct s_ocp_qcqp_dim *dim); +// +void s_ocp_qcqp_dim_set_nsq(int stage, int value, struct s_ocp_qcqp_dim *dim); +// +void s_ocp_qcqp_dim_get(struct s_ocp_qcqp_dim *dim, char *field, int stage, int *value); +// +void s_ocp_qcqp_dim_get_N(struct s_ocp_qcqp_dim *dim, int *value); +// +void s_ocp_qcqp_dim_get_nx(struct s_ocp_qcqp_dim *dim, int stage, int *value); +// +void s_ocp_qcqp_dim_get_nu(struct s_ocp_qcqp_dim *dim, int stage, int *value); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_S_OCP_QCQP_DIM_H_ + + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_ocp_qcqp_ipm.h b/phonelibs/acados/include/hpipm/include/hpipm_s_ocp_qcqp_ipm.h new file mode 100644 index 0000000000..c14fc1c9fd --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_ocp_qcqp_ipm.h @@ -0,0 +1,191 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_OCP_QCQP_IPM_H_ +#define HPIPM_S_OCP_QCQP_IPM_H_ + + + +#include +#include + +#include +#include +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_ocp_qcqp_ipm_arg + { + struct s_ocp_qp_ipm_arg *qp_arg; + float mu0; // initial value for complementarity slackness + float alpha_min; // exit cond on step length + float res_g_max; // exit cond on inf norm of residuals + float res_b_max; // exit cond on inf norm of residuals + float res_d_max; // exit cond on inf norm of residuals + float res_m_max; // exit cond on inf norm of residuals + float reg_prim; // reg of primal hessian + float lam_min; // min value in lam vector + float t_min; // min value in t vector + int iter_max; // exit cond in iter number + int stat_max; // iterations saved in stat + int pred_corr; // use Mehrotra's predictor-corrector IPM algirthm + int cond_pred_corr; // conditional Mehrotra's predictor-corrector + int itref_pred_max; // max number of iterative refinement steps for predictor step + int itref_corr_max; // max number of iterative refinement steps for corrector step + int warm_start; // 0 no warm start, 1 warm start primal sol, 2 warm start primal and dual sol + int square_root_alg; // 0 classical Riccati, 1 square-root Riccati + int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq (for square_root_alg==1) + int abs_form; // absolute IPM formulation + int comp_dual_sol_eq; // dual solution of equality constraints (only for abs_form==1) + int comp_res_exit; // compute residuals on exit (only for abs_form==1 and comp_dual_sol_eq==1) + int comp_res_pred; // compute residuals of prediction + int split_step; // use different step for primal and dual variables + int t_lam_min; // clip t and lam: 0 no, 1 in Gamma computation, 2 in solution + int mode; + hpipm_size_t memsize; + }; + + + +struct s_ocp_qcqp_ipm_ws + { + struct s_ocp_qp_ipm_ws *qp_ws; + struct s_ocp_qp *qp; + struct s_ocp_qp_sol *qp_sol; + struct s_ocp_qcqp_res_ws *qcqp_res_ws; + struct s_ocp_qcqp_res *qcqp_res; + struct blasfeo_svec *tmp_nuxM; + int iter; // iteration number + int status; + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_ocp_qcqp_ipm_arg_strsize(); +// +hpipm_size_t s_ocp_qcqp_ipm_arg_memsize(struct s_ocp_qcqp_dim *ocp_dim); +// +void s_ocp_qcqp_ipm_arg_create(struct s_ocp_qcqp_dim *ocp_dim, struct s_ocp_qcqp_ipm_arg *arg, void *mem); +// +void s_ocp_qcqp_ipm_arg_set_default(enum hpipm_mode mode, struct s_ocp_qcqp_ipm_arg *arg); +// +void s_ocp_qcqp_ipm_arg_set(char *field, void *value, struct s_ocp_qcqp_ipm_arg *arg); +// set maximum number of iterations +void s_ocp_qcqp_ipm_arg_set_iter_max(int *value, struct s_ocp_qcqp_ipm_arg *arg); +// set minimum step lenght +void s_ocp_qcqp_ipm_arg_set_alpha_min(float *value, struct s_ocp_qcqp_ipm_arg *arg); +// set initial value of barrier parameter +void s_ocp_qcqp_ipm_arg_set_mu0(float *value, struct s_ocp_qcqp_ipm_arg *arg); +// set exit tolerance on stationarity condition +void s_ocp_qcqp_ipm_arg_set_tol_stat(float *value, struct s_ocp_qcqp_ipm_arg *arg); +// set exit tolerance on equality constr +void s_ocp_qcqp_ipm_arg_set_tol_eq(float *value, struct s_ocp_qcqp_ipm_arg *arg); +// set exit tolerance on inequality constr +void s_ocp_qcqp_ipm_arg_set_tol_ineq(float *value, struct s_ocp_qcqp_ipm_arg *arg); +// set exit tolerance on complementarity condition +void s_ocp_qcqp_ipm_arg_set_tol_comp(float *value, struct s_ocp_qcqp_ipm_arg *arg); +// set regularization of primal variables +void s_ocp_qcqp_ipm_arg_set_reg_prim(float *value, struct s_ocp_qcqp_ipm_arg *arg); +// set warm start: 0 no warm start, 1 primal var +void s_ocp_qcqp_ipm_arg_set_warm_start(int *value, struct s_ocp_qcqp_ipm_arg *arg); +// Mehrotra's predictor-corrector IPM algorithm: 0 no predictor-corrector, 1 use predictor-corrector +void s_ocp_qcqp_ipm_arg_set_pred_corr(int *value, struct s_ocp_qcqp_ipm_arg *arg); +// conditional predictor-corrector: 0 no conditinal predictor-corrector, 1 conditional predictor-corrector +void s_ocp_qcqp_ipm_arg_set_cond_pred_corr(int *value, struct s_ocp_qcqp_ipm_arg *arg); +// set riccati algorithm: 0 classic, 1 square-root +void s_ocp_qcqp_ipm_arg_set_ric_alg(int *value, struct s_ocp_qcqp_ipm_arg *arg); +// compute residuals after solution +void s_ocp_qcqp_ipm_arg_set_comp_res_exit(int *value, struct s_ocp_qcqp_ipm_arg *arg); +// compute residuals of prediction +void s_ocp_qcqp_ipm_arg_set_comp_res_pred(int *value, struct s_ocp_qcqp_ipm_arg *arg); +// min value of lam in the solution +void s_ocp_qcqp_ipm_arg_set_lam_min(float *value, struct s_ocp_qcqp_ipm_arg *arg); +// min value of t in the solution +void s_ocp_qcqp_ipm_arg_set_t_min(float *value, struct s_ocp_qcqp_ipm_arg *arg); +// use different step for primal and dual variables +void s_ocp_qcqp_ipm_arg_set_split_step(int *value, struct s_ocp_qcqp_ipm_arg *arg); +// clip t and lam: 0 no, 1 in Gamma computation, 2 in solution +void s_ocp_qcqp_ipm_arg_set_t_lam_min(int *value, struct s_ocp_qcqp_ipm_arg *arg); + +// +hpipm_size_t s_ocp_qcqp_ipm_ws_strsize(); +// +hpipm_size_t s_ocp_qcqp_ipm_ws_memsize(struct s_ocp_qcqp_dim *ocp_dim, struct s_ocp_qcqp_ipm_arg *arg); +// +void s_ocp_qcqp_ipm_ws_create(struct s_ocp_qcqp_dim *ocp_dim, struct s_ocp_qcqp_ipm_arg *arg, struct s_ocp_qcqp_ipm_ws *ws, void *mem); +// +void s_ocp_qcqp_ipm_get(char *field, struct s_ocp_qcqp_ipm_ws *ws, void *value); +// +void s_ocp_qcqp_ipm_get_status(struct s_ocp_qcqp_ipm_ws *ws, int *status); +// +void s_ocp_qcqp_ipm_get_iter(struct s_ocp_qcqp_ipm_ws *ws, int *iter); +// +void s_ocp_qcqp_ipm_get_max_res_stat(struct s_ocp_qcqp_ipm_ws *ws, float *res_stat); +// +void s_ocp_qcqp_ipm_get_max_res_eq(struct s_ocp_qcqp_ipm_ws *ws, float *res_eq); +// +void s_ocp_qcqp_ipm_get_max_res_ineq(struct s_ocp_qcqp_ipm_ws *ws, float *res_ineq); +// +void s_ocp_qcqp_ipm_get_max_res_comp(struct s_ocp_qcqp_ipm_ws *ws, float *res_comp); +// +void s_ocp_qcqp_ipm_get_stat(struct s_ocp_qcqp_ipm_ws *ws, float **stat); +// +void s_ocp_qcqp_ipm_get_stat_m(struct s_ocp_qcqp_ipm_ws *ws, int *stat_m); +// +void s_ocp_qcqp_init_var(struct s_ocp_qcqp *qp, struct s_ocp_qcqp_sol *qp_sol, struct s_ocp_qcqp_ipm_arg *arg, struct s_ocp_qcqp_ipm_ws *ws); +// +void s_ocp_qcqp_ipm_solve(struct s_ocp_qcqp *qp, struct s_ocp_qcqp_sol *qp_sol, struct s_ocp_qcqp_ipm_arg *arg, struct s_ocp_qcqp_ipm_ws *ws); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + +#endif // HPIPM_S_OCP_QCQP_IPM_H_ + + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_ocp_qcqp_res.h b/phonelibs/acados/include/hpipm/include/hpipm_s_ocp_qcqp_res.h new file mode 100644 index 0000000000..1ceeec93b7 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_ocp_qcqp_res.h @@ -0,0 +1,115 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_OCP_QCQP_RES_H_ +#define HPIPM_S_OCP_QCQP_RES_H_ + + + +#include +#include + +#include +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_ocp_qcqp_res + { + struct s_ocp_qcqp_dim *dim; + struct blasfeo_svec *res_g; // q-residuals + struct blasfeo_svec *res_b; // b-residuals + struct blasfeo_svec *res_d; // d-residuals + struct blasfeo_svec *res_m; // m-residuals + float res_max[4]; // max of residuals + float res_mu; // mu-residual + hpipm_size_t memsize; + }; + + + +struct s_ocp_qcqp_res_ws + { + struct blasfeo_svec *tmp_nuxM; // work space of size nuM+nxM + struct blasfeo_svec *tmp_nbgqM; // work space of size nbM+ngM+nqM + struct blasfeo_svec *tmp_nsM; // work space of size nsM + struct blasfeo_svec *q_fun; // value for evaluation of quadr constr + struct blasfeo_svec *q_adj; // value for adjoint of quadr constr + int use_q_fun; // reuse cached value for evaluation of quadr constr + int use_q_adj; // reuse cached value for adjoint of quadr constr + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_ocp_qcqp_res_memsize(struct s_ocp_qcqp_dim *ocp_dim); +// +void s_ocp_qcqp_res_create(struct s_ocp_qcqp_dim *ocp_dim, struct s_ocp_qcqp_res *res, void *mem); +// +hpipm_size_t s_ocp_qcqp_res_ws_memsize(struct s_ocp_qcqp_dim *ocp_dim); +// +void s_ocp_qcqp_res_ws_create(struct s_ocp_qcqp_dim *ocp_dim, struct s_ocp_qcqp_res_ws *workspace, void *mem); +// +void s_ocp_qcqp_res_compute(struct s_ocp_qcqp *qp, struct s_ocp_qcqp_sol *qp_sol, struct s_ocp_qcqp_res *res, struct s_ocp_qcqp_res_ws *ws); +// +void s_ocp_qcqp_res_compute_inf_norm(struct s_ocp_qcqp_res *res); +// +void s_ocp_qcqp_res_get_max_res_stat(struct s_ocp_qcqp_res *res, float *value); +// +void s_ocp_qcqp_res_get_max_res_eq(struct s_ocp_qcqp_res *res, float *value); +// +void s_ocp_qcqp_res_get_max_res_ineq(struct s_ocp_qcqp_res *res, float *value); +// +void s_ocp_qcqp_res_get_max_res_comp(struct s_ocp_qcqp_res *res, float *value); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + +#endif // HPIPM_S_OCP_QCQP_RES_H_ + + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_ocp_qcqp_sol.h b/phonelibs/acados/include/hpipm/include/hpipm_s_ocp_qcqp_sol.h new file mode 100644 index 0000000000..3d58022cc9 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_ocp_qcqp_sol.h @@ -0,0 +1,115 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_OCP_QCQP_SOL_H_ +#define HPIPM_S_OCP_QCQP_SOL_H_ + + + +#include +#include + +#include "hpipm_s_ocp_qcqp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_ocp_qcqp_sol + { + struct s_ocp_qcqp_dim *dim; + struct blasfeo_svec *ux; + struct blasfeo_svec *pi; + struct blasfeo_svec *lam; + struct blasfeo_svec *t; + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +hpipm_size_t s_ocp_qcqp_sol_strsize(); +// +hpipm_size_t s_ocp_qcqp_sol_memsize(struct s_ocp_qcqp_dim *dim); +// +void s_ocp_qcqp_sol_create(struct s_ocp_qcqp_dim *dim, struct s_ocp_qcqp_sol *qp_sol, void *memory); +// +void s_ocp_qcqp_sol_copy_all(struct s_ocp_qcqp_sol *qp_sol_orig, struct s_ocp_qcqp_sol *qp_sol_dest); +// +void s_ocp_qcqp_sol_get(char *field, int stage, struct s_ocp_qcqp_sol *qp_sol, float *vec); +// +void s_ocp_qcqp_sol_get_u(int stage, struct s_ocp_qcqp_sol *qp_sol, float *vec); +// +void s_ocp_qcqp_sol_get_x(int stage, struct s_ocp_qcqp_sol *qp_sol, float *vec); +// +void s_ocp_qcqp_sol_get_sl(int stage, struct s_ocp_qcqp_sol *qp_sol, float *vec); +// +void s_ocp_qcqp_sol_get_su(int stage, struct s_ocp_qcqp_sol *qp_sol, float *vec); +// +void s_ocp_qcqp_sol_get_pi(int stage, struct s_ocp_qcqp_sol *qp_sol, float *vec); +// +void s_ocp_qcqp_sol_get_lam_lb(int stage, struct s_ocp_qcqp_sol *qp_sol, float *vec); +// +void s_ocp_qcqp_sol_get_lam_ub(int stage, struct s_ocp_qcqp_sol *qp_sol, float *vec); +// +void s_ocp_qcqp_sol_get_lam_lg(int stage, struct s_ocp_qcqp_sol *qp_sol, float *vec); +// +void s_ocp_qcqp_sol_get_lam_ug(int stage, struct s_ocp_qcqp_sol *qp_sol, float *vec); +// +void s_ocp_qcqp_sol_set(char *field, int stage, float *vec, struct s_ocp_qcqp_sol *qp_sol); +// +void s_ocp_qcqp_sol_set_u(int stage, float *vec, struct s_ocp_qcqp_sol *qp_sol); +// +void s_ocp_qcqp_sol_set_x(int stage, float *vec, struct s_ocp_qcqp_sol *qp_sol); +// +void s_ocp_qcqp_sol_set_sl(int stage, float *vec, struct s_ocp_qcqp_sol *qp_sol); +// +void s_ocp_qcqp_sol_set_su(int stage, float *vec, struct s_ocp_qcqp_sol *qp_sol); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_S_OCP_QCQP_SOL_H_ + + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_ocp_qcqp_utils.h b/phonelibs/acados/include/hpipm/include/hpipm_s_ocp_qcqp_utils.h new file mode 100644 index 0000000000..d64e3aabe7 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_ocp_qcqp_utils.h @@ -0,0 +1,82 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_OCP_QCQP_UTILS_H_ +#define HPIPM_S_OCP_QCQP_UTILS_H_ + + + +#include +#include + +#include "hpipm_s_ocp_qcqp_dim.h" +#include "hpipm_s_ocp_qp.h" +#include "hpipm_s_ocp_qcqp_sol.h" +#include "hpipm_s_ocp_qcqp_ipm.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +// +void s_ocp_qcqp_dim_print(struct s_ocp_qcqp_dim *qcqp_dim); +// +void s_ocp_qcqp_dim_codegen(char *file_name, char *mode, struct s_ocp_qcqp_dim *qcqp_dim); +// +void s_ocp_qcqp_print(struct s_ocp_qcqp_dim *qcqp_dim, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_codegen(char *file_name, char *mode, struct s_ocp_qcqp_dim *qcqp_dim, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_sol_print(struct s_ocp_qcqp_dim *qcqp_dim, struct s_ocp_qcqp_sol *ocp_qcqp_sol); +// +void s_ocp_qcqp_ipm_arg_codegen(char *file_name, char *mode, struct s_ocp_qcqp_dim *qcqp_dim, struct s_ocp_qcqp_ipm_arg *arg); +// +void s_ocp_qcqp_res_print(struct s_ocp_qcqp_dim *qcqp_dim, struct s_ocp_qcqp_res *ocp_qcqp_res); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_S_OCP_QCQP_UTILS_H_ + + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_ocp_qp.h b/phonelibs/acados/include/hpipm/include/hpipm_s_ocp_qp.h new file mode 100644 index 0000000000..b49191f192 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_ocp_qp.h @@ -0,0 +1,306 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_OCP_QP_H_ +#define HPIPM_S_OCP_QP_H_ + + + +#include +#include + +#include "hpipm_s_ocp_qp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_ocp_qp + { + struct s_ocp_qp_dim *dim; + struct blasfeo_smat *BAbt; // dynamics matrix & vector work space + struct blasfeo_smat *RSQrq; // hessian of cost & vector work space + struct blasfeo_smat *DCt; // inequality constraints matrix + struct blasfeo_svec *b; // dynamics vector + struct blasfeo_svec *rqz; // gradient of cost & gradient of slacks + struct blasfeo_svec *d; // inequality constraints vector + struct blasfeo_svec *d_mask; // inequality constraints mask vector + struct blasfeo_svec *m; // rhs of complementarity condition + struct blasfeo_svec *Z; // (diagonal) hessian of slacks + int **idxb; // indices of box constrained variables within [u; x] + int **idxs_rev; // index of soft constraints (reverse storage) + int **idxe; // indices of constraints within [bu, bx, g] that are equalities, subset of [0, ..., nbu+nbx+ng-1] + int *diag_H_flag; // flag the fact that Hessian is diagonal + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +hpipm_size_t s_ocp_qp_strsize(); +// +hpipm_size_t s_ocp_qp_memsize(struct s_ocp_qp_dim *dim); +// +void s_ocp_qp_create(struct s_ocp_qp_dim *dim, struct s_ocp_qp *qp, void *memory); +// +void s_ocp_qp_copy_all(struct s_ocp_qp *qp_orig, struct s_ocp_qp *qp_dest); + +// setters +// +void s_ocp_qp_set_all_zero(struct s_ocp_qp *qp); +// +void s_ocp_qp_set_rhs_zero(struct s_ocp_qp *qp); +// +void s_ocp_qp_set_all(float **A, float **B, float **b, float **Q, float **S, float **R, float **q, float **r, int **idxbx, float **lbx, float **ubx, int **idxbu, float **lbu, float **ubu, float **C, float **D, float **lg, float **ug, float **Zl, float **Zu, float **zl, float **zu, int **idxs, float **ls, float **us, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_all_rowmaj(float **A, float **B, float **b, float **Q, float **S, float **R, float **q, float **r, int **idxbx, float **lbx, float **ubx, int **idxbu, float **lbu, float **ubu, float **C, float **D, float **lg, float **ug, float **Zl, float **Zu, float **zl, float **zu, int **idxs, float **ls, float **us, struct s_ocp_qp *qp); +// +void s_ocp_qp_set(char *fiels_name, int stage, void *value, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_el(char *fiels_name, int stage, int index, void *value, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_A(int stage, float *mat, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_B(int stage, float *mat, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_b(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_Q(int stage, float *mat, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_S(int stage, float *mat, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_R(int stage, float *mat, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_q(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_r(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_lb(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_lb_mask(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_ub(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_ub_mask(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_lbx(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_lbx_mask(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_el_lbx(int stage, int index, float *elem, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_ubx(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_ubx_mask(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_el_ubx(int stage, int index, float *elem, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_lbu(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_lbu_mask(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_ubu(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_ubu_mask(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_idxb(int stage, int *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_idxbx(int stage, int *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_Jbx(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_idxbu(int stage, int *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_Jbu(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_C(int stage, float *mat, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_D(int stage, float *mat, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_lg(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_lg_mask(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_ug(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_ug_mask(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_Zl(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_Zu(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_zl(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_zu(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_lls(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_lls_mask(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_lus(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_lus_mask(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_idxs(int stage, int *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_idxs_rev(int stage, int *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_Jsbu(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_Jsbx(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_Jsg(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_idxe(int stage, int *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_idxbxe(int stage, int *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_idxbue(int stage, int *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_idxge(int stage, int *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_Jbxe(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_Jbue(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_Jge(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_diag_H_flag(int stage, int *value, struct s_ocp_qp *qp); + +// getters +// +void s_ocp_qp_get(char *field, int stage, struct s_ocp_qp *qp, void *value); +// +void s_ocp_qp_get_A(int stage, struct s_ocp_qp *qp, float *mat); +// +void s_ocp_qp_get_B(int stage, struct s_ocp_qp *qp, float *mat); +// +void s_ocp_qp_get_b(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_Q(int stage, struct s_ocp_qp *qp, float *mat); +// +void s_ocp_qp_get_S(int stage, struct s_ocp_qp *qp, float *mat); +// +void s_ocp_qp_get_R(int stage, struct s_ocp_qp *qp, float *mat); +// +void s_ocp_qp_get_q(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_r(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_ub(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_ub_mask(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_lb(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_lb_mask(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_lbx(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_lbx_mask(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_ubx(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_ubx_mask(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_lbu(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_lbu_mask(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_ubu(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_ubu_mask(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_idxb(int stage, struct s_ocp_qp *qp, int *vec); +// +//void s_ocp_qp_get_idxbx(int stage, struct s_ocp_qp *qp, int *vec); +// +//void s_ocp_qp_get_Jbx(int stage, struct s_ocp_qp *qp, float *vec); +// +//void s_ocp_qp_get_idxbu(int stage, struct s_ocp_qp *qp, int *vec); +// +//void s_ocp_qp_get_Jbu(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_C(int stage, struct s_ocp_qp *qp, float *mat); +// +void s_ocp_qp_get_D(int stage, struct s_ocp_qp *qp, float *mat); +// +void s_ocp_qp_get_lg(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_lg_mask(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_ug(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_ug_mask(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_Zl(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_Zu(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_zl(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_zu(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_lls(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_lls_mask(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_lus(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_lus_mask(int stage, struct s_ocp_qp *qp, float *vec); +// XXX only valid if there is one slack per softed constraint !!! +void s_ocp_qp_get_idxs(int stage, struct s_ocp_qp *qp, int *vec); +// +void s_ocp_qp_get_idxs_rev(int stage, struct s_ocp_qp *qp, int *vec); +// +//void s_ocp_qp_get_Jsbu(int stage, struct s_ocp_qp *qp, float *vec); +// +//void s_ocp_qp_get_Jsbx(int stage, struct s_ocp_qp *qp, float *vec); +// +//void s_ocp_qp_get_Jsg(int stage, struct s_ocp_qp *qp, float *vec); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_S_OCP_QP_H_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_ocp_qp_dim.h b/phonelibs/acados/include/hpipm/include/hpipm_s_ocp_qp_dim.h new file mode 100644 index 0000000000..bce80243b3 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_ocp_qp_dim.h @@ -0,0 +1,141 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_OCP_QP_DIM_H_ +#define HPIPM_S_OCP_QP_DIM_H_ + +#include "hpipm_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_ocp_qp_dim + { + int *nx; // number of states + int *nu; // number of inputs + int *nb; // number of box constraints + int *nbx; // number of state box constraints + int *nbu; // number of input box constraints + int *ng; // number of general constraints + int *ns; // number of soft constraints + int *nsbx; // number of soft state box constraints + int *nsbu; // number of soft input box constraints + int *nsg; // number of soft general constraints + int *nbxe; // number of state box constraints which are equality + int *nbue; // number of input box constraints which are equality + int *nge; // number of general constraints which are equality + int N; // horizon length + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_ocp_qp_dim_strsize(); +// +hpipm_size_t s_ocp_qp_dim_memsize(int N); +// +void s_ocp_qp_dim_create(int N, struct s_ocp_qp_dim *qp_dim, void *memory); +// +void s_ocp_qp_dim_copy_all(struct s_ocp_qp_dim *dim_orig, struct s_ocp_qp_dim *dim_dest); +// +void s_ocp_qp_dim_set_all(int *nx, int *nu, int *nbx, int *nbu, int *ng, int *nsbx, int *nsbu, int *nsg, struct s_ocp_qp_dim *dim); +// +void s_ocp_qp_dim_set(char *field, int stage, int value, struct s_ocp_qp_dim *dim); +// +void s_ocp_qp_dim_set_nx(int stage, int value, struct s_ocp_qp_dim *dim); +// +void s_ocp_qp_dim_set_nu(int stage, int value, struct s_ocp_qp_dim *dim); +// +void s_ocp_qp_dim_set_nbx(int stage, int value, struct s_ocp_qp_dim *dim); +// +void s_ocp_qp_dim_set_nbu(int stage, int value, struct s_ocp_qp_dim *dim); +// +void s_ocp_qp_dim_set_ng(int stage, int value, struct s_ocp_qp_dim *dim); +// +void s_ocp_qp_dim_set_ns(int stage, int value, struct s_ocp_qp_dim *dim); +// +void s_ocp_qp_dim_set_nsbx(int stage, int value, struct s_ocp_qp_dim *dim); +// +void s_ocp_qp_dim_set_nsbu(int stage, int value, struct s_ocp_qp_dim *dim); +// +void s_ocp_qp_dim_set_nsg(int stage, int value, struct s_ocp_qp_dim *dim); +// +void s_ocp_qp_dim_set_nbxe(int stage, int value, struct s_ocp_qp_dim *dim); +// +void s_ocp_qp_dim_set_nbue(int stage, int value, struct s_ocp_qp_dim *dim); +// +void s_ocp_qp_dim_set_nge(int stage, int value, struct s_ocp_qp_dim *dim); +// +void s_ocp_qp_dim_get(struct s_ocp_qp_dim *dim, char *field, int stage, int *value); +// +void s_ocp_qp_dim_get_N(struct s_ocp_qp_dim *dim, int *value); +// +void s_ocp_qp_dim_get_nx(struct s_ocp_qp_dim *dim, int stage, int *value); +// +void s_ocp_qp_dim_get_nu(struct s_ocp_qp_dim *dim, int stage, int *value); +// +void s_ocp_qp_dim_get_nbx(struct s_ocp_qp_dim *dim, int stage, int *value); +// +void s_ocp_qp_dim_get_nbu(struct s_ocp_qp_dim *dim, int stage, int *value); +// +void s_ocp_qp_dim_get_ng(struct s_ocp_qp_dim *dim, int stage, int *value); +// +void s_ocp_qp_dim_get_ns(struct s_ocp_qp_dim *dim, int stage, int *value); +// +void s_ocp_qp_dim_get_nsbx(struct s_ocp_qp_dim *dim, int stage, int *value); +// +void s_ocp_qp_dim_get_nsbu(struct s_ocp_qp_dim *dim, int stage, int *value); +// +void s_ocp_qp_dim_get_nsg(struct s_ocp_qp_dim *dim, int stage, int *value); +// +void s_ocp_qp_dim_get_nbxe(struct s_ocp_qp_dim *dim, int stage, int *value); +// +void s_ocp_qp_dim_get_nbue(struct s_ocp_qp_dim *dim, int stage, int *value); +// +void s_ocp_qp_dim_get_nge(struct s_ocp_qp_dim *dim, int stage, int *value); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_S_OCP_QP_DIM_H_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_ocp_qp_ipm.h b/phonelibs/acados/include/hpipm/include/hpipm_s_ocp_qp_ipm.h new file mode 100644 index 0000000000..11f3c47be6 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_ocp_qp_ipm.h @@ -0,0 +1,250 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_OCP_QP_IPM_H_ +#define HPIPM_S_OCP_QP_IPM_H_ + + + +#include +#include + +#include +#include +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_ocp_qp_ipm_arg + { + float mu0; // initial value for complementarity slackness + float alpha_min; // exit cond on step length + float res_g_max; // exit cond on inf norm of residuals + float res_b_max; // exit cond on inf norm of residuals + float res_d_max; // exit cond on inf norm of residuals + float res_m_max; // exit cond on inf norm of residuals + float reg_prim; // reg of primal hessian + float lam_min; // min value in lam vector + float t_min; // min value in t vector + float tau_min; // min value of barrier parameter + int iter_max; // exit cond in iter number + int stat_max; // iterations saved in stat + int pred_corr; // use Mehrotra's predictor-corrector IPM algirthm + int cond_pred_corr; // conditional Mehrotra's predictor-corrector + int itref_pred_max; // max number of iterative refinement steps for predictor step + int itref_corr_max; // max number of iterative refinement steps for corrector step + int warm_start; // 0 no warm start, 1 warm start primal sol, 2 warm start primal and dual sol + int square_root_alg; // 0 classical Riccati, 1 square-root Riccati + int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq (for square_root_alg==1) + int abs_form; // absolute IPM formulation + int comp_dual_sol_eq; // dual solution of equality constraints (only for abs_form==1) + int comp_res_exit; // compute residuals on exit (only for abs_form==1 and comp_dual_sol_eq==1) + int comp_res_pred; // compute residuals of prediction + int split_step; // use different steps for primal and dual variables + int var_init_scheme; // variables initialization scheme + int t_lam_min; // clip t and lam: 0 no, 1 in Gamma computation, 2 in solution + int mode; + hpipm_size_t memsize; + }; + + + +struct s_ocp_qp_ipm_ws + { + struct s_core_qp_ipm_workspace *core_workspace; + struct s_ocp_qp_res_ws *res_workspace; + struct s_ocp_qp_dim *dim; // cache dim + struct s_ocp_qp_sol *sol_step; + struct s_ocp_qp_sol *sol_itref; + struct s_ocp_qp *qp_step; + struct s_ocp_qp *qp_itref; + struct s_ocp_qp_res *res; + struct s_ocp_qp_res *res_itref; + struct blasfeo_svec *Gamma; // hessian update + struct blasfeo_svec *gamma; // hessian update + struct blasfeo_svec *tmp_nuxM; // work space of size nxM + struct blasfeo_svec *tmp_nbgM; // work space of size nbM+ngM + struct blasfeo_svec *tmp_nsM; // work space of size nsM + struct blasfeo_svec *Pb; // Pb + struct blasfeo_svec *Zs_inv; + struct blasfeo_svec *tmp_m; + struct blasfeo_svec *l; // cache linear part for _get_ric_xxx + struct blasfeo_smat *L; + struct blasfeo_smat *Ls; + struct blasfeo_smat *P; + struct blasfeo_smat *Lh; + struct blasfeo_smat *AL; + struct blasfeo_smat *lq0; + struct blasfeo_smat *tmp_nxM_nxM; + float *stat; // convergence statistics + int *use_hess_fact; + void *lq_work0; + float qp_res[4]; // infinity norm of residuals + int iter; // iteration number + int stat_max; // iterations saved in stat + int stat_m; // number of recorded stat per IPM iter + int use_Pb; + int status; // solver status + int square_root_alg; // cache from arg + int lq_fact; // cache from arg + int mask_constr; // use constr mask + int valid_ric_vec; // meaningful riccati vectors + int valid_ric_p; // form of riccati p: 0 p*inv(L), 1 p + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_ocp_qp_ipm_arg_strsize(); +// +hpipm_size_t s_ocp_qp_ipm_arg_memsize(struct s_ocp_qp_dim *ocp_dim); +// +void s_ocp_qp_ipm_arg_create(struct s_ocp_qp_dim *ocp_dim, struct s_ocp_qp_ipm_arg *arg, void *mem); +// +void s_ocp_qp_ipm_arg_set_default(enum hpipm_mode mode, struct s_ocp_qp_ipm_arg *arg); +// +void s_ocp_qp_ipm_arg_set(char *field, void *value, struct s_ocp_qp_ipm_arg *arg); +// set maximum number of iterations +void s_ocp_qp_ipm_arg_set_iter_max(int *iter_max, struct s_ocp_qp_ipm_arg *arg); +// set minimum step lenght +void s_ocp_qp_ipm_arg_set_alpha_min(float *alpha_min, struct s_ocp_qp_ipm_arg *arg); +// set initial value of barrier parameter +void s_ocp_qp_ipm_arg_set_mu0(float *mu0, struct s_ocp_qp_ipm_arg *arg); +// set exit tolerance on stationarity condition +void s_ocp_qp_ipm_arg_set_tol_stat(float *tol_stat, struct s_ocp_qp_ipm_arg *arg); +// set exit tolerance on equality constr +void s_ocp_qp_ipm_arg_set_tol_eq(float *tol_eq, struct s_ocp_qp_ipm_arg *arg); +// set exit tolerance on inequality constr +void s_ocp_qp_ipm_arg_set_tol_ineq(float *tol_ineq, struct s_ocp_qp_ipm_arg *arg); +// set exit tolerance on complementarity condition +void s_ocp_qp_ipm_arg_set_tol_comp(float *tol_comp, struct s_ocp_qp_ipm_arg *arg); +// set regularization of primal variables +void s_ocp_qp_ipm_arg_set_reg_prim(float *tol_comp, struct s_ocp_qp_ipm_arg *arg); +// set warm start: 0 no warm start, 1 primal var +void s_ocp_qp_ipm_arg_set_warm_start(int *warm_start, struct s_ocp_qp_ipm_arg *arg); +// Mehrotra's predictor-corrector IPM algorithm: 0 no predictor-corrector, 1 use predictor-corrector +void s_ocp_qp_ipm_arg_set_pred_corr(int *pred_corr, struct s_ocp_qp_ipm_arg *arg); +// conditional predictor-corrector: 0 no conditinal predictor-corrector, 1 conditional predictor-corrector +void s_ocp_qp_ipm_arg_set_cond_pred_corr(int *value, struct s_ocp_qp_ipm_arg *arg); +// set riccati algorithm: 0 classic, 1 square-root +void s_ocp_qp_ipm_arg_set_ric_alg(int *alg, struct s_ocp_qp_ipm_arg *arg); +// dual solution of equality constraints (only for abs_form==1) +void s_ocp_qp_ipm_arg_set_comp_dual_sol_eq(int *value, struct s_ocp_qp_ipm_arg *arg); +// compute residuals after solution +void s_ocp_qp_ipm_arg_set_comp_res_exit(int *value, struct s_ocp_qp_ipm_arg *arg); +// compute residuals of prediction +void s_ocp_qp_ipm_arg_set_comp_res_pred(int *alg, struct s_ocp_qp_ipm_arg *arg); +// min value of lam in the solution +void s_ocp_qp_ipm_arg_set_lam_min(float *value, struct s_ocp_qp_ipm_arg *arg); +// min value of t in the solution +void s_ocp_qp_ipm_arg_set_t_min(float *value, struct s_ocp_qp_ipm_arg *arg); +// min value of tau in the solution +void s_ocp_qp_ipm_arg_set_tau_min(float *value, struct s_ocp_qp_ipm_arg *arg); +// set split step: 0 same step, 1 different step for primal and dual variables +void s_ocp_qp_ipm_arg_set_split_step(int *value, struct s_ocp_qp_ipm_arg *arg); +// variables initialization scheme +void s_ocp_qp_ipm_arg_set_var_init_scheme(int *value, struct s_ocp_qp_ipm_arg *arg); +// clip t and lam: 0 no, 1 in Gamma computation, 2 in solution +void s_ocp_qp_ipm_arg_set_t_lam_min(int *value, struct s_ocp_qp_ipm_arg *arg); + +// +hpipm_size_t s_ocp_qp_ipm_ws_strsize(); +// +hpipm_size_t s_ocp_qp_ipm_ws_memsize(struct s_ocp_qp_dim *ocp_dim, struct s_ocp_qp_ipm_arg *arg); +// +void s_ocp_qp_ipm_ws_create(struct s_ocp_qp_dim *ocp_dim, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws, void *mem); +// +void s_ocp_qp_ipm_get(char *field, struct s_ocp_qp_ipm_ws *ws, void *value); +// +void s_ocp_qp_ipm_get_status(struct s_ocp_qp_ipm_ws *ws, int *status); +// +void s_ocp_qp_ipm_get_iter(struct s_ocp_qp_ipm_ws *ws, int *iter); +// +void s_ocp_qp_ipm_get_max_res_stat(struct s_ocp_qp_ipm_ws *ws, float *res_stat); +// +void s_ocp_qp_ipm_get_max_res_eq(struct s_ocp_qp_ipm_ws *ws, float *res_eq); +// +void s_ocp_qp_ipm_get_max_res_ineq(struct s_ocp_qp_ipm_ws *ws, float *res_ineq); +// +void s_ocp_qp_ipm_get_max_res_comp(struct s_ocp_qp_ipm_ws *ws, float *res_comp); +// +void s_ocp_qp_ipm_get_stat(struct s_ocp_qp_ipm_ws *ws, float **stat); +// +void s_ocp_qp_ipm_get_stat_m(struct s_ocp_qp_ipm_ws *ws, int *stat_m); +// +void s_ocp_qp_ipm_get_ric_Lr(struct s_ocp_qp *qp, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws, int stage, float *Lr); +// +void s_ocp_qp_ipm_get_ric_Ls(struct s_ocp_qp *qp, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws, int stage, float *Ls); +// +void s_ocp_qp_ipm_get_ric_P(struct s_ocp_qp *qp, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws, int stage, float *P); +// +void s_ocp_qp_ipm_get_ric_lr(struct s_ocp_qp *qp, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws, int stage, float *lr); +// +void s_ocp_qp_ipm_get_ric_p(struct s_ocp_qp *qp, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws, int stage, float *p); +// feedback control gain in the form u = K x + k +void s_ocp_qp_ipm_get_ric_K(struct s_ocp_qp *qp, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws, int stage, float *K); +// feedback control gain in the form u = K x + k +void s_ocp_qp_ipm_get_ric_k(struct s_ocp_qp *qp, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws, int stage, float *k); +// +void s_ocp_qp_init_var(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws); +// +void s_ocp_qp_ipm_abs_step(int kk, struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws); +// +void s_ocp_qp_ipm_delta_step(int kk, struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws); +// +void s_ocp_qp_ipm_solve(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws); +// +void s_ocp_qp_ipm_predict(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws); +// +void s_ocp_qp_ipm_sens(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_S_OCP_QP_IPM_H_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_ocp_qp_kkt.h b/phonelibs/acados/include/hpipm/include/hpipm_s_ocp_qp_kkt.h new file mode 100644 index 0000000000..3eb1f4aae8 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_ocp_qp_kkt.h @@ -0,0 +1,66 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + +#ifndef HPIPM_S_OCP_QP_KKT_H_ +#define HPIPM_S_OCP_QP_KKT_H_ + +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + +// +void s_ocp_qp_fact_solve_kkt_unconstr(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws); +// +void s_ocp_qp_fact_solve_kkt_step(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws); +// +void s_ocp_qp_fact_lq_solve_kkt_step(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws); +// +void s_ocp_qp_solve_kkt_step(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + +#endif // HPIPM_S_OCP_QP_KKT_H_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_ocp_qp_red.h b/phonelibs/acados/include/hpipm/include/hpipm_s_ocp_qp_red.h new file mode 100644 index 0000000000..5a7b3b0703 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_ocp_qp_red.h @@ -0,0 +1,118 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_OCP_QP_RED_H_ +#define HPIPM_S_OCP_QP_RED_H_ + + + +#include +#include + +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_ocp_qp_reduce_eq_dof_arg + { + float lam_min; + float t_min; + int alias_unchanged; // do not keep copy unchanged stage + int comp_prim_sol; // primal solution (v) + int comp_dual_sol_eq; // dual solution equality constr (pi) + int comp_dual_sol_ineq; // dual solution inequality constr (lam t) + hpipm_size_t memsize; // memory size in bytes + }; + + + +struct s_ocp_qp_reduce_eq_dof_ws + { + struct blasfeo_svec *tmp_nuxM; + struct blasfeo_svec *tmp_nbgM; + int *e_imask_ux; + int *e_imask_d; + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +void s_ocp_qp_dim_reduce_eq_dof(struct s_ocp_qp_dim *dim, struct s_ocp_qp_dim *dim_red); +// +hpipm_size_t s_ocp_qp_reduce_eq_dof_arg_memsize(); +// +void s_ocp_qp_reduce_eq_dof_arg_create(struct s_ocp_qp_reduce_eq_dof_arg *arg, void *mem); +// +void s_ocp_qp_reduce_eq_dof_arg_set_default(struct s_ocp_qp_reduce_eq_dof_arg *arg); +// +void s_ocp_qp_reduce_eq_dof_arg_set_alias_unchanged(struct s_ocp_qp_reduce_eq_dof_arg *arg, int value); +// +void s_ocp_qp_reduce_eq_dof_arg_set_comp_prim_sol(struct s_ocp_qp_reduce_eq_dof_arg *arg, int value); +// +void s_ocp_qp_reduce_eq_dof_arg_set_comp_dual_sol_eq(struct s_ocp_qp_reduce_eq_dof_arg *arg, int value); +// +void s_ocp_qp_reduce_eq_dof_arg_set_comp_dual_sol_ineq(struct s_ocp_qp_reduce_eq_dof_arg *arg, int value); +// +hpipm_size_t s_ocp_qp_reduce_eq_dof_ws_memsize(struct s_ocp_qp_dim *dim); +// +void s_ocp_qp_reduce_eq_dof_ws_create(struct s_ocp_qp_dim *dim, struct s_ocp_qp_reduce_eq_dof_ws *work, void *mem); +// +void s_ocp_qp_reduce_eq_dof(struct s_ocp_qp *qp, struct s_ocp_qp *qp_red, struct s_ocp_qp_reduce_eq_dof_arg *arg, struct s_ocp_qp_reduce_eq_dof_ws *work); +// +void s_ocp_qp_reduce_eq_dof_lhs(struct s_ocp_qp *qp, struct s_ocp_qp *qp_red, struct s_ocp_qp_reduce_eq_dof_arg *arg, struct s_ocp_qp_reduce_eq_dof_ws *work); +// +void s_ocp_qp_reduce_eq_dof_rhs(struct s_ocp_qp *qp, struct s_ocp_qp *qp_red, struct s_ocp_qp_reduce_eq_dof_arg *arg, struct s_ocp_qp_reduce_eq_dof_ws *work); +// +void s_ocp_qp_restore_eq_dof(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol_red, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_reduce_eq_dof_arg *arg, struct s_ocp_qp_reduce_eq_dof_ws *work); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_S_OCP_QP_RED_H_ + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_ocp_qp_res.h b/phonelibs/acados/include/hpipm/include/hpipm_s_ocp_qp_res.h new file mode 100644 index 0000000000..821585da65 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_ocp_qp_res.h @@ -0,0 +1,114 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_OCP_QP_RES_H_ +#define HPIPM_S_OCP_QP_RES_H_ + + + +#include +#include + +#include +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_ocp_qp_res + { + struct s_ocp_qp_dim *dim; + struct blasfeo_svec *res_g; // q-residuals + struct blasfeo_svec *res_b; // b-residuals + struct blasfeo_svec *res_d; // d-residuals + struct blasfeo_svec *res_m; // m-residuals + float res_max[4]; // max of residuals + float res_mu; // mu-residual + hpipm_size_t memsize; + }; + + + +struct s_ocp_qp_res_ws + { + struct blasfeo_svec *tmp_nbgM; // work space of size nbM+ngM + struct blasfeo_svec *tmp_nsM; // work space of size nsM + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_ocp_qp_res_memsize(struct s_ocp_qp_dim *ocp_dim); +// +void s_ocp_qp_res_create(struct s_ocp_qp_dim *ocp_dim, struct s_ocp_qp_res *res, void *mem); +// +hpipm_size_t s_ocp_qp_res_ws_memsize(struct s_ocp_qp_dim *ocp_dim); +// +void s_ocp_qp_res_ws_create(struct s_ocp_qp_dim *ocp_dim, struct s_ocp_qp_res_ws *workspace, void *mem); +// +void s_ocp_qp_res_compute(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_res *res, struct s_ocp_qp_res_ws *ws); +// +void s_ocp_qp_res_compute_lin(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_sol *qp_step, struct s_ocp_qp_res *res, struct s_ocp_qp_res_ws *ws); +// +void s_ocp_qp_res_compute_inf_norm(struct s_ocp_qp_res *res); +// +void s_ocp_qp_res_get_all(struct s_ocp_qp_res *res, float **res_r, float **res_q, float **res_ls, float **res_us, float **res_b, float **res_d_lb, float **res_d_ub, float **res_d_lg, float **res_d_ug, float **res_d_ls, float **res_d_us, float **res_m_lb, float **res_m_ub, float **res_m_lg, float **res_m_ug, float **res_m_ls, float **res_m_us); +// +void s_ocp_qp_res_get_max_res_stat(struct s_ocp_qp_res *res, float *value); +// +void s_ocp_qp_res_get_max_res_eq(struct s_ocp_qp_res *res, float *value); +// +void s_ocp_qp_res_get_max_res_ineq(struct s_ocp_qp_res *res, float *value); +// +void s_ocp_qp_res_get_max_res_comp(struct s_ocp_qp_res *res, float *value); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + +#endif // HPIPM_S_OCP_QP_RES_H_ + + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_ocp_qp_sol.h b/phonelibs/acados/include/hpipm/include/hpipm_s_ocp_qp_sol.h new file mode 100644 index 0000000000..94dfa0d003 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_ocp_qp_sol.h @@ -0,0 +1,128 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_OCP_QP_SOL_H_ +#define HPIPM_S_OCP_QP_SOL_H_ + + + +#include +#include + +#include "hpipm_s_ocp_qp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_ocp_qp_sol + { + struct s_ocp_qp_dim *dim; + struct blasfeo_svec *ux; + struct blasfeo_svec *pi; + struct blasfeo_svec *lam; + struct blasfeo_svec *t; + void *misc; + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +hpipm_size_t s_ocp_qp_sol_strsize(); +// +hpipm_size_t s_ocp_qp_sol_memsize(struct s_ocp_qp_dim *dim); +// +void s_ocp_qp_sol_create(struct s_ocp_qp_dim *dim, struct s_ocp_qp_sol *qp_sol, void *memory); +// +void s_ocp_qp_sol_copy_all(struct s_ocp_qp_sol *qp_sol_orig, struct s_ocp_qp_sol *qp_sol_dest); +// +void s_qp_sol_get_all(struct s_ocp_qp_sol *qp_sol, float **u, float **x, float **ls, float **us, float **pi, float **lam_lb, float **lam_ub, float **lam_lg, float **lam_ug, float **lam_ls, float **lam_us); +// +void s_qp_sol_get_all_rowmaj(struct s_ocp_qp_sol *qp_sol, float **u, float **x, float **ls, float **us, float **pi, float **lam_lb, float **lam_ub, float **lam_lg, float **lam_ug, float **lam_ls, float **lam_us); +// +void s_ocp_qp_sol_set_all(float **u, float **x, float **ls, float **us, float **pi, float **lam_lb, float **lam_ub, float **lam_lg, float **lam_ug, float **lam_ls, float **lam_us, struct s_ocp_qp_sol *qp_sol); +// +void s_ocp_qp_sol_get(char *field, int stage, struct s_ocp_qp_sol *qp_sol, float *vec); +// +void s_ocp_qp_sol_get_u(int stage, struct s_ocp_qp_sol *qp_sol, float *vec); +// +void s_ocp_qp_sol_get_x(int stage, struct s_ocp_qp_sol *qp_sol, float *vec); +// +void s_ocp_qp_sol_get_sl(int stage, struct s_ocp_qp_sol *qp_sol, float *vec); +// +void s_ocp_qp_sol_get_su(int stage, struct s_ocp_qp_sol *qp_sol, float *vec); +// +void s_ocp_qp_sol_get_pi(int stage, struct s_ocp_qp_sol *qp_sol, float *vec); +// +void s_ocp_qp_sol_get_lam_lb(int stage, struct s_ocp_qp_sol *qp_sol, float *vec); +// +void s_ocp_qp_sol_get_lam_lbu(int stage, struct s_ocp_qp_sol *qp_sol, float *vec); +// +void s_ocp_qp_sol_get_lam_lbx(int stage, struct s_ocp_qp_sol *qp_sol, float *vec); +// +void s_ocp_qp_sol_get_lam_ub(int stage, struct s_ocp_qp_sol *qp_sol, float *vec); +// +void s_ocp_qp_sol_get_lam_ubu(int stage, struct s_ocp_qp_sol *qp_sol, float *vec); +// +void s_ocp_qp_sol_get_lam_ubx(int stage, struct s_ocp_qp_sol *qp_sol, float *vec); +// +void s_ocp_qp_sol_get_lam_lg(int stage, struct s_ocp_qp_sol *qp_sol, float *vec); +// +void s_ocp_qp_sol_get_lam_ug(int stage, struct s_ocp_qp_sol *qp_sol, float *vec); +// +void s_ocp_qp_sol_set(char *field, int stage, float *vec, struct s_ocp_qp_sol *qp_sol); +// +void s_ocp_qp_sol_set_u(int stage, float *vec, struct s_ocp_qp_sol *qp_sol); +// +void s_ocp_qp_sol_set_x(int stage, float *vec, struct s_ocp_qp_sol *qp_sol); +// +void s_ocp_qp_sol_set_sl(int stage, float *vec, struct s_ocp_qp_sol *qp_sol); +// +void s_ocp_qp_sol_set_su(int stage, float *vec, struct s_ocp_qp_sol *qp_sol); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_S_OCP_QP_SOL_H_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_ocp_qp_utils.h b/phonelibs/acados/include/hpipm/include/hpipm_s_ocp_qp_utils.h new file mode 100644 index 0000000000..a4f832a5ea --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_ocp_qp_utils.h @@ -0,0 +1,83 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_OCP_QP_UTILS_H_ +#define HPIPM_S_OCP_QP_UTILS_H_ + + + +#include +#include + +#include "hpipm_s_ocp_qp_dim.h" +#include "hpipm_s_ocp_qp.h" +#include "hpipm_s_ocp_qp_sol.h" +#include "hpipm_s_ocp_qp_ipm.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +// +void s_ocp_qp_dim_print(struct s_ocp_qp_dim *qp_dim); +// +void s_ocp_qp_dim_codegen(char *file_name, char *mode, struct s_ocp_qp_dim *qp_dim); +// +void s_ocp_qp_print(struct s_ocp_qp_dim *qp_dim, struct s_ocp_qp *qp); +// +void s_ocp_qp_codegen(char *file_name, char *mode, struct s_ocp_qp_dim *qp_dim, struct s_ocp_qp *qp); +// +void s_ocp_qp_sol_print(struct s_ocp_qp_dim *qp_dim, struct s_ocp_qp_sol *ocp_qp_sol); +// +void s_ocp_qp_ipm_arg_print(struct s_ocp_qp_dim *qp_dim, struct s_ocp_qp_ipm_arg *arg); +// +void s_ocp_qp_ipm_arg_codegen(char *file_name, char *mode, struct s_ocp_qp_dim *qp_dim, struct s_ocp_qp_ipm_arg *arg); +// +void s_ocp_qp_res_print(struct s_ocp_qp_dim *qp_dim, struct s_ocp_qp_res *ocp_qp_res); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_D_OCP_QP_UTILS_H_ + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_part_cond.h b/phonelibs/acados/include/hpipm/include/hpipm_s_part_cond.h new file mode 100644 index 0000000000..e40511e69f --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_part_cond.h @@ -0,0 +1,115 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_S_PART_COND_H_ +#define HPIPM_S_PART_COND_H_ + + + +#include +#include + +#include "hpipm_s_cond.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_part_cond_qp_arg + { + struct s_cond_qp_arg *cond_arg; + int N2; + hpipm_size_t memsize; + }; + + + +struct s_part_cond_qp_ws + { + struct s_cond_qp_ws *cond_workspace; + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_part_cond_qp_arg_memsize(int N2); +// +void s_part_cond_qp_arg_create(int N2, struct s_part_cond_qp_arg *cond_arg, void *mem); +// +void s_part_cond_qp_arg_set_default(struct s_part_cond_qp_arg *cond_arg); +// set riccati-like algorithm: 0 classical, 1 squre-root +void s_part_cond_qp_arg_set_ric_alg(int ric_alg, struct s_part_cond_qp_arg *cond_arg); +// +void s_part_cond_qp_arg_set_comp_prim_sol(int value, struct s_part_cond_qp_arg *cond_arg); +// +void s_part_cond_qp_arg_set_comp_dual_sol_eq(int value, struct s_part_cond_qp_arg *cond_arg); +// +void s_part_cond_qp_arg_set_comp_dual_sol_ineq(int value, struct s_part_cond_qp_arg *cond_arg); + +// +void s_part_cond_qp_compute_block_size(int N, int N2, int *block_size); +// +void s_part_cond_qp_compute_dim(struct s_ocp_qp_dim *ocp_dim, int *block_size, struct s_ocp_qp_dim *part_dense_dim); +// +hpipm_size_t s_part_cond_qp_ws_memsize(struct s_ocp_qp_dim *ocp_dim, int *block_size, struct s_ocp_qp_dim *part_dense_dim, struct s_part_cond_qp_arg *cond_arg); +// +void s_part_cond_qp_ws_create(struct s_ocp_qp_dim *ocp_dim, int *block_size, struct s_ocp_qp_dim *part_dense_dim, struct s_part_cond_qp_arg *cond_arg, struct s_part_cond_qp_ws *cond_ws, void *mem); +// +void s_part_cond_qp_cond(struct s_ocp_qp *ocp_qp, struct s_ocp_qp *part_dense_qp, struct s_part_cond_qp_arg *cond_arg, struct s_part_cond_qp_ws *cond_ws); +// +void s_part_cond_qp_cond_lhs(struct s_ocp_qp *ocp_qp, struct s_ocp_qp *part_dense_qp, struct s_part_cond_qp_arg *cond_arg, struct s_part_cond_qp_ws *cond_ws); +// +void s_part_cond_qp_cond_rhs(struct s_ocp_qp *ocp_qp, struct s_ocp_qp *part_dense_qp, struct s_part_cond_qp_arg *cond_arg, struct s_part_cond_qp_ws *cond_ws); +// +void s_part_cond_qp_expand_sol(struct s_ocp_qp *ocp_qp, struct s_ocp_qp *part_dense_qp, struct s_ocp_qp_sol *part_dense_qp_sol, struct s_ocp_qp_sol *ocp_qp_sol, struct s_part_cond_qp_arg *cond_arg, struct s_part_cond_qp_ws *cond_ws); + +// +void s_part_cond_qp_update(int *idxc, struct s_ocp_qp *ocp_qp, struct s_ocp_qp *part_dense_qp, struct s_part_cond_qp_arg *cond_arg, struct s_part_cond_qp_ws *cond_ws); + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_S_PART_COND_H_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_part_cond_qcqp.h b/phonelibs/acados/include/hpipm/include/hpipm_s_part_cond_qcqp.h new file mode 100644 index 0000000000..311f7000bf --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_part_cond_qcqp.h @@ -0,0 +1,107 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_S_PART_COND_QCQP_H_ +#define HPIPM_S_PART_COND_QCQP_H_ + + + +#include +#include + +#include "hpipm_s_cond_qcqp.h" + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_part_cond_qcqp_arg + { + struct s_cond_qcqp_arg *cond_arg; + int N2; + hpipm_size_t memsize; + }; + + + +struct s_part_cond_qcqp_ws + { + struct s_cond_qcqp_ws *cond_ws; + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_part_cond_qcqp_arg_memsize(int N2); +// +void s_part_cond_qcqp_arg_create(int N2, struct s_part_cond_qcqp_arg *cond_arg, void *mem); +// +void s_part_cond_qcqp_arg_set_default(struct s_part_cond_qcqp_arg *cond_arg); +// set riccati-like algorithm: 0 classical, 1 squre-root +void s_part_cond_qcqp_arg_set_ric_alg(int ric_alg, struct s_part_cond_qcqp_arg *cond_arg); + +// +void s_part_cond_qcqp_compute_block_size(int N, int N2, int *block_size); +// +void s_part_cond_qcqp_compute_dim(struct s_ocp_qcqp_dim *ocp_dim, int *block_size, struct s_ocp_qcqp_dim *part_dense_dim); +// +hpipm_size_t s_part_cond_qcqp_ws_memsize(struct s_ocp_qcqp_dim *ocp_dim, int *block_size, struct s_ocp_qcqp_dim *part_dense_dim, struct s_part_cond_qcqp_arg *cond_arg); +// +void s_part_cond_qcqp_ws_create(struct s_ocp_qcqp_dim *ocp_dim, int *block_size, struct s_ocp_qcqp_dim *part_dense_dim, struct s_part_cond_qcqp_arg *cond_arg, struct s_part_cond_qcqp_ws *cond_ws, void *mem); +// +void s_part_cond_qcqp_cond(struct s_ocp_qcqp *ocp_qp, struct s_ocp_qcqp *part_dense_qp, struct s_part_cond_qcqp_arg *cond_arg, struct s_part_cond_qcqp_ws *cond_ws); +// +void s_part_cond_qcqp_cond_lhs(struct s_ocp_qcqp *ocp_qp, struct s_ocp_qcqp *part_dense_qp, struct s_part_cond_qcqp_arg *cond_arg, struct s_part_cond_qcqp_ws *cond_ws); +// +void s_part_cond_qcqp_cond_rhs(struct s_ocp_qcqp *ocp_qp, struct s_ocp_qcqp *part_dense_qp, struct s_part_cond_qcqp_arg *cond_arg, struct s_part_cond_qcqp_ws *cond_ws); +// +void s_part_cond_qcqp_expand_sol(struct s_ocp_qcqp *ocp_qp, struct s_ocp_qcqp *part_dense_qp, struct s_ocp_qcqp_sol *part_dense_qp_sol, struct s_ocp_qcqp_sol *ocp_qp_sol, struct s_part_cond_qcqp_arg *cond_arg, struct s_part_cond_qcqp_ws *cond_ws); + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_D_PART_COND_H_ + + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_sim_erk.h b/phonelibs/acados/include/hpipm/include/hpipm_s_sim_erk.h new file mode 100644 index 0000000000..1a05ad1a28 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_sim_erk.h @@ -0,0 +1,121 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_SIM_ERK_H_ +#define HPIPM_S_SIM_ERK_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +struct s_sim_erk_arg + { + struct s_sim_rk_data *rk_data; // integrator data + float h; // step size + int steps; // number of steps +// int for_sens; // compute adjoint sensitivities +// int adj_sens; // compute adjoint sensitivities + hpipm_size_t memsize; + }; + + + +struct s_sim_erk_ws + { + void (*ode)(int t, float *x, float *p, void *ode_args, float *xdot); // function pointer to ode + void (*vde_for)(int t, float *x, float *p, void *ode_args, float *xdot); // function pointer to forward vde + void (*vde_adj)(int t, float *adj_in, void *ode_args, float *adj_out); // function pointer to adjoint vde + void *ode_args; // pointer to ode args + struct s_sim_erk_arg *erk_arg; // erk arg + float *K; // internal variables + float *x_for; // states and forward sensitivities + float *x_traj; // states at all steps + float *l; // adjoint sensitivities + float *p; // parameter + float *x_tmp; // temporary states and forward sensitivities + float *adj_in; + float *adj_tmp; + int nx; // number of states + int np; // number of parameters + int nf; // number of forward sensitivities + int na; // number of adjoint sensitivities + int nf_max; // max number of forward sensitivities + int na_max; // max number of adjoint sensitivities + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_sim_erk_arg_memsize(); +// +void s_sim_erk_arg_create(struct s_sim_erk_arg *erk_arg, void *mem); +// +void s_sim_erk_arg_set_all(struct s_sim_rk_data *rk_data, float h, int steps, struct s_sim_erk_arg *erk_arg); + +// +hpipm_size_t s_sim_erk_ws_memsize(struct s_sim_erk_arg *erk_arg, int nx, int np, int nf_max, int na_max); +// +void s_sim_erk_ws_create(struct s_sim_erk_arg *erk_arg, int nx, int np, int nf_max, int na_max, struct s_sim_erk_ws *work, void *memory); +// +void s_sim_erk_ws_set_all(int nf, int na, float *x, float *fs, float *bs, float *p, void (*ode)(int t, float *x, float *p, void *ode_args, float *xdot), void (*vde_for)(int t, float *x, float *p, void *ode_args, float *xdot), void (*vde_adj)(int t, float *adj_in, void *ode_args, float *adj_out), void *ode_args, struct s_sim_erk_ws *work); +// number of directions for forward sensitivities +void s_sim_erk_ws_set_nf(int *nf, struct s_sim_erk_ws *work); +// parameters (e.g. inputs) +void s_sim_erk_ws_set_p(float *p, struct s_sim_erk_ws *work); +// state +void s_sim_erk_ws_set_x(float *x, struct s_sim_erk_ws *work); +// forward sensitivities +void s_sim_erk_ws_set_fs(float *fs, struct s_sim_erk_ws *work); +// ode funtion +void s_sim_erk_ws_set_ode(void (*ode)(int t, float *x, float *p, void *ode_args, float *xdot), struct s_sim_erk_ws *work); +// forward vde function +void s_sim_erk_ws_set_vde_for(void (*ode)(int t, float *x, float *p, void *ode_args, float *xdot), struct s_sim_erk_ws *work); +// ode_args, passed straight to the ode/vde_for/vde_adj functions +void s_sim_erk_ws_set_ode_args(void *ode_args, struct s_sim_erk_ws *work); +// state +void s_sim_erk_ws_get_x(struct s_sim_erk_ws *work, float *x); +// forward sensitivities +void s_sim_erk_ws_get_fs(struct s_sim_erk_ws *work, float *fs); +// +void s_sim_erk_solve(struct s_sim_erk_arg *arg, struct s_sim_erk_ws *work); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // HPIPM_D_SIM_ERK_H_ + + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_sim_rk.h b/phonelibs/acados/include/hpipm/include/hpipm_s_sim_rk.h new file mode 100644 index 0000000000..53acd71498 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_sim_rk.h @@ -0,0 +1,72 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_SIM_RK_H_ +#define HPIPM_S_SIM_RK_H_ + +#include "hpipm_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +struct s_sim_rk_data + { + float *A_rk; // A in butcher tableau + float *B_rk; // b in butcher tableau + float *C_rk; // c in butcher tableau + int expl; // erk vs irk + int ns; // number of stages + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_sim_rk_data_memsize(int ns); +// +void s_sim_rk_data_create(int ns, struct s_sim_rk_data *rk_data, void *memory); +// +void s_sim_rk_data_init_default(char *field, struct s_sim_rk_data *rk_data); +// +void s_sim_rk_data_set_all(int expl, float *A_rk, float *B_rk, float *C_rk, struct s_sim_rk_data *rk_data); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // HPIPM_S_SIM_RK_H_ + + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp.h b/phonelibs/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp.h new file mode 100644 index 0000000000..450e992624 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp.h @@ -0,0 +1,213 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_TREE_OCP_QCQP_H_ +#define HPIPM_S_TREE_OCP_QCQP_H_ + + + +#include +#include + +#include "hpipm_s_tree_ocp_qcqp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_tree_ocp_qcqp + { + struct s_tree_ocp_qcqp_dim *dim; + struct blasfeo_smat *BAbt; // Nn-1 + struct blasfeo_smat *RSQrq; // Nn + struct blasfeo_smat *DCt; // Nn + struct blasfeo_smat **Hq; // Nn + struct blasfeo_svec *b; // Nn-1 + struct blasfeo_svec *rqz; // Nn + struct blasfeo_svec *d; // Nn + struct blasfeo_svec *d_mask; // Nn + struct blasfeo_svec *m; // Nn + struct blasfeo_svec *Z; // Nn + int **idxb; // indices of box constrained variables within [u; x] // Nn + int **idxs_rev; // index of soft constraints + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +hpipm_size_t s_tree_ocp_qcqp_memsize(struct s_tree_ocp_qcqp_dim *dim); +// +void s_tree_ocp_qcqp_create(struct s_tree_ocp_qcqp_dim *dim, struct s_tree_ocp_qcqp *qp, void *memory); +// +void s_tree_ocp_qcqp_set_all(float **A, float **B, float **b, float **Q, float **S, float **R, float **q, float **r, int **idxb, float **d_lb, float **d_ub, float **C, float **D, float **d_lg, float **d_ug, float **Zl, float **Zu, float **zl, float **zu, int **idxs, float **d_ls, float **d_us, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set(char *field_name, int node_edge, void *value, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_A(int edge, float *mat, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_B(int edge, float *mat, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_b(int edge, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_Q(int node, float *mat, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_S(int node, float *mat, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_R(int node, float *mat, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_q(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_r(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_lb(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_lb_mask(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_ub(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_ub_mask(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_lbx(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_lbx_mask(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_ubx(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_ubx_mask(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_lbu(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_lbu_mask(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_ubu(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_ubu_mask(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_idxb(int node, int *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_idxbx(int node, int *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_Jbx(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_idxbu(int node, int *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_Jbu(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_C(int node, float *mat, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_D(int node, float *mat, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_lg(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_lg_mask(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_ug(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_ug_mask(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_Qq(int node, float *mat, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_Sq(int node, float *mat, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_Rq(int node, float *mat, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_qq(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_rq(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_uq(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_uq_mask(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_Zl(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_Zu(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_zl(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_zu(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_lls(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_lls_mask(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_lus(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_lus_mask(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_idxs(int node, int *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_idxs_rev(int node, int *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_Jsbu(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_Jsbx(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_Jsg(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_Jsq(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +//void s_tree_ocp_qcqp_set_idxe(int node, int *vec, struct s_tree_ocp_qcqp *qp); +// +//void s_tree_ocp_qcqp_set_idxbxe(int node, int *vec, struct s_tree_ocp_qcqp *qp); +// +//void s_tree_ocp_qcqp_set_idxbue(int node, int *vec, struct s_tree_ocp_qcqp *qp); +// +//void s_tree_ocp_qcqp_set_idxge(int node, int *vec, struct s_tree_ocp_qcqp *qp); +// +//void s_tree_ocp_qcqp_set_Jbxe(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +//void s_tree_ocp_qcqp_set_Jbue(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +//void s_tree_ocp_qcqp_set_Jge(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +//void s_tree_ocp_qcqp_set_diag_H_flag(int node, int *value, struct s_tree_ocp_qcqp *qp); + + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_S_TREE_OCP_QCQP_H_ + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_dim.h b/phonelibs/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_dim.h new file mode 100644 index 0000000000..7a49d51995 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_dim.h @@ -0,0 +1,118 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_TREE_OCP_QCQP_DIM_H_ +#define HPIPM_S_TREE_OCP_QCQP_DIM_H_ + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_tree_ocp_qcqp_dim + { + struct s_tree_ocp_qp_dim *qp_dim; // dim of qp approximation + struct tree *ttree; // tree describing node conndection + int *nx; // number of states // Nn + int *nu; // number of inputs // Nn + int *nb; // number of box constraints // Nn + int *nbx; // number of state box constraints // Nn + int *nbu; // number of input box constraints // Nn + int *ng; // number of general constraints // Nn + int *nq; // number of (upper) quadratic constraints + int *ns; // number of soft constraints // Nn + int *nsbx; // number of soft state box constraints + int *nsbu; // number of soft input box constraints + int *nsg; // number of soft general constraints + int *nsq; // number of (upper) soft quadratic constraints + int Nn; // number of nodes + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_tree_ocp_qcqp_dim_strsize(); +// +hpipm_size_t s_tree_ocp_qcqp_dim_memsize(int Nn); +// +void s_tree_ocp_qcqp_dim_create(int Nn, struct s_tree_ocp_qcqp_dim *qp_dim, void *memory); +// +void s_tree_ocp_qcqp_dim_set_tree(struct tree *ttree, struct s_tree_ocp_qcqp_dim *dim); +// +void s_tree_ocp_qcqp_dim_set(char *field, int stage, int value, struct s_tree_ocp_qcqp_dim *dim); +// +void s_tree_ocp_qcqp_dim_set_nx(int stage, int value, struct s_tree_ocp_qcqp_dim *dim); +// +void s_tree_ocp_qcqp_dim_set_nu(int stage, int value, struct s_tree_ocp_qcqp_dim *dim); +// +void s_tree_ocp_qcqp_dim_set_nbx(int stage, int value, struct s_tree_ocp_qcqp_dim *dim); +// +void s_tree_ocp_qcqp_dim_set_nbu(int stage, int value, struct s_tree_ocp_qcqp_dim *dim); +// +void s_tree_ocp_qcqp_dim_set_ng(int stage, int value, struct s_tree_ocp_qcqp_dim *dim); +// +void s_tree_ocp_qcqp_dim_set_nq(int stage, int value, struct s_tree_ocp_qcqp_dim *dim); +// +void s_tree_ocp_qcqp_dim_set_ns(int stage, int value, struct s_tree_ocp_qcqp_dim *dim); +// +void s_tree_ocp_qcqp_dim_set_nsbx(int stage, int value, struct s_tree_ocp_qcqp_dim *dim); +// +void s_tree_ocp_qcqp_dim_set_nsbu(int stage, int value, struct s_tree_ocp_qcqp_dim *dim); +// +void s_tree_ocp_qcqp_dim_set_nsg(int stage, int value, struct s_tree_ocp_qcqp_dim *dim); +// +void s_tree_ocp_qcqp_dim_set_nsq(int stage, int value, struct s_tree_ocp_qcqp_dim *dim); +// +//void s_tree_ocp_qcqp_dim_set_nbxe(int stage, int value, struct s_tree_ocp_qcqp_dim *dim); +// +//void s_tree_ocp_qcqp_dim_set_nbue(int stage, int value, struct s_tree_ocp_qcqp_dim *dim); +// +//void s_tree_ocp_qcqp_dim_set_nge(int stage, int value, struct s_tree_ocp_qcqp_dim *dim); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_S_TREE_OCP_QCQP_DIM_H_ + + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_ipm.h b/phonelibs/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_ipm.h new file mode 100644 index 0000000000..69d65914c5 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_ipm.h @@ -0,0 +1,192 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_TREE_OCP_QCQP_IPM_H_ +#define HPIPM_S_TREE_OCP_QCQP_IPM_H_ + + + +#include +#include + +#include +#include +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_tree_ocp_qcqp_ipm_arg + { + struct s_tree_ocp_qp_ipm_arg *qp_arg; + float mu0; // initial value for complementarity slackness + float alpha_min; // exit cond on step length + float res_g_max; // exit cond on inf norm of residuals + float res_b_max; // exit cond on inf norm of residuals + float res_d_max; // exit cond on inf norm of residuals + float res_m_max; // exit cond on inf norm of residuals + float reg_prim; // reg of primal hessian + float lam_min; // min value in lam vector + float t_min; // min value in t vector + int iter_max; // exit cond in iter number + int stat_max; // iterations saved in stat + int pred_corr; // use Mehrotra's predictor-corrector IPM algirthm + int cond_pred_corr; // conditional Mehrotra's predictor-corrector + int itref_pred_max; // max number of iterative refinement steps for predictor step + int itref_corr_max; // max number of iterative refinement steps for corrector step + int warm_start; // 0 no warm start, 1 warm start primal sol, 2 warm start primal and dual sol +// int square_root_alg; // 0 classical Riccati, 1 square-root Riccati + int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq (for square_root_alg==1) + int abs_form; // absolute IPM formulation + int comp_dual_sol_eq; // dual solution of equality constraints (only for abs_form==1) + int comp_res_exit; // compute residuals on exit (only for abs_form==1 and comp_dual_sol_eq==1) +// int comp_res_pred; // compute residuals of prediction + int split_step; // use different step for primal and dual variables + int t_lam_min; // clip t and lam: 0 no, 1 in Gamma computation, 2 in solution + int mode; + hpipm_size_t memsize; + }; + + + +struct s_tree_ocp_qcqp_ipm_ws + { + struct s_tree_ocp_qp_ipm_ws *qp_ws; + struct s_tree_ocp_qp *qp; + struct s_tree_ocp_qp_sol *qp_sol; + struct s_tree_ocp_qcqp_res_ws *qcqp_res_ws; + struct s_tree_ocp_qcqp_res *qcqp_res; + struct blasfeo_svec *tmp_nuxM; + int iter; // iteration number + int status; + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_tree_ocp_qcqp_ipm_arg_strsize(); +// +hpipm_size_t s_tree_ocp_qcqp_ipm_arg_memsize(struct s_tree_ocp_qcqp_dim *ocp_dim); +// +void s_tree_ocp_qcqp_ipm_arg_create(struct s_tree_ocp_qcqp_dim *ocp_dim, struct s_tree_ocp_qcqp_ipm_arg *arg, void *mem); +// +void s_tree_ocp_qcqp_ipm_arg_set_default(enum hpipm_mode mode, struct s_tree_ocp_qcqp_ipm_arg *arg); +// +void s_tree_ocp_qcqp_ipm_arg_set(char *field, void *value, struct s_tree_ocp_qcqp_ipm_arg *arg); +// set maximum number of iterations +void s_tree_ocp_qcqp_ipm_arg_set_iter_max(int *value, struct s_tree_ocp_qcqp_ipm_arg *arg); +// set minimum step lenght +void s_tree_ocp_qcqp_ipm_arg_set_alpha_min(float *value, struct s_tree_ocp_qcqp_ipm_arg *arg); +// set initial value of barrier parameter +void s_tree_ocp_qcqp_ipm_arg_set_mu0(float *value, struct s_tree_ocp_qcqp_ipm_arg *arg); +// set exit tolerance on stationarity condition +void s_tree_ocp_qcqp_ipm_arg_set_tol_stat(float *value, struct s_tree_ocp_qcqp_ipm_arg *arg); +// set exit tolerance on equality constr +void s_tree_ocp_qcqp_ipm_arg_set_tol_eq(float *value, struct s_tree_ocp_qcqp_ipm_arg *arg); +// set exit tolerance on inequality constr +void s_tree_ocp_qcqp_ipm_arg_set_tol_ineq(float *value, struct s_tree_ocp_qcqp_ipm_arg *arg); +// set exit tolerance on complementarity condition +void s_tree_ocp_qcqp_ipm_arg_set_tol_comp(float *value, struct s_tree_ocp_qcqp_ipm_arg *arg); +// set regularization of primal variables +void s_tree_ocp_qcqp_ipm_arg_set_reg_prim(float *value, struct s_tree_ocp_qcqp_ipm_arg *arg); +// set warm start: 0 no warm start, 1 primal var +void s_tree_ocp_qcqp_ipm_arg_set_warm_start(int *value, struct s_tree_ocp_qcqp_ipm_arg *arg); +// Mehrotra's predictor-corrector IPM algorithm: 0 no predictor-corrector, 1 use predictor-corrector +void s_tree_ocp_qcqp_ipm_arg_set_pred_corr(int *value, struct s_tree_ocp_qcqp_ipm_arg *arg); +// conditional predictor-corrector: 0 no conditinal predictor-corrector, 1 conditional predictor-corrector +void s_tree_ocp_qcqp_ipm_arg_set_cond_pred_corr(int *value, struct s_tree_ocp_qcqp_ipm_arg *arg); +// set riccati algorithm: 0 classic, 1 square-root +//void s_tree_ocp_qcqp_ipm_arg_set_ric_alg(int *value, struct s_tree_ocp_qcqp_ipm_arg *arg); +// compute residuals after solution +void s_tree_ocp_qcqp_ipm_arg_set_comp_res_exit(int *value, struct s_tree_ocp_qcqp_ipm_arg *arg); +// compute residuals of prediction +//void s_tree_ocp_qcqp_ipm_arg_set_comp_res_pred(int *value, struct s_tree_ocp_qcqp_ipm_arg *arg); +// min value of lam in the solution +void s_tree_ocp_qcqp_ipm_arg_set_lam_min(float *value, struct s_tree_ocp_qcqp_ipm_arg *arg); +// min value of t in the solution +void s_tree_ocp_qcqp_ipm_arg_set_t_min(float *value, struct s_tree_ocp_qcqp_ipm_arg *arg); +// use different step for primal and dual variables +void s_tree_ocp_qcqp_ipm_arg_set_split_step(int *value, struct s_tree_ocp_qcqp_ipm_arg *arg); +// clip t and lam: 0 no, 1 in Gamma computation, 2 in solution +void s_tree_ocp_qcqp_ipm_arg_set_t_lam_min(int *value, struct s_tree_ocp_qcqp_ipm_arg *arg); + +// +hpipm_size_t s_tree_ocp_qcqp_ipm_ws_strsize(); +// +hpipm_size_t s_tree_ocp_qcqp_ipm_ws_memsize(struct s_tree_ocp_qcqp_dim *ocp_dim, struct s_tree_ocp_qcqp_ipm_arg *arg); +// +void s_tree_ocp_qcqp_ipm_ws_create(struct s_tree_ocp_qcqp_dim *ocp_dim, struct s_tree_ocp_qcqp_ipm_arg *arg, struct s_tree_ocp_qcqp_ipm_ws *ws, void *mem); +// +void s_tree_ocp_qcqp_ipm_get(char *field, struct s_tree_ocp_qcqp_ipm_ws *ws, void *value); +// +void s_tree_ocp_qcqp_ipm_get_status(struct s_tree_ocp_qcqp_ipm_ws *ws, int *status); +// +void s_tree_ocp_qcqp_ipm_get_iter(struct s_tree_ocp_qcqp_ipm_ws *ws, int *iter); +// +void s_tree_ocp_qcqp_ipm_get_max_res_stat(struct s_tree_ocp_qcqp_ipm_ws *ws, float *res_stat); +// +void s_tree_ocp_qcqp_ipm_get_max_res_eq(struct s_tree_ocp_qcqp_ipm_ws *ws, float *res_eq); +// +void s_tree_ocp_qcqp_ipm_get_max_res_ineq(struct s_tree_ocp_qcqp_ipm_ws *ws, float *res_ineq); +// +void s_tree_ocp_qcqp_ipm_get_max_res_comp(struct s_tree_ocp_qcqp_ipm_ws *ws, float *res_comp); +// +void s_tree_ocp_qcqp_ipm_get_stat(struct s_tree_ocp_qcqp_ipm_ws *ws, float **stat); +// +void s_tree_ocp_qcqp_ipm_get_stat_m(struct s_tree_ocp_qcqp_ipm_ws *ws, int *stat_m); +// +void s_tree_ocp_qcqp_init_var(struct s_tree_ocp_qcqp *qp, struct s_tree_ocp_qcqp_sol *qp_sol, struct s_tree_ocp_qcqp_ipm_arg *arg, struct s_tree_ocp_qcqp_ipm_ws *ws); +// +void s_tree_ocp_qcqp_ipm_solve(struct s_tree_ocp_qcqp *qp, struct s_tree_ocp_qcqp_sol *qp_sol, struct s_tree_ocp_qcqp_ipm_arg *arg, struct s_tree_ocp_qcqp_ipm_ws *ws); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + +#endif // HPIPM_S_TREE_OCP_QCQP_IPM_H_ + + + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_res.h b/phonelibs/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_res.h new file mode 100644 index 0000000000..d0d84a6f23 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_res.h @@ -0,0 +1,109 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_TREE_OCP_QCQP_RES_H_ +#define HPIPM_S_TREE_OCP_QCQP_RES_H_ + + + +#include +#include + +#include +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_tree_ocp_qcqp_res + { + struct s_tree_ocp_qcqp_dim *dim; + struct blasfeo_svec *res_g; // q-residuals + struct blasfeo_svec *res_b; // b-residuals + struct blasfeo_svec *res_d; // d-residuals + struct blasfeo_svec *res_m; // m-residuals + float res_max[4]; // max of residuals + float res_mu; // mu-residual + hpipm_size_t memsize; + }; + + + +struct s_tree_ocp_qcqp_res_ws + { + struct blasfeo_svec *tmp_nuxM; // work space of size nuM+nxM + struct blasfeo_svec *tmp_nbgqM; // work space of size nbM+ngM+nqM + struct blasfeo_svec *tmp_nsM; // work space of size nsM + struct blasfeo_svec *q_fun; // value for evaluation of quadr constr + struct blasfeo_svec *q_adj; // value for adjoint of quadr constr + int use_q_fun; // reuse cached value for evaluation of quadr constr + int use_q_adj; // reuse cached value for adjoint of quadr constr + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_tree_ocp_qcqp_res_memsize(struct s_tree_ocp_qcqp_dim *ocp_dim); +// +void s_tree_ocp_qcqp_res_create(struct s_tree_ocp_qcqp_dim *ocp_dim, struct s_tree_ocp_qcqp_res *res, void *mem); +// +hpipm_size_t s_tree_ocp_qcqp_res_ws_memsize(struct s_tree_ocp_qcqp_dim *ocp_dim); +// +void s_tree_ocp_qcqp_res_ws_create(struct s_tree_ocp_qcqp_dim *ocp_dim, struct s_tree_ocp_qcqp_res_ws *ws, void *mem); +// +void s_tree_ocp_qcqp_res_compute(struct s_tree_ocp_qcqp *qp, struct s_tree_ocp_qcqp_sol *qp_sol, struct s_tree_ocp_qcqp_res *res, struct s_tree_ocp_qcqp_res_ws *ws); +// +void s_tree_ocp_qcqp_res_compute_inf_norm(struct s_tree_ocp_qcqp_res *res); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + +#endif // HPIPM_S_TREE_OCP_QCQP_RES_H_ + + + + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_sol.h b/phonelibs/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_sol.h new file mode 100644 index 0000000000..47f038c6b3 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_sol.h @@ -0,0 +1,97 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_TREE_OCP_QCQP_SOL_H_ +#define HPIPM_S_TREE_OCP_QCQP_SOL_H_ + + + +#include +#include + +#include "hpipm_s_tree_ocp_qcqp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + +struct s_tree_ocp_qcqp_sol + { + struct s_tree_ocp_qcqp_dim *dim; + struct blasfeo_svec *ux; + struct blasfeo_svec *pi; + struct blasfeo_svec *lam; + struct blasfeo_svec *t; + void *misc; + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +hpipm_size_t s_tree_ocp_qcqp_sol_memsize(struct s_tree_ocp_qcqp_dim *dim); +// +void s_tree_ocp_qcqp_sol_create(struct s_tree_ocp_qcqp_dim *dim, struct s_tree_ocp_qcqp_sol *qp_sol, void *memory); +// +void s_tree_ocp_qcqp_sol_get_u(int node, struct s_tree_ocp_qcqp_sol *qp_sol, float *vec); +// +void s_tree_ocp_qcqp_sol_get_x(int node, struct s_tree_ocp_qcqp_sol *qp_sol, float *vec); +// +void s_tree_ocp_qcqp_sol_get_sl(int node, struct s_tree_ocp_qcqp_sol *qp_sol, float *vec); +// +void s_tree_ocp_qcqp_sol_get_su(int node, struct s_tree_ocp_qcqp_sol *qp_sol, float *vec); +// +void s_tree_ocp_qcqp_sol_get_pi(int edge, struct s_tree_ocp_qcqp_sol *qp_sol, float *vec); +// +void s_tree_ocp_qcqp_sol_get_lam_lb(int node, struct s_tree_ocp_qcqp_sol *qp_sol, float *vec); +// +void s_tree_ocp_qcqp_sol_get_lam_ub(int node, struct s_tree_ocp_qcqp_sol *qp_sol, float *vec); +// +void s_tree_ocp_qcqp_sol_get_lam_lg(int node, struct s_tree_ocp_qcqp_sol *qp_sol, float *vec); +// +void s_tree_ocp_qcqp_sol_get_lam_ug(int node, struct s_tree_ocp_qcqp_sol *qp_sol, float *vec); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_S_TREE_OCP_QCQP_SOL_H_ + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_utils.h b/phonelibs/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_utils.h new file mode 100644 index 0000000000..79528de1ce --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_utils.h @@ -0,0 +1,85 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_TREE_OCP_QCQP_UTILS_H_ +#define HPIPM_S_TREE_OCP_QCQP_UTILS_H_ + + + +#include +#include + +#include "hpipm_s_tree_ocp_qcqp_dim.h" +#include "hpipm_s_tree_ocp_qcqp.h" +#include "hpipm_s_tree_ocp_qcqp_sol.h" +#include "hpipm_s_tree_ocp_qcqp_ipm.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +// +void s_tree_ocp_qcqp_dim_print(struct s_tree_ocp_qcqp_dim *qp_dim); +// +//void s_tree_ocp_qcqp_dim_codegen(char *file_name, char *mode, struct s_tree_ocp_qcqp_dim *qp_dim); +// +void s_tree_ocp_qcqp_print(struct s_tree_ocp_qcqp_dim *qp_dim, struct s_tree_ocp_qcqp *qp); +// +//void s_tree_ocp_qcqp_codegen(char *file_name, char *mode, struct s_tree_ocp_qcqp_dim *qp_dim, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_sol_print(struct s_tree_ocp_qcqp_dim *qp_dim, struct s_tree_ocp_qcqp_sol *ocp_qcqp_sol); +// +void s_tree_ocp_qcqp_ipm_arg_print(struct s_tree_ocp_qcqp_dim *qp_dim, struct s_tree_ocp_qcqp_ipm_arg *arg); +// +//void s_tree_ocp_qcqp_ipm_arg_codegen(char *file_name, char *mode, struct s_tree_ocp_qcqp_dim *qp_dim, struct s_tree_ocp_qcqp_ipm_arg *arg); +// +void s_tree_ocp_qcqp_res_print(struct s_tree_ocp_qcqp_dim *qp_dim, struct s_tree_ocp_qcqp_res *ocp_qcqp_res); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_S_TREE_OCP_QCQP_UTILS_H_ + + + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_tree_ocp_qp.h b/phonelibs/acados/include/hpipm/include/hpipm_s_tree_ocp_qp.h new file mode 100644 index 0000000000..722b930b9a --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_tree_ocp_qp.h @@ -0,0 +1,196 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_TREE_OCP_QP_H_ +#define HPIPM_S_TREE_OCP_QP_H_ + + + +#include +#include + +#include "hpipm_s_tree_ocp_qp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_tree_ocp_qp + { + struct s_tree_ocp_qp_dim *dim; + struct blasfeo_smat *BAbt; // Nn-1 + struct blasfeo_smat *RSQrq; // Nn + struct blasfeo_smat *DCt; // Nn + struct blasfeo_svec *b; // Nn-1 + struct blasfeo_svec *rqz; // Nn + struct blasfeo_svec *d; // Nn + struct blasfeo_svec *d_mask; // Nn + struct blasfeo_svec *m; // Nn + struct blasfeo_svec *Z; // Nn + int **idxb; // indices of box constrained variables within [u; x] // Nn +// int **idxs; // index of soft constraints + int **idxs_rev; // index of soft constraints + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +hpipm_size_t s_tree_ocp_qp_memsize(struct s_tree_ocp_qp_dim *dim); +// +void s_tree_ocp_qp_create(struct s_tree_ocp_qp_dim *dim, struct s_tree_ocp_qp *qp, void *memory); +// +void s_tree_ocp_qp_set_all(float **A, float **B, float **b, float **Q, float **S, float **R, float **q, float **r, int **idxb, float **d_lb, float **d_ub, float **C, float **D, float **d_lg, float **d_ug, float **Zl, float **Zu, float **zl, float **zu, int **idxs, float **d_ls, float **d_us, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set(char *field_name, int node_edge, void *value, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_A(int edge, float *mat, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_B(int edge, float *mat, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_b(int edge, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_Q(int node, float *mat, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_S(int node, float *mat, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_R(int node, float *mat, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_q(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_r(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_lb(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_lb_mask(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_ub(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_ub_mask(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_lbx(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_lbx_mask(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_ubx(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_ubx_mask(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_lbu(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_lbu_mask(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_ubu(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_ubu_mask(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_idxb(int node, int *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_idxbx(int node, int *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_Jbx(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_idxbu(int node, int *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_Jbu(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_C(int node, float *mat, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_D(int node, float *mat, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_lg(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_lg_mask(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_ug(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_ug_mask(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_Zl(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_Zu(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_zl(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_zu(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_lls(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_lls_mask(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_lus(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_lus_mask(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_idxs(int node, int *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_idxs_rev(int node, int *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_Jsbu(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_Jsbx(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_Jsg(int node, float *vec, struct s_tree_ocp_qp *qp); +// +//void s_tree_ocp_qp_set_idxe(int node, int *vec, struct s_tree_ocp_qp *qp); +// +//void s_tree_ocp_qp_set_idxbxe(int node, int *vec, struct s_tree_ocp_qp *qp); +// +//void s_tree_ocp_qp_set_idxbue(int node, int *vec, struct s_tree_ocp_qp *qp); +// +//void s_tree_ocp_qp_set_idxge(int node, int *vec, struct s_tree_ocp_qp *qp); +// +//void s_tree_ocp_qp_set_Jbxe(int node, float *vec, struct s_tree_ocp_qp *qp); +// +//void s_tree_ocp_qp_set_Jbue(int node, float *vec, struct s_tree_ocp_qp *qp); +// +//void s_tree_ocp_qp_set_Jge(int node, float *vec, struct s_tree_ocp_qp *qp); +// +//void s_tree_ocp_qp_set_diag_H_flag(int node, int *value, struct s_tree_ocp_qp *qp); + + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_S_TREE_OCP_QP_H_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_dim.h b/phonelibs/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_dim.h new file mode 100644 index 0000000000..90df57182b --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_dim.h @@ -0,0 +1,111 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_TREE_OCP_QP_DIM_H_ +#define HPIPM_S_TREE_OCP_QP_DIM_H_ + +#include "hpipm_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_tree_ocp_qp_dim + { + struct tree *ttree; // tree describing node conndection + int *nx; // number of states // Nn + int *nu; // number of inputs // Nn + int *nb; // number of box constraints // Nn + int *nbx; // number of state box constraints // Nn + int *nbu; // number of input box constraints // Nn + int *ng; // number of general constraints // Nn + int *ns; // number of soft constraints // Nn + int *nsbx; // number of soft state box constraints + int *nsbu; // number of soft input box constraints + int *nsg; // number of soft general constraints + int Nn; // number of nodes + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_tree_ocp_qp_dim_strsize(); +// +hpipm_size_t s_tree_ocp_qp_dim_memsize(int Nn); +// +void s_tree_ocp_qp_dim_create(int Nn, struct s_tree_ocp_qp_dim *qp_dim, void *memory); +// +void s_tree_ocp_qp_dim_set_all(struct tree *ttree, int *nx, int *nu, int *nbx, int *nbu, int *ng, int *nsbx, int *nsbu, int *nsg, struct s_tree_ocp_qp_dim *dim); +// +void s_tree_ocp_qp_dim_set_tree(struct tree *ttree, struct s_tree_ocp_qp_dim *dim); +// +void s_tree_ocp_qp_dim_set(char *field, int stage, int value, struct s_tree_ocp_qp_dim *dim); +// +void s_tree_ocp_qp_dim_set_nx(int stage, int value, struct s_tree_ocp_qp_dim *dim); +// +void s_tree_ocp_qp_dim_set_nu(int stage, int value, struct s_tree_ocp_qp_dim *dim); +// +void s_tree_ocp_qp_dim_set_nbx(int stage, int value, struct s_tree_ocp_qp_dim *dim); +// +void s_tree_ocp_qp_dim_set_nbu(int stage, int value, struct s_tree_ocp_qp_dim *dim); +// +void s_tree_ocp_qp_dim_set_ng(int stage, int value, struct s_tree_ocp_qp_dim *dim); +// +void s_tree_ocp_qp_dim_set_ns(int stage, int value, struct s_tree_ocp_qp_dim *dim); +// +void s_tree_ocp_qp_dim_set_nsbx(int stage, int value, struct s_tree_ocp_qp_dim *dim); +// +void s_tree_ocp_qp_dim_set_nsbu(int stage, int value, struct s_tree_ocp_qp_dim *dim); +// +void s_tree_ocp_qp_dim_set_nsg(int stage, int value, struct s_tree_ocp_qp_dim *dim); +// +void s_tree_ocp_qp_dim_set_nbxe(int stage, int value, struct s_tree_ocp_qp_dim *dim); +// +void s_tree_ocp_qp_dim_set_nbue(int stage, int value, struct s_tree_ocp_qp_dim *dim); +// +void s_tree_ocp_qp_dim_set_nge(int stage, int value, struct s_tree_ocp_qp_dim *dim); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_S_TREE_OCP_QP_DIM_H_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_ipm.h b/phonelibs/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_ipm.h new file mode 100644 index 0000000000..f8c26d3173 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_ipm.h @@ -0,0 +1,208 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_S_TREE_OCP_QP_IPM_H_ +#define HPIPM_S_TREE_OCP_QP_IPM_H_ + + + +#include +#include + +#include +#include +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_tree_ocp_qp_ipm_arg + { + float mu0; // initial value for duality measure + float alpha_min; // exit cond on step length + float res_g_max; // exit cond on inf norm of residuals + float res_b_max; // exit cond on inf norm of residuals + float res_d_max; // exit cond on inf norm of residuals + float res_m_max; // exit cond on inf norm of residuals + float reg_prim; // reg of primal hessian + float lam_min; // min value in lam vector + float t_min; // min value in t vector + float tau_min; // min value of barrier parameter + int iter_max; // exit cond in iter number + int stat_max; // iterations saved in stat + int stat_m; // number of recorded stat per IPM iter + int pred_corr; // use Mehrotra's predictor-corrector IPM algirthm + int cond_pred_corr; // conditional Mehrotra's predictor-corrector + int itref_pred_max; // max number of iterative refinement steps for predictor step + int itref_corr_max; // max number of iterative refinement steps for corrector step + int warm_start; // 0 no warm start, 1 warm start primal sol, 2 warm start primal and dual sol + int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq + int abs_form; // absolute IPM formulation + int comp_dual_sol_eq; // dual solution (only for abs_form==1) + int comp_res_exit; // compute residuals on exit (only for abs_form==1 and comp_dual_sol==1) + int split_step; // use different steps for primal and dual variables + int t_lam_min; // clip t and lam: 0 no, 1 in Gamma computation, 2 in solution + int mode; + hpipm_size_t memsize; + }; + + + +struct s_tree_ocp_qp_ipm_ws + { + struct s_core_qp_ipm_workspace *core_workspace; + struct s_tree_ocp_qp_res_ws *res_workspace; + struct s_tree_ocp_qp_sol *sol_step; + struct s_tree_ocp_qp_sol *sol_itref; + struct s_tree_ocp_qp *qp_step; + struct s_tree_ocp_qp *qp_itref; + struct s_tree_ocp_qp_res *res_itref; + struct s_tree_ocp_qp_res *res; + struct blasfeo_svec *Gamma; // hessian update + struct blasfeo_svec *gamma; // hessian update + struct blasfeo_svec *tmp_nxM; // work space of size nxM + struct blasfeo_svec *tmp_nbgM; // work space of size nbgM + struct blasfeo_svec *tmp_nsM; // work space of size nsM + struct blasfeo_svec *Pb; // Pb + struct blasfeo_svec *Zs_inv; + struct blasfeo_smat *L; + struct blasfeo_smat *Lh; + struct blasfeo_smat *AL; + struct blasfeo_smat *lq0; + struct blasfeo_svec *tmp_m; + float *stat; // convergence statistics + int *use_hess_fact; + void *lq_work0; + float qp_res[4]; // infinity norm of residuals + int iter; // iteration number + int stat_max; // iterations saved in stat + int stat_m; // number of recorded stat per IPM iter + int use_Pb; + int status; // solver status + int lq_fact; // cache from arg + int mask_constr; // use constr mask + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_tree_ocp_qp_ipm_arg_memsize(struct s_tree_ocp_qp_dim *dim); +// +void s_tree_ocp_qp_ipm_arg_create(struct s_tree_ocp_qp_dim *dim, struct s_tree_ocp_qp_ipm_arg *arg, void *mem); +// +void s_tree_ocp_qp_ipm_arg_set_default(enum hpipm_mode mode, struct s_tree_ocp_qp_ipm_arg *arg); +// +void s_tree_ocp_qp_ipm_arg_set_iter_max(int *iter_max, struct s_tree_ocp_qp_ipm_arg *arg); +// +void s_tree_ocp_qp_ipm_arg_set_alpha_min(float *alpha_min, struct s_tree_ocp_qp_ipm_arg *arg); +// +void s_tree_ocp_qp_ipm_arg_set_mu0(float *mu0, struct s_tree_ocp_qp_ipm_arg *arg); +// +void s_tree_ocp_qp_ipm_arg_set_tol_stat(float *tol_stat, struct s_tree_ocp_qp_ipm_arg *arg); +// +void s_tree_ocp_qp_ipm_arg_set_tol_eq(float *tol_eq, struct s_tree_ocp_qp_ipm_arg *arg); +// +void s_tree_ocp_qp_ipm_arg_set_tol_ineq(float *tol_ineq, struct s_tree_ocp_qp_ipm_arg *arg); +// +void s_tree_ocp_qp_ipm_arg_set_tol_comp(float *tol_comp, struct s_tree_ocp_qp_ipm_arg *arg); +// +void s_tree_ocp_qp_ipm_arg_set_reg_prim(float *reg, struct s_tree_ocp_qp_ipm_arg *arg); +// +void s_tree_ocp_qp_ipm_arg_set_warm_start(int *warm_start, struct s_tree_ocp_qp_ipm_arg *arg); +// +void s_tree_ocp_qp_ipm_arg_set_pred_corr(int *pred_corr, struct s_tree_ocp_qp_ipm_arg *arg); +// +void s_tree_ocp_qp_ipm_arg_set_cond_pred_corr(int *value, struct s_tree_ocp_qp_ipm_arg *arg); +// +void s_tree_ocp_qp_ipm_arg_set_comp_dual_sol_eq(int *value, struct s_tree_ocp_qp_ipm_arg *arg); +// +void s_tree_ocp_qp_ipm_arg_set_comp_res_exit(int *value, struct s_tree_ocp_qp_ipm_arg *arg); +// +void s_tree_ocp_qp_ipm_arg_set_lam_min(float *value, struct s_tree_ocp_qp_ipm_arg *arg); +// +void s_tree_ocp_qp_ipm_arg_set_t_min(float *value, struct s_tree_ocp_qp_ipm_arg *arg); +// +void s_tree_ocp_qp_ipm_arg_set_tau_min(float *value, struct s_tree_ocp_qp_ipm_arg *arg); +// +void s_tree_ocp_qp_ipm_arg_set_split_step(int *value, struct s_tree_ocp_qp_ipm_arg *arg); +// +void s_tree_ocp_qp_ipm_arg_set_t_lam_min(int *value, struct s_tree_ocp_qp_ipm_arg *arg); + +// +hpipm_size_t s_tree_ocp_qp_ipm_ws_memsize(struct s_tree_ocp_qp_dim *dim, struct s_tree_ocp_qp_ipm_arg *arg); +// +void s_tree_ocp_qp_ipm_ws_create(struct s_tree_ocp_qp_dim *dim, struct s_tree_ocp_qp_ipm_arg *arg, struct s_tree_ocp_qp_ipm_ws *ws, void *mem); +// +void s_tree_ocp_qp_ipm_get_status(struct s_tree_ocp_qp_ipm_ws *ws, int *status); +// +void s_tree_ocp_qp_ipm_get_iter(struct s_tree_ocp_qp_ipm_ws *ws, int *iter); +// +void s_tree_ocp_qp_ipm_get_max_res_stat(struct s_tree_ocp_qp_ipm_ws *ws, float *res_stat); +// +void s_tree_ocp_qp_ipm_get_max_res_eq(struct s_tree_ocp_qp_ipm_ws *ws, float *res_eq); +// +void s_tree_ocp_qp_ipm_get_max_res_ineq(struct s_tree_ocp_qp_ipm_ws *ws, float *res_ineq); +// +void s_tree_ocp_qp_ipm_get_max_res_comp(struct s_tree_ocp_qp_ipm_ws *ws, float *res_comp); +// +void s_tree_ocp_qp_ipm_get_stat(struct s_tree_ocp_qp_ipm_ws *ws, float **stat); +// +void s_tree_ocp_qp_ipm_get_stat_m(struct s_tree_ocp_qp_ipm_ws *ws, int *stat_m); +// +void s_tree_ocp_qp_init_var(struct s_tree_ocp_qp *qp, struct s_tree_ocp_qp_sol *qp_sol, struct s_tree_ocp_qp_ipm_arg *arg, struct s_tree_ocp_qp_ipm_ws *ws); +// +void s_tree_ocp_qp_ipm_abs_step(int kk, struct s_tree_ocp_qp *qp, struct s_tree_ocp_qp_sol *qp_sol, struct s_tree_ocp_qp_ipm_arg *arg, struct s_tree_ocp_qp_ipm_ws *ws); +// +void s_tree_ocp_qp_ipm_delta_step(int kk, struct s_tree_ocp_qp *qp, struct s_tree_ocp_qp_sol *qp_sol, struct s_tree_ocp_qp_ipm_arg *arg, struct s_tree_ocp_qp_ipm_ws *ws); +// +void s_tree_ocp_qp_ipm_solve(struct s_tree_ocp_qp *qp, struct s_tree_ocp_qp_sol *qp_sol, struct s_tree_ocp_qp_ipm_arg *arg, struct s_tree_ocp_qp_ipm_ws *ws); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_S_TREE_OCP_QP_IPM_H_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_kkt.h b/phonelibs/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_kkt.h new file mode 100644 index 0000000000..bcf7354783 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_kkt.h @@ -0,0 +1,54 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + +#ifdef __cplusplus +extern "C" { +#endif + +// +void s_tree_ocp_qp_fact_solve_kkt_unconstr(struct s_tree_ocp_qp *qp, struct s_tree_ocp_qp_sol *qp_sol, struct s_tree_ocp_qp_ipm_arg *arg, struct s_tree_ocp_qp_ipm_ws *ws); +// +void s_tree_ocp_qp_fact_solve_kkt_step(struct s_tree_ocp_qp *qp, struct s_tree_ocp_qp_sol *qp_sol, struct s_tree_ocp_qp_ipm_arg *arg, struct s_tree_ocp_qp_ipm_ws *ws); +// +void s_tree_ocp_qp_fact_lq_solve_kkt_step(struct s_tree_ocp_qp *qp, struct s_tree_ocp_qp_sol *qp_sol, struct s_tree_ocp_qp_ipm_arg *arg, struct s_tree_ocp_qp_ipm_ws *ws); +// +void s_tree_ocp_qp_solve_kkt_step(struct s_tree_ocp_qp *qp, struct s_tree_ocp_qp_sol *qp_sol, struct s_tree_ocp_qp_ipm_arg *arg, struct s_tree_ocp_qp_ipm_ws *ws); + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_res.h b/phonelibs/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_res.h new file mode 100644 index 0000000000..5c6797204f --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_res.h @@ -0,0 +1,107 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_TREE_OCP_QP_RES_H_ +#define HPIPM_S_TREE_OCP_QP_RES_H_ + + + +#include +#include + +#include +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_tree_ocp_qp_res + { + struct s_tree_ocp_qp_dim *dim; + struct blasfeo_svec *res_g; // q-residuals + struct blasfeo_svec *res_b; // b-residuals + struct blasfeo_svec *res_d; // d-residuals + struct blasfeo_svec *res_m; // m-residuals + float res_max[4]; // max of residuals + float res_mu; // mu-residual + hpipm_size_t memsize; + }; + + + +struct s_tree_ocp_qp_res_ws + { + struct blasfeo_svec *tmp_nbgM; // work space of size nbM+ngM + struct blasfeo_svec *tmp_nsM; // work space of size nsM + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_tree_ocp_qp_res_memsize(struct s_tree_ocp_qp_dim *ocp_dim); +// +void s_tree_ocp_qp_res_create(struct s_tree_ocp_qp_dim *ocp_dim, struct s_tree_ocp_qp_res *res, void *mem); +// +hpipm_size_t s_tree_ocp_qp_res_ws_memsize(struct s_tree_ocp_qp_dim *ocp_dim); +// +void s_tree_ocp_qp_res_ws_create(struct s_tree_ocp_qp_dim *ocp_dim, struct s_tree_ocp_qp_res_ws *ws, void *mem); +// +void s_tree_ocp_qp_res_get_all(struct s_tree_ocp_qp_res *res, float **res_r, float **res_q, float **res_ls, float **res_us, float **res_b, float **res_d_lb, float **res_d_ub, float **res_d_lg, float **res_d_ug, float **res_d_ls, float **res_d_us, float **res_m_lb, float **res_m_ub, float **res_m_lg, float **res_m_ug, float **res_m_ls, float **res_m_us); +// +void s_tree_ocp_qp_res_compute(struct s_tree_ocp_qp *qp, struct s_tree_ocp_qp_sol *qp_sol, struct s_tree_ocp_qp_res *res, struct s_tree_ocp_qp_res_ws *ws); +// +void s_tree_ocp_qp_res_compute_lin(struct s_tree_ocp_qp *qp, struct s_tree_ocp_qp_sol *qp_sol, struct s_tree_ocp_qp_sol *qp_step, struct s_tree_ocp_qp_res *res, struct s_tree_ocp_qp_res_ws *ws); +// +void s_tree_ocp_qp_res_compute_inf_norm(struct s_tree_ocp_qp_res *res); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + +#endif // HPIPM_S_TREE_OCP_QP_RES_H_ + + + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_sol.h b/phonelibs/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_sol.h new file mode 100644 index 0000000000..71e887675b --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_sol.h @@ -0,0 +1,98 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_TREE_OCP_QP_SOL_H_ +#define HPIPM_S_TREE_OCP_QP_SOL_H_ + + + +#include +#include + +#include "hpipm_s_tree_ocp_qp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + +struct s_tree_ocp_qp_sol + { + struct s_tree_ocp_qp_dim *dim; + struct blasfeo_svec *ux; + struct blasfeo_svec *pi; + struct blasfeo_svec *lam; + struct blasfeo_svec *t; + void *misc; + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +hpipm_size_t s_tree_ocp_qp_sol_memsize(struct s_tree_ocp_qp_dim *dim); +// +void s_tree_ocp_qp_sol_create(struct s_tree_ocp_qp_dim *dim, struct s_tree_ocp_qp_sol *qp_sol, void *memory); +// +void s_tree_ocp_qp_sol_get_all(struct s_tree_ocp_qp *qp, struct s_tree_ocp_qp_sol *qp_sol, float **u, float **x, float **ls, float **us, float **pi, float **lam_lb, float **lam_ub, float **lam_lg, float **lam_ug, float **lam_ls, float **lam_us); +// +void s_tree_ocp_qp_sol_get_u(int node, struct s_tree_ocp_qp_sol *qp_sol, float *vec); +// +void s_tree_ocp_qp_sol_get_x(int node, struct s_tree_ocp_qp_sol *qp_sol, float *vec); +// +void s_tree_ocp_qp_sol_get_sl(int node, struct s_tree_ocp_qp_sol *qp_sol, float *vec); +// +void s_tree_ocp_qp_sol_get_su(int node, struct s_tree_ocp_qp_sol *qp_sol, float *vec); +// +void s_tree_ocp_qp_sol_get_pi(int edge, struct s_tree_ocp_qp_sol *qp_sol, float *vec); +// +void s_tree_ocp_qp_sol_get_lam_lb(int node, struct s_tree_ocp_qp_sol *qp_sol, float *vec); +// +void s_tree_ocp_qp_sol_get_lam_ub(int node, struct s_tree_ocp_qp_sol *qp_sol, float *vec); +// +void s_tree_ocp_qp_sol_get_lam_lg(int node, struct s_tree_ocp_qp_sol *qp_sol, float *vec); +// +void s_tree_ocp_qp_sol_get_lam_ug(int node, struct s_tree_ocp_qp_sol *qp_sol, float *vec); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_S_TREE_OCP_QP_SOL_H_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_utils.h b/phonelibs/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_utils.h new file mode 100644 index 0000000000..ec1747568f --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_utils.h @@ -0,0 +1,84 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_TREE_OCP_QP_UTILS_H_ +#define HPIPM_S_TREE_OCP_QP_UTILS_H_ + + + +#include +#include + +#include "hpipm_s_tree_ocp_qp_dim.h" +#include "hpipm_s_tree_ocp_qp.h" +#include "hpipm_s_tree_ocp_qp_sol.h" +#include "hpipm_s_tree_ocp_qp_ipm.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +// +void s_tree_ocp_qp_dim_print(struct s_tree_ocp_qp_dim *qp_dim); +// +//void s_tree_ocp_qp_dim_codegen(char *file_name, char *mode, struct s_tree_ocp_qp_dim *qp_dim); +// +void s_tree_ocp_qp_print(struct s_tree_ocp_qp_dim *qp_dim, struct s_tree_ocp_qp *qp); +// +//void s_tree_ocp_qp_codegen(char *file_name, char *mode, struct s_tree_ocp_qp_dim *qp_dim, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_sol_print(struct s_tree_ocp_qp_dim *qp_dim, struct s_tree_ocp_qp_sol *ocp_qp_sol); +// +void s_tree_ocp_qp_ipm_arg_print(struct s_tree_ocp_qp_dim *qp_dim, struct s_tree_ocp_qp_ipm_arg *arg); +// +//void s_tree_ocp_qp_ipm_arg_codegen(char *file_name, char *mode, struct s_tree_ocp_qp_dim *qp_dim, struct s_tree_ocp_qp_ipm_arg *arg); +// +void s_tree_ocp_qp_res_print(struct s_tree_ocp_qp_dim *qp_dim, struct s_tree_ocp_qp_res *ocp_qp_res); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_S_TREE_OCP_QP_UTILS_H_ + + diff --git a/phonelibs/acados/include/hpipm/include/hpipm_scenario_tree.h b/phonelibs/acados/include/hpipm/include/hpipm_scenario_tree.h new file mode 100644 index 0000000000..a3c77a670a --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_scenario_tree.h @@ -0,0 +1,70 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_SCENARIO_TREE_H_ +#define HPIPM_SCENARIO_TREE_H_ + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct sctree + { + struct node *root; // pointer to root + int *kids; // pointer to array of kids + int Nn; // numer of nodes + int md; // number of realizations + int Nr; // robust horizion + int Nh; // control horizion + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t sctree_memsize(int md, int Nr, int Nh); +// +void sctree_create(int md, int Nr, int Nh, struct sctree *st, void *memory); +// +void sctree_cast_to_tree(struct sctree *st, struct tree *tt); + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // HPIPM_SCENARIO_TREE_H_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_timing.h b/phonelibs/acados/include/hpipm/include/hpipm_timing.h new file mode 100644 index 0000000000..bd0f2dbcb3 --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_timing.h @@ -0,0 +1,67 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_TIMING_H_ +#define HPIPM_TIMING_H_ + + + +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +typedef blasfeo_timer hpipm_timer; + + + +// +void hpipm_tic(hpipm_timer *t); +// +double hpipm_toc(hpipm_timer *t); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + +#endif // HPIPM_TIMING_H_ diff --git a/phonelibs/acados/include/hpipm/include/hpipm_tree.h b/phonelibs/acados/include/hpipm/include/hpipm_tree.h new file mode 100644 index 0000000000..f9d69ecddd --- /dev/null +++ b/phonelibs/acados/include/hpipm/include/hpipm_tree.h @@ -0,0 +1,76 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* 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 OWNER 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. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + +#ifndef HPIPM_TREE_H_ +#define HPIPM_TREE_H_ + +#include "hpipm_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct node + { + int *kids; // 64 bits + int idx; // 32 bits + int dad; // 32 bits + int nkids; // 32 bits + int stage; // 32 bits + int real; // 32 bits + int idxkid; // 32 bits // XXX needed ??? + // total 256 bits + }; + + + +struct tree + { + struct node *root; // pointer to root + int *kids; // pointer to array of kids + int Nn; // numer of nodes + hpipm_size_t memsize; + }; + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // HPIPM_TREE_H_ diff --git a/phonelibs/acados/include/qpOASES_e/Bounds.h b/phonelibs/acados/include/qpOASES_e/Bounds.h new file mode 100644 index 0000000000..4e41c1d163 --- /dev/null +++ b/phonelibs/acados/include/qpOASES_e/Bounds.h @@ -0,0 +1,543 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, + * Christian Kirches et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file include/qpOASES_e/Bounds.h + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches + * \version 3.1embedded + * \date 2007-2015 + * + * Declaration of the Bounds class designed to manage working sets of + * bounds within a QProblem. + */ + + +#ifndef QPOASES_BOUNDS_H +#define QPOASES_BOUNDS_H + + +#include + + +BEGIN_NAMESPACE_QPOASES + + +/** + * \brief Manages working sets of bounds (= box constraints). + * + * This class manages working sets of bounds (= box constraints) + * by storing index sets and other status information. + * + * \author Hans Joachim Ferreau + * \version 3.1embedded + * \date 2007-2015 + */ +typedef struct +{ + Indexlist *freee; /**< Index list of free variables. */ + Indexlist *fixed; /**< Index list of fixed variables. */ + + Indexlist *shiftedFreee; /**< Memory for shifting free variables. */ + Indexlist *shiftedFixed; /**< Memory for shifting fixed variables. */ + + Indexlist *rotatedFreee; /**< Memory for rotating free variables. */ + Indexlist *rotatedFixed; /**< Memory for rotating fixed variables. */ + + SubjectToType *type; /**< Type of bounds. */ + SubjectToStatus *status; /**< Status of bounds. */ + + SubjectToType *typeTmp; /**< Temp memory for type of bounds. */ + SubjectToStatus *statusTmp; /**< Temp memory for status of bounds. */ + + BooleanType noLower; /**< This flag indicates if there is no lower bound on any variable. */ + BooleanType noUpper; /**< This flag indicates if there is no upper bound on any variable. */ + + int n; /**< Total number of bounds. */ +} Bounds; + +int Bounds_calculateMemorySize( int n); + +char *Bounds_assignMemory(int n, Bounds **mem, void *raw_memory); + +Bounds *Bounds_createMemory( int n ); + +/** Constructor which takes the number of bounds. */ +void BoundsCON( Bounds* _THIS, + int _n /**< Number of bounds. */ + ); + +/** Copies all members from given rhs object. + * \return SUCCESSFUL_RETURN */ +void BoundsCPY( Bounds* FROM, + Bounds* TO + ); + + +/** Initialises object with given number of bounds. + * \return SUCCESSFUL_RETURN \n + RET_INVALID_ARGUMENTS */ +returnValue Bounds_init( Bounds* _THIS, + int _n /**< Number of bounds. */ + ); + + +/** Initially adds number of a new (i.e. not yet in the list) bound to + * given index set. + * \return SUCCESSFUL_RETURN \n + RET_SETUP_BOUND_FAILED \n + RET_INDEX_OUT_OF_BOUNDS \n + RET_INVALID_ARGUMENTS */ +returnValue Bounds_setupBound( Bounds* _THIS, + int number, /**< Number of new bound. */ + SubjectToStatus _status /**< Status of new bound. */ + ); + +/** Initially adds all numbers of new (i.e. not yet in the list) bounds to + * to the index set of free bounds; the order depends on the SujectToType + * of each index. + * \return SUCCESSFUL_RETURN \n + RET_SETUP_BOUND_FAILED */ +returnValue Bounds_setupAllFree( Bounds* _THIS + ); + +/** Initially adds all numbers of new (i.e. not yet in the list) bounds to + * to the index set of fixed bounds (on their lower bounds); + * the order depends on the SujectToType of each index. + * \return SUCCESSFUL_RETURN \n + RET_SETUP_BOUND_FAILED */ +returnValue Bounds_setupAllLower( Bounds* _THIS + ); + +/** Initially adds all numbers of new (i.e. not yet in the list) bounds to + * to the index set of fixed bounds (on their upper bounds); + * the order depends on the SujectToType of each index. + * \return SUCCESSFUL_RETURN \n + RET_SETUP_BOUND_FAILED */ +returnValue Bounds_setupAllUpper( Bounds* _THIS + ); + + +/** Moves index of a bound from index list of fixed to that of free bounds. + * \return SUCCESSFUL_RETURN \n + RET_MOVING_BOUND_FAILED \n + RET_INDEX_OUT_OF_BOUNDS */ +returnValue Bounds_moveFixedToFree( Bounds* _THIS, + int number /**< Number of bound to be freed. */ + ); + +/** Moves index of a bound from index list of free to that of fixed bounds. + * \return SUCCESSFUL_RETURN \n + RET_MOVING_BOUND_FAILED \n + RET_INDEX_OUT_OF_BOUNDS */ +returnValue Bounds_moveFreeToFixed( Bounds* _THIS, + int number, /**< Number of bound to be fixed. */ + SubjectToStatus _status /**< Status of bound to be fixed. */ + ); + +/** Flip fixed bound. + * \return SUCCESSFUL_RETURN \n + RET_MOVING_BOUND_FAILED \n + RET_INDEX_OUT_OF_BOUNDS */ +returnValue Bounds_flipFixed( Bounds* _THIS, + int number + ); + +/** Swaps the indices of two free bounds within the index set. + * \return SUCCESSFUL_RETURN \n + RET_SWAPINDEX_FAILED */ +returnValue Bounds_swapFree( Bounds* _THIS, + int number1, /**< Number of first bound. */ + int number2 /**< Number of second bound. */ + ); + + +/** Returns number of variables. + * \return Number of variables. */ +static inline int Bounds_getNV( Bounds* _THIS + ); + +/** Returns number of implicitly fixed variables. + * \return Number of implicitly fixed variables. */ +static inline int Bounds_getNFV( Bounds* _THIS + ); + +/** Returns number of bounded (but possibly free) variables. + * \return Number of bounded (but possibly free) variables. */ +static inline int Bounds_getNBV( Bounds* _THIS + ); + +/** Returns number of unbounded variables. + * \return Number of unbounded variables. */ +static inline int Bounds_getNUV( Bounds* _THIS + ); + +/** Returns number of free variables. + * \return Number of free variables. */ +static inline int Bounds_getNFR( Bounds* _THIS + ); + +/** Returns number of fixed variables. + * \return Number of fixed variables. */ +static inline int Bounds_getNFX( Bounds* _THIS + ); + + +/** Returns a pointer to free variables index list. + * \return Pointer to free variables index list. */ +static inline Indexlist* Bounds_getFree( Bounds* _THIS + ); + +/** Returns a pointer to fixed variables index list. + * \return Pointer to fixed variables index list. */ +static inline Indexlist* Bounds_getFixed( Bounds* _THIS + ); + + +/** Returns number of bounds with given SubjectTo type. + * \return Number of bounds with given type. */ +static inline int Bounds_getNumberOfType( Bounds* _THIS, + SubjectToType _type /**< Type of bound. */ + ); + + +/** Returns type of bound. + * \return Type of bound \n + RET_INDEX_OUT_OF_BOUNDS */ +static inline SubjectToType Bounds_getType( Bounds* _THIS, + int i /**< Number of bound. */ + ); + +/** Returns status of bound. + * \return Status of bound \n + ST_UNDEFINED */ +static inline SubjectToStatus Bounds_getStatus( Bounds* _THIS, + int i /**< Number of bound. */ + ); + + +/** Sets type of bound. + * \return SUCCESSFUL_RETURN \n + RET_INDEX_OUT_OF_BOUNDS */ +static inline returnValue Bounds_setType( Bounds* _THIS, + int i, /**< Number of bound. */ + SubjectToType value /**< Type of bound. */ + ); + +/** Sets status of bound. + * \return SUCCESSFUL_RETURN \n + RET_INDEX_OUT_OF_BOUNDS */ +static inline returnValue Bounds_setStatus( Bounds* _THIS, + int i, /**< Number of bound. */ + SubjectToStatus value /**< Status of bound. */ + ); + + +/** Sets status of lower bounds. */ +static inline void Bounds_setNoLower( Bounds* _THIS, + BooleanType _status /**< Status of lower bounds. */ + ); + +/** Sets status of upper bounds. */ +static inline void Bounds_setNoUpper( Bounds* _THIS, + BooleanType _status /**< Status of upper bounds. */ + ); + + +/** Returns status of lower bounds. + * \return BT_TRUE if there is no lower bound on any variable. */ +static inline BooleanType Bounds_hasNoLower( Bounds* _THIS + ); + +/** Returns status of upper bounds. + * \return BT_TRUE if there is no upper bound on any variable. */ +static inline BooleanType Bounds_hasNoUpper( Bounds* _THIS + ); + + +/** Shifts forward type and status of all bounds by a given + * offset. This offset has to lie within the range [0,n/2] and has to + * be an integer divisor of the total number of bounds n. + * Type and status of the first \ bounds is thrown away, + * type and status of the last \ bounds is doubled, + * e.g. for offset = 2: \n + * shift( {b1,b2,b3,b4,b5,b6} ) = {b3,b4,b5,b6,b5,b6} + * \return SUCCESSFUL_RETURN \n + RET_INDEX_OUT_OF_BOUNDS \n + RET_INVALID_ARGUMENTS \n + RET_SHIFTING_FAILED */ +returnValue Bounds_shift( Bounds* _THIS, + int offset /**< Shift offset within the range [0,n/2] and integer divisor of n. */ + ); + +/** Rotates forward type and status of all bounds by a given + * offset. This offset has to lie within the range [0,n]. + * Example for offset = 2: \n + * rotate( {b1,b2,b3,b4,b5,b6} ) = {b3,b4,b5,b6,b1,b2} + * \return SUCCESSFUL_RETURN \n + RET_INDEX_OUT_OF_BOUNDS \n + RET_ROTATING_FAILED */ +returnValue Bounds_rotate( Bounds* _THIS, + int offset /**< Rotation offset within the range [0,n]. */ + ); + + +/** Prints information on bounds object + * (in particular, lists of free and fixed bounds. + * \return SUCCESSFUL_RETURN \n + RET_INDEXLIST_CORRUPTED */ +returnValue Bounds_print( Bounds* _THIS + ); + + +/** Initially adds all numbers of new (i.e. not yet in the list) bounds to + * to the index set corresponding to the desired status; + * the order depends on the SujectToType of each index. + * \return SUCCESSFUL_RETURN \n + RET_SETUP_BOUND_FAILED */ +returnValue Bounds_setupAll( Bounds* _THIS, + SubjectToStatus _status /**< Desired initial status for all bounds. */ + ); + + +/** Adds the index of a new bound to index set. + * \return SUCCESSFUL_RETURN \n + RET_ADDINDEX_FAILED \n + RET_INVALID_ARGUMENTS */ +returnValue Bounds_addIndex( Bounds* _THIS, + Indexlist* const indexlist, /**< Index list to which the new index shall be added. */ + int newnumber, /**< Number of new bound. */ + SubjectToStatus newstatus /**< Status of new bound. */ + ); + +/** Removes the index of a bound from index set. + * \return SUCCESSFUL_RETURN \n + RET_REMOVEINDEX_FAILED \n + RET_INVALID_ARGUMENTS */ +returnValue Bounds_removeIndex( Bounds* _THIS, + Indexlist* const indexlist, /**< Index list from which the new index shall be removed. */ + int removenumber /**< Number of bound to be removed. */ + ); + +/** Swaps the indices of two constraints or bounds within the index set. + * \return SUCCESSFUL_RETURN \n + RET_SWAPINDEX_FAILED \n + RET_INVALID_ARGUMENTS */ +returnValue Bounds_swapIndex( Bounds* _THIS, + Indexlist* const indexlist, /**< Index list in which the indices shold be swapped. */ + int number1, /**< Number of first bound. */ + int number2 /**< Number of second bound. */ + ); + + + +/* + * g e t N u m b e r O f T y p e + */ +static inline int Bounds_getNumberOfType( Bounds* _THIS, SubjectToType _type ) +{ + int i; + int numberOfType = 0; + + if ( _THIS->type != 0 ) + { + for( i=0; i<_THIS->n; ++i ) + if ( _THIS->type[i] == _type ) + ++numberOfType; + } + + return numberOfType; +} + + +/* + * g e t T y p e + */ +static inline SubjectToType Bounds_getType( Bounds* _THIS, int i ) +{ + if ( ( i >= 0 ) && ( i < _THIS->n ) ) + return _THIS->type[i]; + + return ST_UNKNOWN; +} + + +/* + * g e t S t a t u s + */ +static inline SubjectToStatus Bounds_getStatus( Bounds* _THIS, int i ) +{ + if ( ( i >= 0 ) && ( i < _THIS->n ) ) + return _THIS->status[i]; + + return ST_UNDEFINED; +} + + +/* + * s e t T y p e + */ +static inline returnValue Bounds_setType( Bounds* _THIS, int i, SubjectToType value ) +{ + if ( ( i >= 0 ) && ( i < _THIS->n ) ) + { + _THIS->type[i] = value; + return SUCCESSFUL_RETURN; + } + else + return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); +} + + +/* + * s e t S t a t u s + */ +static inline returnValue Bounds_setStatus( Bounds* _THIS, int i, SubjectToStatus value ) +{ + if ( ( i >= 0 ) && ( i < _THIS->n ) ) + { + _THIS->status[i] = value; + return SUCCESSFUL_RETURN; + } + else + return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); +} + + +/* + * s e t N o L o w e r + */ +static inline void Bounds_setNoLower( Bounds* _THIS, BooleanType _status ) +{ + _THIS->noLower = _status; +} + + +/* + * s e t N o U p p e r + */ +static inline void Bounds_setNoUpper( Bounds* _THIS, BooleanType _status ) +{ + _THIS->noUpper = _status; +} + + +/* + * h a s N o L o w e r + */ +static inline BooleanType Bounds_hasNoLower( Bounds* _THIS ) +{ + return _THIS->noLower; +} + + +/* + * h a s N o U p p p e r + */ +static inline BooleanType Bounds_hasNoUpper( Bounds* _THIS ) +{ + return _THIS->noUpper; +} + + + +/* + * g e t N V + */ +static inline int Bounds_getNV( Bounds* _THIS ) +{ + return _THIS->n; +} + + +/* + * g e t N F V + */ +static inline int Bounds_getNFV( Bounds* _THIS ) +{ + return Bounds_getNumberOfType( _THIS,ST_EQUALITY ); +} + + +/* + * g e t N B V + */ +static inline int Bounds_getNBV( Bounds* _THIS ) +{ + return Bounds_getNumberOfType( _THIS,ST_BOUNDED ); +} + + +/* + * g e t N U V + */ +static inline int Bounds_getNUV( Bounds* _THIS ) +{ + return Bounds_getNumberOfType( _THIS,ST_UNBOUNDED ); +} + + +/* + * g e t N F R + */ +static inline int Bounds_getNFR( Bounds* _THIS ) +{ + return Indexlist_getLength( _THIS->freee ); +} + + +/* + * g e t N F X + */ +static inline int Bounds_getNFX( Bounds* _THIS ) +{ + return Indexlist_getLength( _THIS->fixed ); +} + + +/* + * g e t F r e e + */ +static inline Indexlist* Bounds_getFree( Bounds* _THIS ) +{ + return _THIS->freee; +} + + +/* + * g e t F i x e d + */ +static inline Indexlist* Bounds_getFixed( Bounds* _THIS ) +{ + return _THIS->fixed; +} + + +END_NAMESPACE_QPOASES + +#endif /* QPOASES_BOUNDS_H */ + + +/* + * end of file + */ diff --git a/phonelibs/acados/include/qpOASES_e/Constants.h b/phonelibs/acados/include/qpOASES_e/Constants.h new file mode 100644 index 0000000000..0e3dcd19f4 --- /dev/null +++ b/phonelibs/acados/include/qpOASES_e/Constants.h @@ -0,0 +1,134 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, + * Christian Kirches et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file include/qpOASES_e/Constants.h + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches + * \version 3.1embedded + * \date 2007-2015 + * + * Definition of all global constants. + */ + + +#ifndef QPOASES_CONSTANTS_H +#define QPOASES_CONSTANTS_H + + +#include + +#ifdef __CODE_GENERATION__ + + #define CONVERTTOSTRINGAUX(x) #x + #define CONVERTTOSTRING(x) CONVERTTOSTRINGAUX(x) + + #ifndef QPOASES_CUSTOM_INTERFACE + #include "acado_qpoases3_interface.h" + #else + #include CONVERTTOSTRING(QPOASES_CUSTOM_INTERFACE) + #endif + +#endif + + +BEGIN_NAMESPACE_QPOASES + + +#ifndef __EXTERNAL_DIMENSIONS__ + + /*#define QPOASES_NVMAX 50 + #define QPOASES_NCMAX 100*/ + #define QPOASES_NVMAX 287 + #define QPOASES_NCMAX 709 + +#endif /* __EXTERNAL_DIMENSIONS__ */ + + +/** Maximum number of variables within a QP formulation. + * Note: this value has to be positive! */ +#define NVMAX QPOASES_NVMAX + +/** Maximum number of constraints within a QP formulation. + * Note: this value has to be positive! */ +#define NCMAX QPOASES_NCMAX + +#if ( QPOASES_NVMAX > QPOASES_NCMAX ) +#define NVCMAX QPOASES_NVMAX +#else +#define NVCMAX QPOASES_NCMAX +#endif + +#if ( QPOASES_NVMAX > QPOASES_NCMAX ) +#define NVCMIN QPOASES_NCMAX +#else +#define NVCMIN QPOASES_NVMAX +#endif + + +/** Maximum number of QPs in a sequence solved by means of the OQP interface. + * Note: this value has to be positive! */ +#define NQPMAX 1000 + + +/** Numerical value of machine precision (min eps, s.t. 1+eps > 1). + * Note: this value has to be positive! */ +#ifndef __CODE_GENERATION__ + + #ifdef __USE_SINGLE_PRECISION__ + static const real_t QPOASES_EPS = 1.193e-07; + #else + static const real_t QPOASES_EPS = 2.221e-16; + #endif /* __USE_SINGLE_PRECISION__ */ + +#endif /* __CODE_GENERATION__ */ + + +/** Numerical value of zero (for situations in which it would be + * unreasonable to compare with 0.0). + * Note: this value has to be positive! */ +static const real_t QPOASES_ZERO = 1.0e-25; + +/** Numerical value of infinity (e.g. for non-existing bounds). + * Note: this value has to be positive! */ +static const real_t QPOASES_INFTY = 1.0e20; + +/** Tolerance to used for isEqual, isZero etc. + * Note: this value has to be positive! */ +static const real_t QPOASES_TOL = 1.0e-25; + + +/** Maximum number of characters within a string. + * Note: this value should be at least 41! */ +#define QPOASES_MAX_STRING_LENGTH 160 + + +END_NAMESPACE_QPOASES + + +#endif /* QPOASES_CONSTANTS_H */ + + +/* + * end of file + */ diff --git a/phonelibs/acados/include/qpOASES_e/ConstraintProduct.h b/phonelibs/acados/include/qpOASES_e/ConstraintProduct.h new file mode 100644 index 0000000000..ddfcfbe5dc --- /dev/null +++ b/phonelibs/acados/include/qpOASES_e/ConstraintProduct.h @@ -0,0 +1,62 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, + * Christian Kirches et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file include/qpOASES_e/ConstraintProduct.h + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches (thanks to D. Kwame Minde Kufoalor) + * \version 3.1embedded + * \date 2009-2015 + * + * Declaration of the ConstraintProduct interface which allows to specify a + * user-defined function for evaluating the constraint product at the + * current iterate to speed-up QP solution in case of a specially structured + * constraint matrix. + */ + + + +#ifndef QPOASES_CONSTRAINT_PRODUCT_H +#define QPOASES_CONSTRAINT_PRODUCT_H + + +BEGIN_NAMESPACE_QPOASES + + +/** + * \brief Interface for specifying user-defined evaluations of constraint products. + * + * An interface which allows to specify a user-defined function for evaluating the + * constraint product at the current iterate to speed-up QP solution in case + * of a specially structured constraint matrix. + * + * \author Hans Joachim Ferreau (thanks to Kwame Minde Kufoalor) + * \version 3.1embedded + * \date 2009-2015 + */ +typedef int(*ConstraintProduct)( int, const real_t* const, real_t* const ); + + +END_NAMESPACE_QPOASES + +#endif /* QPOASES_CONSTRAINT_PRODUCT_H */ diff --git a/phonelibs/acados/include/qpOASES_e/Constraints.h b/phonelibs/acados/include/qpOASES_e/Constraints.h new file mode 100644 index 0000000000..8aca10d5d9 --- /dev/null +++ b/phonelibs/acados/include/qpOASES_e/Constraints.h @@ -0,0 +1,535 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, + * Christian Kirches et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file include/qpOASES_e/Constraints.h + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches + * \version 3.1embedded + * \date 2007-2015 + * + * Declaration of the Constraints class designed to manage working sets of + * constraints within a QProblem. + */ + + +#ifndef QPOASES_CONSTRAINTS_H +#define QPOASES_CONSTRAINTS_H + + +#include + + +BEGIN_NAMESPACE_QPOASES + + +/** + * \brief Manages working sets of constraints. + * + * This class manages working sets of constraints by storing + * index sets and other status information. + * + * \author Hans Joachim Ferreau + * \version 3.1embedded + * \date 2007-2015 + */ +typedef struct +{ + Indexlist *active; /**< Index list of active constraints. */ + Indexlist *inactive; /**< Index list of inactive constraints. */ + + Indexlist *shiftedActive; /**< Memory for shifting active constraints. */ + Indexlist *shiftedInactive; /**< Memory for shifting inactive constraints. */ + + Indexlist *rotatedActive; /**< Memory for rotating active constraints. */ + Indexlist *rotatedInactive; /**< Memory for rotating inactive constraints. */ + + SubjectToType *type; /**< Type of constraints. */ + SubjectToStatus *status; /**< Status of constraints. */ + + SubjectToType *typeTmp; /**< Temp memory for type of constraints. */ + SubjectToStatus *statusTmp; /**< Temp memory for status of constraints. */ + + BooleanType noLower; /**< This flag indicates if there is no lower bound on any variable. */ + BooleanType noUpper; /**< This flag indicates if there is no upper bound on any variable. */ + + int n; /**< Total number of constraints. */ +} Constraints; + +int Constraints_calculateMemorySize( int n); + +char *Constraints_assignMemory(int n, Constraints **mem, void *raw_memory); + +Constraints *Constraints_createMemory( int n ); + +/** Constructor which takes the number of constraints. */ +void ConstraintsCON( Constraints* _THIS, + int _n /**< Number of constraints. */ + ); + +/** Copies all members from given rhs object. + * \return SUCCESSFUL_RETURN */ +void ConstraintsCPY( Constraints* FROM, + Constraints* TO + ); + + +/** Initialises object with given number of constraints. + * \return SUCCESSFUL_RETURN \n + RET_INVALID_ARGUMENTS */ +returnValue Constraints_init( Constraints* _THIS, + int _n /**< Number of constraints. */ + ); + + +/** Initially adds number of a new (i.e. not yet in the list) constraint to + * a given index set. + * \return SUCCESSFUL_RETURN \n + RET_SETUP_CONSTRAINT_FAILED \n + RET_INDEX_OUT_OF_BOUNDS \n + RET_INVALID_ARGUMENTS */ +returnValue Constraints_setupConstraint( Constraints* _THIS, + int number, /**< Number of new constraint. */ + SubjectToStatus _status /**< Status of new constraint. */ + ); + +/** Initially adds all enabled numbers of new (i.e. not yet in the list) constraints to + * to the index set of inactive constraints; the order depends on the SujectToType + * of each index. Only disabled constraints are added to index set of disabled constraints! + * \return SUCCESSFUL_RETURN \n + RET_SETUP_CONSTRAINT_FAILED */ +returnValue Constraints_setupAllInactive( Constraints* _THIS + ); + +/** Initially adds all enabled numbers of new (i.e. not yet in the list) constraints to + * to the index set of active constraints (on their lower bounds); the order depends on the SujectToType + * of each index. Only disabled constraints are added to index set of disabled constraints! + * \return SUCCESSFUL_RETURN \n + RET_SETUP_CONSTRAINT_FAILED */ +returnValue Constraints_setupAllLower( Constraints* _THIS + ); + +/** Initially adds all enabled numbers of new (i.e. not yet in the list) constraints to + * to the index set of active constraints (on their upper bounds); the order depends on the SujectToType + * of each index. Only disabled constraints are added to index set of disabled constraints! + * \return SUCCESSFUL_RETURN \n + RET_SETUP_CONSTRAINT_FAILED */ +returnValue Constraints_setupAllUpper( Constraints* _THIS + ); + + +/** Moves index of a constraint from index list of active to that of inactive constraints. + * \return SUCCESSFUL_RETURN \n + RET_MOVING_CONSTRAINT_FAILED */ +returnValue Constraints_moveActiveToInactive( Constraints* _THIS, + int number /**< Number of constraint to become inactive. */ + ); + +/** Moves index of a constraint from index list of inactive to that of active constraints. + * \return SUCCESSFUL_RETURN \n + RET_MOVING_CONSTRAINT_FAILED */ +returnValue Constraints_moveInactiveToActive( Constraints* _THIS, + int number, /**< Number of constraint to become active. */ + SubjectToStatus _status /**< Status of constraint to become active. */ + ); + +/** Flip fixed constraint. + * \return SUCCESSFUL_RETURN \n + RET_MOVING_CONSTRAINT_FAILED \n + RET_INDEX_OUT_OF_BOUNDS */ +returnValue Constraints_flipFixed( Constraints* _THIS, + int number + ); + + +/** Returns the number of constraints. + * \return Number of constraints. */ +static inline int Constraints_getNC( Constraints* _THIS + ); + +/** Returns the number of implicit equality constraints. + * \return Number of implicit equality constraints. */ +static inline int Constraints_getNEC( Constraints* _THIS + ); + +/** Returns the number of "real" inequality constraints. + * \return Number of "real" inequality constraints. */ +static inline int Constraints_getNIC( Constraints* _THIS + ); + +/** Returns the number of unbounded constraints (i.e. without any bounds). + * \return Number of unbounded constraints (i.e. without any bounds). */ +static inline int Constraints_getNUC( Constraints* _THIS + ); + +/** Returns the number of active constraints. + * \return Number of active constraints. */ +static inline int Constraints_getNAC( Constraints* _THIS + ); + +/** Returns the number of inactive constraints. + * \return Number of inactive constraints. */ +static inline int Constraints_getNIAC( Constraints* _THIS + ); + + +/** Returns a pointer to active constraints index list. + * \return Pointer to active constraints index list. */ +static inline Indexlist* Constraints_getActive( Constraints* _THIS + ); + +/** Returns a pointer to inactive constraints index list. + * \return Pointer to inactive constraints index list. */ +static inline Indexlist* Constraints_getInactive( Constraints* _THIS + ); + + +/** Returns number of constraints with given SubjectTo type. + * \return Number of constraints with given type. */ +static inline int Constraints_getNumberOfType( Constraints* _THIS, + SubjectToType _type /**< Type of constraints' bound. */ + ); + + +/** Returns type of constraints' bound. + * \return Type of constraints' bound \n + RET_INDEX_OUT_OF_BOUNDS */ +static inline SubjectToType Constraints_getType( Constraints* _THIS, + int i /**< Number of constraints' bound. */ + ); + +/** Returns status of constraints' bound. + * \return Status of constraints' bound \n + ST_UNDEFINED */ +static inline SubjectToStatus Constraints_getStatus( Constraints* _THIS, + int i /**< Number of constraints' bound. */ + ); + + +/** Sets type of constraints' bound. + * \return SUCCESSFUL_RETURN \n + RET_INDEX_OUT_OF_BOUNDS */ +static inline returnValue Constraints_setType( Constraints* _THIS, + int i, /**< Number of constraints' bound. */ + SubjectToType value /**< Type of constraints' bound. */ + ); + +/** Sets status of constraints' bound. + * \return SUCCESSFUL_RETURN \n + RET_INDEX_OUT_OF_BOUNDS */ +static inline returnValue Constraints_setStatus( Constraints* _THIS, + int i, /**< Number of constraints' bound. */ + SubjectToStatus value /**< Status of constraints' bound. */ + ); + + +/** Sets status of lower constraints' bounds. */ +static inline void Constraints_setNoLower( Constraints* _THIS, + BooleanType _status /**< Status of lower constraints' bounds. */ + ); + +/** Sets status of upper constraints' bounds. */ +static inline void Constraints_setNoUpper( Constraints* _THIS, + BooleanType _status /**< Status of upper constraints' bounds. */ + ); + + +/** Returns status of lower constraints' bounds. + * \return BT_TRUE if there is no lower constraints' bound on any variable. */ +static inline BooleanType Constraints_hasNoLower( Constraints* _THIS + ); + +/** Returns status of upper bounds. + * \return BT_TRUE if there is no upper constraints' bound on any variable. */ +static inline BooleanType Constraints_hasNoUpper( Constraints* _THIS + ); + + +/** Shifts forward type and status of all constraints by a given + * offset. This offset has to lie within the range [0,n/2] and has to + * be an integer divisor of the total number of constraints n. + * Type and status of the first \ constraints is thrown away, + * type and status of the last \ constraints is doubled, + * e.g. for offset = 2: \n + * shift( {c/b1,c/b2,c/b3,c/b4,c/b5,c/b6} ) = {c/b3,c/b4,c/b5,c/b6,c/b5,c/b6} + * \return SUCCESSFUL_RETURN \n + RET_INDEX_OUT_OF_BOUNDS \n + RET_INVALID_ARGUMENTS \n + RET_SHIFTING_FAILED */ +returnValue Constraints_shift( Constraints* _THIS, + int offset /**< Shift offset within the range [0,n/2] and integer divisor of n. */ + ); + +/** Rotates forward type and status of all constraints by a given + * offset. This offset has to lie within the range [0,n]. + * Example for offset = 2: \n + * rotate( {c1,c2,c3,c4,c5,c6} ) = {c3,c4,c5,c6,c1,c2} + * \return SUCCESSFUL_RETURN \n + RET_INDEX_OUT_OF_BOUNDS \n + RET_ROTATING_FAILED */ +returnValue Constraints_rotate( Constraints* _THIS, + int offset /**< Rotation offset within the range [0,n]. */ + ); + + +/** Prints information on constraints object + * (in particular, lists of inactive and active constraints. + * \return SUCCESSFUL_RETURN \n + RET_INDEXLIST_CORRUPTED */ +returnValue Constraints_print( Constraints* _THIS + ); + + +/** Initially adds all numbers of new (i.e. not yet in the list) bounds to + * to the index set corresponding to the desired status; + * the order depends on the SujectToType of each index. + * \return SUCCESSFUL_RETURN \n + RET_SETUP_CONSTRAINT_FAILED */ +returnValue Constraints_setupAll( Constraints* _THIS, + SubjectToStatus _status /**< Desired initial status for all bounds. */ + ); + + +/** Adds the index of a new constraint to index set. + * \return SUCCESSFUL_RETURN \n + RET_ADDINDEX_FAILED \n + RET_INVALID_ARGUMENTS */ +returnValue Constraints_addIndex( Constraints* _THIS, + Indexlist* const indexlist, /**< Index list to which the new index shall be added. */ + int newnumber, /**< Number of new constraint. */ + SubjectToStatus newstatus /**< Status of new constraint. */ + ); + +/** Removes the index of a constraint from index set. + * \return SUCCESSFUL_RETURN \n + RET_REMOVEINDEX_FAILED \n + RET_INVALID_ARGUMENTS */ +returnValue Constraints_removeIndex( Constraints* _THIS, + Indexlist* const indexlist, /**< Index list from which the new index shall be removed. */ + int removenumber /**< Number of constraint to be removed. */ + ); + +/** Swaps the indices of two constraints or bounds within the index set. + * \return SUCCESSFUL_RETURN \n + RET_SWAPINDEX_FAILED \n + RET_INVALID_ARGUMENTS */ +returnValue Constraints_swapIndex( Constraints* _THIS, + Indexlist* const indexlist, /**< Index list in which the indices shold be swapped. */ + int number1, /**< Number of first constraint. */ + int number2 /**< Number of second constraint. */ + ); + + + +/* + * g e t N u m b e r O f T y p e + */ +static inline int Constraints_getNumberOfType( Constraints* _THIS, SubjectToType _type ) +{ + int i; + int numberOfType = 0; + + if ( _THIS->type != 0 ) + { + for( i=0; i<_THIS->n; ++i ) + if ( _THIS->type[i] == _type ) + ++numberOfType; + } + + return numberOfType; +} + + +/* + * g e t T y p e + */ +static inline SubjectToType Constraints_getType( Constraints* _THIS, int i ) +{ + if ( ( i >= 0 ) && ( i < _THIS->n ) ) + return _THIS->type[i]; + + return ST_UNKNOWN; +} + + +/* + * g e t S t a t u s + */ +static inline SubjectToStatus Constraints_getStatus( Constraints* _THIS, int i ) +{ + if ( ( i >= 0 ) && ( i < _THIS->n ) ) + return _THIS->status[i]; + + return ST_UNDEFINED; +} + + +/* + * s e t T y p e + */ +static inline returnValue Constraints_setType( Constraints* _THIS, int i, SubjectToType value ) +{ + if ( ( i >= 0 ) && ( i < _THIS->n ) ) + { + _THIS->type[i] = value; + return SUCCESSFUL_RETURN; + } + else + return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); +} + + +/* + * s e t S t a t u s + */ +static inline returnValue Constraints_setStatus( Constraints* _THIS, int i, SubjectToStatus value ) +{ + if ( ( i >= 0 ) && ( i < _THIS->n ) ) + { + _THIS->status[i] = value; + return SUCCESSFUL_RETURN; + } + else + return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); +} + + +/* + * s e t N o L o w e r + */ +static inline void Constraints_setNoLower( Constraints* _THIS, BooleanType _status ) +{ + _THIS->noLower = _status; +} + + +/* + * s e t N o U p p e r + */ +static inline void Constraints_setNoUpper( Constraints* _THIS, BooleanType _status ) +{ + _THIS->noUpper = _status; +} + + +/* + * h a s N o L o w e r + */ +static inline BooleanType Constraints_hasNoLower( Constraints* _THIS ) +{ + return _THIS->noLower; +} + + +/* + * h a s N o U p p p e r + */ +static inline BooleanType Constraints_hasNoUpper( Constraints* _THIS ) +{ + return _THIS->noUpper; +} + + + +/* + * g e t N C + */ +static inline int Constraints_getNC( Constraints* _THIS ) +{ + return _THIS->n; +} + + +/* + * g e t N E C + */ +static inline int Constraints_getNEC( Constraints* _THIS ) +{ + return Constraints_getNumberOfType( _THIS,ST_EQUALITY ); +} + + +/* + * g e t N I C + */ +static inline int Constraints_getNIC( Constraints* _THIS ) +{ + return Constraints_getNumberOfType( _THIS,ST_BOUNDED ); +} + + +/* + * g e t N U C + */ +static inline int Constraints_getNUC( Constraints* _THIS ) +{ + return Constraints_getNumberOfType( _THIS,ST_UNBOUNDED ); +} + + +/* + * g e t N A C + */ +static inline int Constraints_getNAC( Constraints* _THIS ) +{ + return Indexlist_getLength( _THIS->active ); +} + + +/* + * g e t N I A C + */ +static inline int Constraints_getNIAC( Constraints* _THIS ) +{ + return Indexlist_getLength( _THIS->inactive ); +} + + + +/* + * g e t A c t i v e + */ +static inline Indexlist* Constraints_getActive( Constraints* _THIS ) +{ + return _THIS->active; +} + + +/* + * g e t I n a c t i v e + */ +static inline Indexlist* Constraints_getInactive( Constraints* _THIS ) +{ + return _THIS->inactive; +} + + +END_NAMESPACE_QPOASES + + +#endif /* QPOASES_CONSTRAINTS_H */ + + +/* + * end of file + */ diff --git a/phonelibs/acados/include/qpOASES_e/Flipper.h b/phonelibs/acados/include/qpOASES_e/Flipper.h new file mode 100644 index 0000000000..63526a30f3 --- /dev/null +++ b/phonelibs/acados/include/qpOASES_e/Flipper.h @@ -0,0 +1,129 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, + * Christian Kirches et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file include/qpOASES_e/Flipper.h + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches + * \version 3.1embedded + * \date 2007-2015 + * + * Declaration of the Options class designed to manage user-specified + * options for solving a QProblem. + */ + + +#ifndef QPOASES_FLIPPER_H +#define QPOASES_FLIPPER_H + + +#include +#include + + +BEGIN_NAMESPACE_QPOASES + + +/** + * \brief Auxiliary class for storing a copy of the current matrix factorisations. + * + * This auxiliary class stores a copy of the current matrix factorisations. It + * is used by the classe QProblemB and QProblem in case flipping bounds are enabled. + * + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches + * \version 3.1embedded + * \date 2007-2015 + */ +typedef struct +{ + Bounds *bounds; /**< Data structure for problem's bounds. */ + Constraints *constraints; /**< Data structure for problem's constraints. */ + + real_t *R; /**< Cholesky factor of H (i.e. H = R^T*R). */ + real_t *Q; /**< Orthonormal quadratic matrix, A = [0 T]*Q'. */ + real_t *T; /**< Reverse triangular matrix, A = [0 T]*Q'. */ + + unsigned int nV; /**< Number of variables. */ + unsigned int nC; /**< Number of constraints. */ +} Flipper; + +int Flipper_calculateMemorySize( unsigned int nV, unsigned int nC ); + +char *Flipper_assignMemory( unsigned int nV, unsigned int nC, Flipper **mem, void *raw_memory ); + +Flipper *Flipper_createMemory( unsigned int nV, unsigned int nC ); + +/** Constructor which takes the number of bounds and constraints. */ +void FlipperCON( Flipper* _THIS, + unsigned int _nV, /**< Number of bounds. */ + unsigned int _nC /**< Number of constraints. */ + ); + +/** Copy constructor (deep copy). */ +void FlipperCPY( Flipper* FROM, + Flipper* TO + ); + +/** Initialises object with given number of bounds and constraints. + * \return SUCCESSFUL_RETURN \n + RET_INVALID_ARGUMENTS */ +returnValue Flipper_init( Flipper* _THIS, + unsigned int _nV, /**< Number of bounds. */ + unsigned int _nC /**< Number of constraints. */ + ); + + +/** Copies current values to non-null arguments (assumed to be allocated with consistent size). + * \return SUCCESSFUL_RETURN */ +returnValue Flipper_get( Flipper* _THIS, + Bounds* const _bounds, /**< Pointer to new bounds. */ + real_t* const R, /**< New matrix R. */ + Constraints* const _constraints, /**< Pointer to new constraints. */ + real_t* const _Q, /**< New matrix Q. */ + real_t* const _T /**< New matrix T. */ + ); + +/** Assigns new values to non-null arguments. + * \return SUCCESSFUL_RETURN */ +returnValue Flipper_set( Flipper* _THIS, + const Bounds* const _bounds, /**< Pointer to new bounds. */ + const real_t* const _R, /**< New matrix R. */ + const Constraints* const _constraints, /**< Pointer to new constraints. */ + const real_t* const _Q, /**< New matrix Q. */ + const real_t* const _T /**< New matrix T. */ + ); + +/** Returns dimension of matrix T. + * \return Dimension of matrix T. */ +unsigned int Flipper_getDimT( Flipper* _THIS ); + + +END_NAMESPACE_QPOASES + + +#endif /* QPOASES_FLIPPER_H */ + + +/* + * end of file + */ diff --git a/phonelibs/acados/include/qpOASES_e/Indexlist.h b/phonelibs/acados/include/qpOASES_e/Indexlist.h new file mode 100644 index 0000000000..c3026a7ffc --- /dev/null +++ b/phonelibs/acados/include/qpOASES_e/Indexlist.h @@ -0,0 +1,221 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, + * Christian Kirches et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file include/qpOASES_e/Indexlist.h + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches + * \version 3.1embedded + * \date 2007-2015 + * + * Declaration of the Indexlist class designed to manage index lists of + * constraints and bounds within a SubjectTo object. + */ + + +#ifndef QPOASES_INDEXLIST_H +#define QPOASES_INDEXLIST_H + + +#include + + +BEGIN_NAMESPACE_QPOASES + + +/** + * \brief Stores and manages index lists. + * + * This class manages index lists of active/inactive bounds/constraints. + * + * \author Hans Joachim Ferreau + * \version 3.1embedded + * \date 2007-2015 + */ +typedef struct +{ + int *number; /**< Array to store numbers of constraints or bounds. */ + int *iSort; /**< Index list to sort vector \a number */ + + int length; /**< Length of index list. */ + int first; /**< Physical index of first element. */ + int last; /**< Physical index of last element. */ + int lastusedindex; /**< Physical index of last entry in index list. */ + int physicallength; /**< Physical length of index list. */ +} Indexlist; + +int Indexlist_calculateMemorySize( int n); + +char *Indexlist_assignMemory(int n, Indexlist **mem, void *raw_memory); + +Indexlist *Indexlist_createMemory( int n ); + +/** Constructor which takes the desired physical length of the index list. */ +void IndexlistCON( Indexlist* _THIS, + int n /**< Physical length of index list. */ + ); + +/** Copies all members from given rhs object. + * \return SUCCESSFUL_RETURN */ +void IndexlistCPY( Indexlist* FROM, + Indexlist* TO + ); + +/** Initialises index list of desired physical length. + * \return SUCCESSFUL_RETURN \n + RET_INVALID_ARGUMENTS */ +returnValue Indexlist_init( Indexlist* _THIS, + int n /**< Physical length of index list. */ + ); + +/** Creates an array of all numbers within the index set in correct order. + * \return SUCCESSFUL_RETURN \n + RET_INDEXLIST_CORRUPTED */ +returnValue Indexlist_getNumberArray( Indexlist* _THIS, + int** const numberarray /**< Output: Array of numbers (NULL on error). */ + ); + +/** Creates an array of all numbers within the index set in correct order. + * \return SUCCESSFUL_RETURN \n + RET_INDEXLIST_CORRUPTED */ +returnValue Indexlist_getISortArray( Indexlist* _THIS, + int** const iSortArray /**< Output: iSort Array. */ + ); + + +/** Determines the index within the index list at which a given number is stored. + * \return >= 0: Index of given number. \n + -1: Number not found. */ +int Indexlist_getIndex( Indexlist* _THIS, + int givennumber /**< Number whose index shall be determined. */ + ); + +/** Returns the number stored at a given physical index. + * \return >= 0: Number stored at given physical index. \n + -RET_INDEXLIST_OUTOFBOUNDS */ +static inline int Indexlist_getNumber( Indexlist* _THIS, + int physicalindex /**< Physical index of the number to be returned. */ + ); + + +/** Returns the current length of the index list. + * \return Current length of the index list. */ +static inline int Indexlist_getLength( Indexlist* _THIS + ); + +/** Returns last number within the index list. + * \return Last number within the index list. */ +static inline int Indexlist_getLastNumber( Indexlist* _THIS + ); + + +/** Adds number to index list. + * \return SUCCESSFUL_RETURN \n + RET_INDEXLIST_MUST_BE_REORDERD \n + RET_INDEXLIST_EXCEEDS_MAX_LENGTH */ +returnValue Indexlist_addNumber( Indexlist* _THIS, + int addnumber /**< Number to be added. */ + ); + +/** Removes number from index list. + * \return SUCCESSFUL_RETURN */ +returnValue Indexlist_removeNumber( Indexlist* _THIS, + int removenumber /**< Number to be removed. */ + ); + +/** Swaps two numbers within index list. + * \return SUCCESSFUL_RETURN */ +returnValue Indexlist_swapNumbers( Indexlist* _THIS, + int number1, /**< First number for swapping. */ + int number2 /**< Second number for swapping. */ + ); + +/** Determines if a given number is contained in the index set. + * \return BT_TRUE iff number is contain in the index set */ +static inline BooleanType Indexlist_isMember( Indexlist* _THIS, + int _number /**< Number to be tested for membership. */ + ); + + +/** Find first index j between -1 and length in sorted list of indices + * iSort such that numbers[iSort[j]] <= i < numbers[iSort[j+1]]. Uses + * bisection. + * \return j. */ +int Indexlist_findInsert( Indexlist* _THIS, + int i + ); + + + +/* + * g e t N u m b e r + */ +static inline int Indexlist_getNumber( Indexlist* _THIS, int physicalindex ) +{ + /* consistency check */ + if ( ( physicalindex < 0 ) || ( physicalindex > _THIS->length ) ) + return -RET_INDEXLIST_OUTOFBOUNDS; + + return _THIS->number[physicalindex]; +} + + +/* + * g e t L e n g t h + */ +static inline int Indexlist_getLength( Indexlist* _THIS ) +{ + return _THIS->length; +} + + +/* + * g e t L a s t N u m b e r + */ +static inline int Indexlist_getLastNumber( Indexlist* _THIS ) +{ + return _THIS->number[_THIS->length-1]; +} + + +/* + * g e t L a s t N u m b e r + */ +static inline BooleanType Indexlist_isMember( Indexlist* _THIS, int _number ) +{ + if ( Indexlist_getIndex( _THIS,_number ) >= 0 ) + return BT_TRUE; + else + return BT_FALSE; +} + + +END_NAMESPACE_QPOASES + + +#endif /* QPOASES_INDEXLIST_H */ + + +/* + * end of file + */ diff --git a/phonelibs/acados/include/qpOASES_e/Matrices.h b/phonelibs/acados/include/qpOASES_e/Matrices.h new file mode 100644 index 0000000000..1d6da8c3c1 --- /dev/null +++ b/phonelibs/acados/include/qpOASES_e/Matrices.h @@ -0,0 +1,287 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, + * Christian Kirches et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file include/qpOASES_e/Matrices.h + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches + * \version 3.1embedded + * \date 2009-2015 + * + * Various matrix classes: Abstract base matrix class, dense and sparse matrices, + * including symmetry exploiting specializations. + */ + + + +#ifndef QPOASES_MATRICES_H +#define QPOASES_MATRICES_H + +#ifdef __USE_SINGLE_PRECISION__ + + // single precision + #define GEMM sgemm_ + #define GEMV sgemv_ +// #define SYR ssyr_ +// #define SYR2 ssyr2_ + #define POTRF spotrf_ + +#else + + // double precision + #define GEMM dgemm_ + #define GEMV dgemv_ +// #define SYR dsyr_ +// #define SYR2 dsyr2_ + #define POTRF dpotrf_ + +#endif /* __USE_SINGLE_PRECISION__ */ + + +#ifdef EXTERNAL_BLAS + // double precision + void dgemm_(char *ta, char *tb, int *m, int *n, int *k, double *alpha, double *A, int *lda, double *B, int ldb, double *beta, double *C, int *ldc); + void dgemv_(char *ta, int *m, int *n, double *alpha, double *A, int *lda, double *x, int *incx, double *beta, double *y, int *incy); + void dpotrf_(char *uplo, int *m, double *A, int *lda, int *info); + // single precision + void sgemm_(char *ta, char *tb, int *m, int *n, int *k, float *alpha, float *A, int *lda, float *B, int ldb, float *beta, float *C, int *ldc); + void sgemv_(char *ta, int *m, int *n, float *alpha, float *A, int *lda, float *x, int *incx, float *beta, float *y, int *incy); + void spotrf_(char *uplo, int *m, float *A, int *lda, int *info); +#else + /** Performs one of the matrix-matrix operation in double precision. */ + void dgemm_ ( const char*, const char*, const unsigned long*, const unsigned long*, const unsigned long*, + const double*, const double*, const unsigned long*, const double*, const unsigned long*, + const double*, double*, const unsigned long* ); + /** Performs one of the matrix-matrix operation in single precision. */ + void sgemm_ ( const char*, const char*, const unsigned long*, const unsigned long*, const unsigned long*, + const float*, const float*, const unsigned long*, const float*, const unsigned long*, + const float*, float*, const unsigned long* ); + + /** Calculates the Cholesky factorization of a real symmetric positive definite matrix in double precision. */ + void dpotrf_ ( const char *, const unsigned long *, double *, const unsigned long *, long * ); + /** Calculates the Cholesky factorization of a real symmetric positive definite matrix in single precision. */ + void spotrf_ ( const char *, const unsigned long *, float *, const unsigned long *, long * ); + +#endif + + /** Performs a symmetric rank 1 operation in double precision. */ +// void dsyr_ ( const char *, const unsigned long *, const double *, const double *, +// const unsigned long *, double *, const unsigned long *); + /** Performs a symmetric rank 1 operation in single precision. */ +// void ssyr_ ( const char *, const unsigned long *, const float *, const float *, +// const unsigned long *, float *, const unsigned long *); + + /** Performs a symmetric rank 2 operation in double precision. */ +// void dsyr2_ ( const char *, const unsigned long *, const double *, const double *, +// const unsigned long *, const double *, const unsigned long *, double *, const unsigned long *); + /** Performs a symmetric rank 2 operation in single precision. */ +// void ssyr2_ ( const char *, const unsigned long *, const float *, const float *, +// const unsigned long *, const float *, const unsigned long *, float *, const unsigned long *); + + +#include + + +BEGIN_NAMESPACE_QPOASES + + +/** + * \brief Interfaces matrix-vector operations tailored to general dense matrices. + * + * Dense matrix class (row major format). + * + * \author Andreas Potschka, Christian Kirches, Hans Joachim Ferreau + * \version 3.1embedded + * \date 2011-2015 + */ +typedef struct +{ + real_t *val; /**< Vector of entries. */ + int nRows; /**< Number of rows. */ + int nCols; /**< Number of columns. */ + int leaDim; /**< Leading dimension. */ +} DenseMatrix; + +int DenseMatrix_calculateMemorySize( int m, int n ); + +char *DenseMatrix_assignMemory( int m, int n, DenseMatrix **mem, void *raw_memory ); + +DenseMatrix *DenseMatrix_createMemory( int m, int n ); + +/** Constructor from vector of values. + * Caution: Data pointer must be valid throughout lifetime + */ +void DenseMatrixCON( DenseMatrix* _THIS, + int m, /**< Number of rows. */ + int n, /**< Number of columns. */ + int lD, /**< Leading dimension. */ + real_t *v /**< Values. */ + ); + +void DenseMatrixCPY( DenseMatrix* FROM, + DenseMatrix* TO + ); + + +/** Frees all internal memory. */ +void DenseMatrix_free( DenseMatrix* _THIS ); + +/** Constructor from vector of values. + * Caution: Data pointer must be valid throughout lifetime + */ +returnValue DenseMatrix_init( DenseMatrix* _THIS, + int m, /**< Number of rows. */ + int n, /**< Number of columns. */ + int lD, /**< Leading dimension. */ + real_t *v /**< Values. */ + ); + + +/** Returns i-th diagonal entry. + * \return i-th diagonal entry */ +real_t DenseMatrix_diag( DenseMatrix* _THIS, + int i /**< Index. */ + ); + +/** Checks whether matrix is square and diagonal. + * \return BT_TRUE iff matrix is square and diagonal; \n + * BT_FALSE otherwise. */ +BooleanType DenseMatrix_isDiag( DenseMatrix* _THIS ); + +/** Get the N-norm of the matrix + * \return N-norm of the matrix + */ +real_t DenseMatrix_getNorm( DenseMatrix* _THIS, + int type /**< Norm type, 1: one-norm, 2: Euclidean norm. */ + ); + +/** Get the N-norm of a row + * \return N-norm of row \a rNum + */ +real_t DenseMatrix_getRowNorm( DenseMatrix* _THIS, + int rNum, /**< Row number. */ + int type /**< Norm type, 1: one-norm, 2: Euclidean norm. */ + ); + +/** Retrieve indexed entries of matrix row multiplied by alpha. + * \return SUCCESSFUL_RETURN */ +returnValue DenseMatrix_getRow( DenseMatrix* _THIS, + int rNum, /**< Row number. */ + const Indexlist* const icols, /**< Index list specifying columns. */ + real_t alpha, /**< Scalar factor. */ + real_t *row /**< Output row vector. */ + ); + +/** Retrieve indexed entries of matrix column multiplied by alpha. + * \return SUCCESSFUL_RETURN */ + returnValue DenseMatrix_getCol( DenseMatrix* _THIS, + int cNum, /**< Column number. */ + const Indexlist* const irows, /**< Index list specifying rows. */ + real_t alpha, /**< Scalar factor. */ + real_t *col /**< Output column vector. */ + ); + +/** Evaluate Y=alpha*A*X + beta*Y. + * \return SUCCESSFUL_RETURN. */ +returnValue DenseMatrix_times( DenseMatrix* _THIS, + int xN, /**< Number of vectors to multiply. */ + real_t alpha, /**< Scalar factor for matrix vector product. */ + const real_t *x, /**< Input vector to be multiplied. */ + int xLD, /**< Leading dimension of input x. */ + real_t beta, /**< Scalar factor for y. */ + real_t *y, /**< Output vector of results. */ + int yLD /**< Leading dimension of output y. */ + ); + +/** Evaluate Y=alpha*A'*X + beta*Y. + * \return SUCCESSFUL_RETURN. */ +returnValue DenseMatrix_transTimes( DenseMatrix* _THIS, + int xN, /**< Number of vectors to multiply. */ + real_t alpha, /**< Scalar factor for matrix vector product. */ + const real_t *x, /**< Input vector to be multiplied. */ + int xLD, /**< Leading dimension of input x. */ + real_t beta, /**< Scalar factor for y. */ + real_t *y, /**< Output vector of results. */ + int yLD /**< Leading dimension of output y. */ + ); + +/** Evaluate matrix vector product with submatrix given by Indexlist. + * \return SUCCESSFUL_RETURN */ + returnValue DenseMatrix_subTimes( DenseMatrix* _THIS, + const Indexlist* const irows, /**< Index list specifying rows. */ + const Indexlist* const icols, /**< Index list specifying columns. */ + int xN, /**< Number of vectors to multiply. */ + real_t alpha, /**< Scalar factor for matrix vector product. */ + const real_t *x, /**< Input vector to be multiplied. */ + int xLD, /**< Leading dimension of input x. */ + real_t beta, /**< Scalar factor for y. */ + real_t *y, /**< Output vector of results. */ + int yLD, /**< Leading dimension of output y. */ + BooleanType yCompr /**< Compressed storage for y. */ + ); + +/** Evaluate matrix transpose vector product. + * \return SUCCESSFUL_RETURN */ +returnValue DenseMatrix_subTransTimes( DenseMatrix* _THIS, + const Indexlist* const irows, /**< Index list specifying rows. */ + const Indexlist* const icols, /**< Index list specifying columns. */ + int xN, /**< Number of vectors to multiply. */ + real_t alpha, /**< Scalar factor for matrix vector product. */ + const real_t *x, /**< Input vector to be multiplied. */ + int xLD, /**< Leading dimension of input x. */ + real_t beta, /**< Scalar factor for y. */ + real_t *y, /**< Output vector of results. */ + int yLD /**< Leading dimension of output y. */ + ); + +/** Adds given offset to diagonal of matrix. + * \return SUCCESSFUL_RETURN \n + RET_NO_DIAGONAL_AVAILABLE */ +returnValue DenseMatrix_addToDiag( DenseMatrix* _THIS, + real_t alpha /**< Diagonal offset. */ + ); + +/** Prints matrix to screen. + * \return SUCCESSFUL_RETURN */ +returnValue DenseMatrix_print( DenseMatrix* _THIS + ); + +static inline real_t* DenseMatrix_getVal( DenseMatrix* _THIS ) { return _THIS->val; } + +/** Compute bilinear form y = x'*H*x using submatrix given by index list. + * \return SUCCESSFUL_RETURN */ +returnValue DenseMatrix_bilinear( DenseMatrix* _THIS, + const Indexlist* const icols, /**< Index list specifying columns of x. */ + int xN, /**< Number of vectors to multiply. */ + const real_t *x, /**< Input vector to be multiplied (uncompressed). */ + int xLD, /**< Leading dimension of input x. */ + real_t *y, /**< Output vector of results (compressed). */ + int yLD /**< Leading dimension of output y. */ + ); + + + +END_NAMESPACE_QPOASES + + +#endif /* QPOASES_MATRICES_H */ diff --git a/phonelibs/acados/include/qpOASES_e/MessageHandling.h b/phonelibs/acados/include/qpOASES_e/MessageHandling.h new file mode 100644 index 0000000000..fe5524948a --- /dev/null +++ b/phonelibs/acados/include/qpOASES_e/MessageHandling.h @@ -0,0 +1,544 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, + * Christian Kirches et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file include/qpOASES_e/MessageHandling.h + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches (thanks to Leonard Wirsching) + * \version 3.1embedded + * \date 2007-2015 + * + * Declaration of the MessageHandling class including global return values. + */ + + +#ifndef QPOASES_MESSAGEHANDLING_H +#define QPOASES_MESSAGEHANDLING_H + + +#include +#include +#include +#include + +#include + + +BEGIN_NAMESPACE_QPOASES + + +/** Default file to display messages. */ +#define stdFile stderr + + +/** + * \brief Defines all symbols for global return values. + * + * The enumeration returnValueType defines all symbols for global return values. + * Important: All return values are assumed to be nonnegative! + * + * \author Hans Joachim Ferreau + */ +typedef enum +{ +TERMINAL_LIST_ELEMENT = -1, /**< Terminal list element, internal usage only! */ +/* miscellaneous */ +SUCCESSFUL_RETURN = 0, /**< Successful return. */ +RET_DIV_BY_ZERO, /**< Division by zero. */ +RET_INDEX_OUT_OF_BOUNDS, /**< Index out of bounds. */ +RET_INVALID_ARGUMENTS, /**< At least one of the arguments is invalid. */ +RET_ERROR_UNDEFINED, /**< Error number undefined. */ +RET_WARNING_UNDEFINED, /**< Warning number undefined. */ +RET_INFO_UNDEFINED, /**< Info number undefined. */ +RET_EWI_UNDEFINED, /**< Error/warning/info number undefined. */ +RET_AVAILABLE_WITH_LINUX_ONLY, /**< This function is available under Linux only. */ +RET_UNKNOWN_BUG, /**< The error occurred is not yet known. */ +RET_PRINTLEVEL_CHANGED, /**< Print level changed. (10) */ +RET_NOT_YET_IMPLEMENTED, /**< Requested function is not yet implemented in this version of qpOASES. */ +/* Indexlist */ +RET_INDEXLIST_MUST_BE_REORDERD, /**< Index list has to be reordered. */ +RET_INDEXLIST_EXCEEDS_MAX_LENGTH, /**< Index list exceeds its maximal physical length. */ +RET_INDEXLIST_CORRUPTED, /**< Index list corrupted. */ +RET_INDEXLIST_OUTOFBOUNDS, /**< Physical index is out of bounds. */ +RET_INDEXLIST_ADD_FAILED, /**< Adding indices from another index set failed. */ +RET_INDEXLIST_INTERSECT_FAILED, /**< Intersection with another index set failed. */ +/* SubjectTo / Bounds / Constraints */ +RET_INDEX_ALREADY_OF_DESIRED_STATUS, /**< Index is already of desired status. (18) */ +RET_ADDINDEX_FAILED, /**< Adding index to index set failed. */ +RET_REMOVEINDEX_FAILED, /**< Removing index from index set failed. (20) */ +RET_SWAPINDEX_FAILED, /**< Cannot swap between different indexsets. */ +RET_NOTHING_TO_DO, /**< Nothing to do. */ +RET_SETUP_BOUND_FAILED, /**< Setting up bound index failed. */ +RET_SETUP_CONSTRAINT_FAILED, /**< Setting up constraint index failed. */ +RET_MOVING_BOUND_FAILED, /**< Moving bound between index sets failed. */ +RET_MOVING_CONSTRAINT_FAILED, /**< Moving constraint between index sets failed. */ +RET_SHIFTING_FAILED, /**< Shifting of bounds/constraints failed. */ +RET_ROTATING_FAILED, /**< Rotating of bounds/constraints failed. */ +/* QProblem */ +RET_QPOBJECT_NOT_SETUP, /**< The QP object has not been setup correctly, use another constructor. */ +RET_QP_ALREADY_INITIALISED, /**< QProblem has already been initialised. (30) */ +RET_NO_INIT_WITH_STANDARD_SOLVER, /**< Initialisation via extern QP solver is not yet implemented. */ +RET_RESET_FAILED, /**< Reset failed. */ +RET_INIT_FAILED, /**< Initialisation failed. */ +RET_INIT_FAILED_TQ, /**< Initialisation failed due to TQ factorisation. */ +RET_INIT_FAILED_CHOLESKY, /**< Initialisation failed due to Cholesky decomposition. */ +RET_INIT_FAILED_HOTSTART, /**< Initialisation failed! QP could not be solved! */ +RET_INIT_FAILED_INFEASIBILITY, /**< Initial QP could not be solved due to infeasibility! */ +RET_INIT_FAILED_UNBOUNDEDNESS, /**< Initial QP could not be solved due to unboundedness! */ +RET_INIT_FAILED_REGULARISATION, /**< Initialisation failed as Hessian matrix could not be regularised. */ +RET_INIT_SUCCESSFUL, /**< Initialisation done. (40) */ +RET_OBTAINING_WORKINGSET_FAILED, /**< Failed to obtain working set for auxiliary QP. */ +RET_SETUP_WORKINGSET_FAILED, /**< Failed to setup working set for auxiliary QP. */ +RET_SETUP_AUXILIARYQP_FAILED, /**< Failed to setup auxiliary QP for initialised homotopy. */ +RET_NO_CHOLESKY_WITH_INITIAL_GUESS, /**< Externally computed Cholesky factor cannot be combined with an initial guess. */ +RET_NO_EXTERN_SOLVER, /**< No extern QP solver available. */ +RET_QP_UNBOUNDED, /**< QP is unbounded. */ +RET_QP_INFEASIBLE, /**< QP is infeasible. */ +RET_QP_NOT_SOLVED, /**< Problems occurred while solving QP with standard solver. */ +RET_QP_SOLVED, /**< QP successfully solved. */ +RET_UNABLE_TO_SOLVE_QP, /**< Problems occurred while solving QP. (50) */ +RET_INITIALISATION_STARTED, /**< Starting problem initialisation... */ +RET_HOTSTART_FAILED, /**< Unable to perform homotopy due to internal error. */ +RET_HOTSTART_FAILED_TO_INIT, /**< Unable to initialise problem. */ +RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED, /**< Unable to perform homotopy as previous QP is not solved. */ +RET_ITERATION_STARTED, /**< Iteration... */ +RET_SHIFT_DETERMINATION_FAILED, /**< Determination of shift of the QP data failed. */ +RET_STEPDIRECTION_DETERMINATION_FAILED, /**< Determination of step direction failed. */ +RET_STEPLENGTH_DETERMINATION_FAILED, /**< Determination of step direction failed. */ +RET_OPTIMAL_SOLUTION_FOUND, /**< Optimal solution of neighbouring QP found. */ +RET_HOMOTOPY_STEP_FAILED, /**< Unable to perform homotopy step. (60) */ +RET_HOTSTART_STOPPED_INFEASIBILITY, /**< Premature homotopy termination because QP is infeasible. */ +RET_HOTSTART_STOPPED_UNBOUNDEDNESS, /**< Premature homotopy termination because QP is unbounded. */ +RET_WORKINGSET_UPDATE_FAILED, /**< Unable to update working sets according to initial guesses. */ +RET_MAX_NWSR_REACHED, /**< Maximum number of working set recalculations performed. */ +RET_CONSTRAINTS_NOT_SPECIFIED, /**< Problem does comprise constraints! You also have to specify new constraints' bounds. */ +RET_INVALID_FACTORISATION_FLAG, /**< Invalid factorisation flag. */ +RET_UNABLE_TO_SAVE_QPDATA, /**< Unable to save QP data. */ +RET_STEPDIRECTION_FAILED_TQ, /**< Abnormal termination due to TQ factorisation. */ +RET_STEPDIRECTION_FAILED_CHOLESKY, /**< Abnormal termination due to Cholesky factorisation. */ +RET_CYCLING_DETECTED, /**< Cycling detected. (70) */ +RET_CYCLING_NOT_RESOLVED, /**< Cycling cannot be resolved, QP probably infeasible. */ +RET_CYCLING_RESOLVED, /**< Cycling probably resolved. */ +RET_STEPSIZE, /**< For displaying performed stepsize. */ +RET_STEPSIZE_NONPOSITIVE, /**< For displaying non-positive stepsize. */ +RET_SETUPSUBJECTTOTYPE_FAILED, /**< Setup of SubjectToTypes failed. */ +RET_ADDCONSTRAINT_FAILED, /**< Addition of constraint to working set failed. */ +RET_ADDCONSTRAINT_FAILED_INFEASIBILITY, /**< Addition of constraint to working set failed (due to QP infeasibility). */ +RET_ADDBOUND_FAILED, /**< Addition of bound to working set failed. */ +RET_ADDBOUND_FAILED_INFEASIBILITY, /**< Addition of bound to working set failed (due to QP infeasibility). */ +RET_REMOVECONSTRAINT_FAILED, /**< Removal of constraint from working set failed. (80) */ +RET_REMOVEBOUND_FAILED, /**< Removal of bound from working set failed. */ +RET_REMOVE_FROM_ACTIVESET, /**< Removing from active set... */ +RET_ADD_TO_ACTIVESET, /**< Adding to active set... */ +RET_REMOVE_FROM_ACTIVESET_FAILED, /**< Removing from active set failed. */ +RET_ADD_TO_ACTIVESET_FAILED, /**< Adding to active set failed. */ +RET_CONSTRAINT_ALREADY_ACTIVE, /**< Constraint is already active. */ +RET_ALL_CONSTRAINTS_ACTIVE, /**< All constraints are active, no further constraint can be added. */ +RET_LINEARLY_DEPENDENT, /**< New bound/constraint is linearly dependent. */ +RET_LINEARLY_INDEPENDENT, /**< New bound/constraint is linearly independent. */ +RET_LI_RESOLVED, /**< Linear independence of active constraint matrix successfully resolved. (90) */ +RET_ENSURELI_FAILED, /**< Failed to ensure linear independence of active constraint matrix. */ +RET_ENSURELI_FAILED_TQ, /**< Abnormal termination due to TQ factorisation. */ +RET_ENSURELI_FAILED_NOINDEX, /**< QP is infeasible. */ +RET_ENSURELI_FAILED_CYCLING, /**< QP is infeasible. */ +RET_BOUND_ALREADY_ACTIVE, /**< Bound is already active. */ +RET_ALL_BOUNDS_ACTIVE, /**< All bounds are active, no further bound can be added. */ +RET_CONSTRAINT_NOT_ACTIVE, /**< Constraint is not active. */ +RET_BOUND_NOT_ACTIVE, /**< Bound is not active. */ +RET_HESSIAN_NOT_SPD, /**< Projected Hessian matrix not positive definite. */ +RET_HESSIAN_INDEFINITE, /**< Hessian matrix is indefinite. (100) */ +RET_MATRIX_SHIFT_FAILED, /**< Unable to update matrices or to transform vectors. */ +RET_MATRIX_FACTORISATION_FAILED, /**< Unable to calculate new matrix factorisations. */ +RET_PRINT_ITERATION_FAILED, /**< Unable to print information on current iteration. */ +RET_NO_GLOBAL_MESSAGE_OUTPUTFILE, /**< No global message output file initialised. */ +RET_DISABLECONSTRAINTS_FAILED, /**< Unable to disbable constraints. */ +RET_ENABLECONSTRAINTS_FAILED, /**< Unable to enbable constraints. */ +RET_ALREADY_ENABLED, /**< Bound or constraint is already enabled. */ +RET_ALREADY_DISABLED, /**< Bound or constraint is already disabled. */ +RET_NO_HESSIAN_SPECIFIED, /**< No Hessian matrix has been specified. */ +RET_USING_REGULARISATION, /**< Using regularisation as Hessian matrix is not positive definite. (110) */ +RET_EPS_MUST_BE_POSITVE, /**< Eps for regularisation must be sufficiently positive. */ +RET_REGSTEPS_MUST_BE_POSITVE, /**< Maximum number of regularisation steps must be non-negative. */ +RET_HESSIAN_ALREADY_REGULARISED, /**< Hessian has been already regularised. */ +RET_CANNOT_REGULARISE_IDENTITY, /**< Identity Hessian matrix cannot be regularised. */ +RET_CANNOT_REGULARISE_SPARSE, /**< Sparse matrix cannot be regularised as diagonal entry is missing. */ +RET_NO_REGSTEP_NWSR, /**< No additional regularisation step could be performed due to limits. */ +RET_FEWER_REGSTEPS_NWSR, /**< Fewer additional regularisation steps have been performed due to limits. */ +RET_CHOLESKY_OF_ZERO_HESSIAN, /**< Cholesky decomposition of (unregularised) zero Hessian matrix. */ +RET_ZERO_HESSIAN_ASSUMED, /**< Zero Hessian matrix assumed as null pointer passed without specifying hessianType. */ +RET_CONSTRAINTS_ARE_NOT_SCALED, /**< (no longer in use) (120) */ +RET_INITIAL_BOUNDS_STATUS_NYI, /**< (no longer in use) */ +RET_ERROR_IN_CONSTRAINTPRODUCT, /**< Error in user-defined constraint product function. */ +RET_FIX_BOUNDS_FOR_LP, /**< All initial bounds must be fixed when solving an (unregularised) LP. */ +RET_USE_REGULARISATION_FOR_LP, /**< Set options.enableRegularisation=BT_TRUE for solving LPs. */ +/* SQProblem */ +RET_UPDATEMATRICES_FAILED, /**< Unable to update QP matrices. */ +RET_UPDATEMATRICES_FAILED_AS_QP_NOT_SOLVED, /**< Unable to update matrices as previous QP is not solved. */ +/* Utils */ +RET_UNABLE_TO_OPEN_FILE, /**< Unable to open file. */ +RET_UNABLE_TO_WRITE_FILE, /**< Unable to write into file. */ +RET_UNABLE_TO_READ_FILE, /**< Unable to read from file. */ +RET_FILEDATA_INCONSISTENT, /**< File contains inconsistent data. (130) */ +/* Options */ +RET_OPTIONS_ADJUSTED, /**< Options needed to be adjusted for consistency reasons. */ +/* SolutionAnalysis */ +RET_UNABLE_TO_ANALYSE_QPROBLEM, /**< Unable to analyse (S)QProblem(B) object. */ +/* Benchmark */ +RET_NWSR_SET_TO_ONE, /**< Maximum number of working set changes was set to 1. */ +RET_UNABLE_TO_READ_BENCHMARK, /**< Unable to read benchmark data. */ +RET_BENCHMARK_ABORTED, /**< Benchmark aborted. */ +RET_INITIAL_QP_SOLVED, /**< Initial QP solved. */ +RET_QP_SOLUTION_STARTED, /**< Solving QP... */ +RET_BENCHMARK_SUCCESSFUL, /**< Benchmark terminated successfully. */ +/* Sparse matrices */ +RET_NO_DIAGONAL_AVAILABLE, /**< Sparse matrix does not have entries on full diagonal. */ +RET_DIAGONAL_NOT_INITIALISED, /**< Diagonal data of sparse matrix has not been initialised. (140) */ +/* Dropping of infeasible constraints */ +RET_ENSURELI_DROPPED, /**< Linear independence resolved by dropping blocking constraint. */ +/* Simple exitflags */ +RET_SIMPLE_STATUS_P1, /**< QP problem could not be solved within given number of iterations. */ +RET_SIMPLE_STATUS_P0, /**< QP problem solved. */ +RET_SIMPLE_STATUS_M1, /**< QP problem could not be solved due to an internal error. */ +RET_SIMPLE_STATUS_M2, /**< QP problem is infeasible (and thus could not be solved). */ +RET_SIMPLE_STATUS_M3 /**< QP problem is unbounded (and thus could not be solved). (146) */ +} returnValue; + + +/** + * \brief Data structure for entries in global message list. + * + * Data structure for entries in global message list. + * + * \author Hans Joachim Ferreau + * \version 3.1embedded + * \date 2007-2015 + */ +typedef struct +{ + returnValue key; /**< Global return value. */ + const char* data; /**< Corresponding message. */ + VisibilityStatus globalVisibilityStatus; /**< Determines if message can be printed. + * If this value is set to VS_HIDDEN, no message is printed! */ +} ReturnValueList; + + + +/** + * \brief Handles all kind of error messages, warnings and other information. + * + * This class handles all kinds of messages (errors, warnings, infos) initiated + * by qpOASES modules and stores the corresponding global preferences. + * + * \author Hans Joachim Ferreau (thanks to Leonard Wirsching) + * \version 3.1embedded + * \date 2007-2015 + */ +typedef struct +{ + VisibilityStatus errorVisibility; /**< Error messages visible? */ + VisibilityStatus warningVisibility; /**< Warning messages visible? */ + VisibilityStatus infoVisibility; /**< Info messages visible? */ + + FILE* outputFile; /**< Output file for messages. */ + + int errorCount; /**< Counts number of errors (for nicer output only). */ +} MessageHandling; + + + +/** Constructor which takes the desired output file and desired visibility states. */ +void MessageHandlingCON( MessageHandling* _THIS, + FILE* _outputFile, /**< Output file. */ + VisibilityStatus _errorVisibility, /**< Visibility status for error messages. */ + VisibilityStatus _warningVisibility,/**< Visibility status for warning messages. */ + VisibilityStatus _infoVisibility /**< Visibility status for info messages. */ + ); + +void MessageHandlingCPY( MessageHandling* FROM, + MessageHandling* TO + ); + + +/** Prints an error message(a simplified macro THROWERROR is also provided). \n + * Errors are definied as abnormal events which cause an immediate termination of the current (sub) function. + * Errors of a sub function should be commented by the calling function by means of a warning message + * (if this error does not cause an error of the calling function, either)! + * \return Error number returned by sub function call + */ +returnValue MessageHandling_throwError( MessageHandling* _THIS, + returnValue Enumber, /**< Error number returned by sub function call. */ + const char* additionaltext, /**< Additional error text (0, if none). */ + const char* functionname, /**< Name of function which caused the error. */ + const char* filename, /**< Name of file which caused the error. */ + const unsigned long linenumber, /**< Number of line which caused the error.incompatible binary file */ + VisibilityStatus localVisibilityStatus /**< Determines (locally) if error message can be printed to stderr. + * If GLOBAL visibility status of the message is set to VS_HIDDEN, + * no message is printed, anyway! */ + ); + +/** Prints a warning message (a simplified macro THROWWARNING is also provided). + * Warnings are definied as abnormal events which does NOT cause an immediate termination of the current (sub) function. + * \return Warning number returned by sub function call + */ +returnValue MessageHandling_throwWarning( MessageHandling* _THIS, + returnValue Wnumber, /**< Warning number returned by sub function call. */ + const char* additionaltext, /**< Additional warning text (0, if none). */ + const char* functionname, /**< Name of function which caused the warning. */ + const char* filename, /**< Name of file which caused the warning. */ + const unsigned long linenumber, /**< Number of line which caused the warning. */ + VisibilityStatus localVisibilityStatus /**< Determines (locally) if warning message can be printed to stderr. + * If GLOBAL visibility status of the message is set to VS_HIDDEN, + * no message is printed, anyway! */ + ); + +/** Prints a info message (a simplified macro THROWINFO is also provided). + * \return Info number returned by sub function call + */ +returnValue MessageHandling_throwInfo( MessageHandling* _THIS, + returnValue Inumber, /**< Info number returned by sub function call. */ + const char* additionaltext, /**< Additional warning text (0, if none). */ + const char* functionname, /**< Name of function which submitted the info. */ + const char* filename, /**< Name of file which submitted the info. */ + const unsigned long linenumber, /**< Number of line which submitted the info. */ + VisibilityStatus localVisibilityStatus /**< Determines (locally) if info message can be printed to stderr. + * If GLOBAL visibility status of the message is set to VS_HIDDEN, + * no message is printed, anyway! */ + ); + + +/** Resets all preferences to default values. + * \return SUCCESSFUL_RETURN */ +returnValue MessageHandling_reset( MessageHandling* _THIS ); + + +/** Prints a complete list of all messages to output file. + * \return SUCCESSFUL_RETURN */ +returnValue MessageHandling_listAllMessages( MessageHandling* _THIS ); + + +/** Returns visibility status for error messages. + * \return Visibility status for error messages. */ +static inline VisibilityStatus MessageHandling_getErrorVisibilityStatus( MessageHandling* _THIS ); + +/** Returns visibility status for warning messages. + * \return Visibility status for warning messages. */ +static inline VisibilityStatus MessageHandling_getWarningVisibilityStatus( MessageHandling* _THIS ); + +/** Returns visibility status for info messages. + * \return Visibility status for info messages. */ +static inline VisibilityStatus MessageHandling_getInfoVisibilityStatus( MessageHandling* _THIS ); + +/** Returns pointer to output file. + * \return Pointer to output file. */ +static inline FILE* MessageHandling_getOutputFile( MessageHandling* _THIS ); + +/** Returns error count value. + * \return Error count value. */ +static inline int MessageHandling_getErrorCount( MessageHandling* _THIS ); + + +/** Changes visibility status for error messages. */ +static inline void MessageHandling_setErrorVisibilityStatus( MessageHandling* _THIS, + VisibilityStatus _errorVisibility /**< New visibility status for error messages. */ + ); + +/** Changes visibility status for warning messages. */ +static inline void MessageHandling_setWarningVisibilityStatus( MessageHandling* _THIS, + VisibilityStatus _warningVisibility /**< New visibility status for warning messages. */ + ); + +/** Changes visibility status for info messages. */ +static inline void MessageHandling_setInfoVisibilityStatus( MessageHandling* _THIS, + VisibilityStatus _infoVisibility /**< New visibility status for info messages. */ + ); + +/** Changes output file for messages. */ +static inline void MessageHandling_setOutputFile( MessageHandling* _THIS, + FILE* _outputFile /**< New output file for messages. */ + ); + +/** Changes error count. + * \return SUCCESSFUL_RETURN \n + * RET_INVALID_ARGUMENT */ +static inline returnValue MessageHandling_setErrorCount( MessageHandling* _THIS, + int _errorCount /**< New error count value. */ + ); + +/** Provides message text corresponding to given \a returnValue. + * \return String containing message text. */ +const char* MessageHandling_getErrorCodeMessage( MessageHandling* _THIS, + const returnValue _returnValue + ); + + +returnValue MessageHandling_throwMessage( MessageHandling* _THIS, + returnValue RETnumber, /**< Error/warning/info number returned by sub function call. */ + const char* additionaltext, /**< Additional warning text (0, if none). */ + const char* functionname, /**< Name of function which caused the error/warning/info. */ + const char* filename, /**< Name of file which caused the error/warning/info. */ + const unsigned long linenumber, /**< Number of line which caused the error/warning/info. */ + VisibilityStatus localVisibilityStatus, /**< Determines (locally) if info message can be printed to stderr. + * If GLOBAL visibility status of the message is set to VS_HIDDEN, + * no message is printed, anyway! */ + const char* RETstring /**< Leading string of error/warning/info message. */ + ); + + +#ifndef __FILE__ + /** Ensures that __FILE__ macro is defined. */ + #define __FILE__ 0 +#endif + +#ifndef __LINE__ + /** Ensures that __LINE__ macro is defined. */ + #define __LINE__ 0 +#endif + +/** Define __FUNC__ macro providing current function for debugging. */ +/*#define __FUNC__ 0*/ +#define __FUNC__ ("(no function name provided)") +/*#define __FUNC__ __func__*/ +/*#define __FUNC__ __FUNCTION__*/ + + +/** Short version of throwError with default values, only returnValue is needed */ +#define THROWERROR(retval) ( MessageHandling_throwError( qpOASES_getGlobalMessageHandler(),(retval),0,__FUNC__,__FILE__,__LINE__,VS_VISIBLE) ) + +/** Short version of throwWarning with default values, only returnValue is needed */ +#define THROWWARNING(retval) ( MessageHandling_throwWarning( qpOASES_getGlobalMessageHandler(),(retval),0,__FUNC__,__FILE__,__LINE__,VS_VISIBLE) ) + +/** Short version of throwInfo with default values, only returnValue is needed */ +#define THROWINFO(retval) ( MessageHandling_throwInfo( qpOASES_getGlobalMessageHandler(),(retval),0,__FUNC__,__FILE__,__LINE__,VS_VISIBLE) ) + + +/** Returns a pointer to global message handler. + * \return Pointer to global message handler. + */ +MessageHandling* qpOASES_getGlobalMessageHandler( ); + + +/* + * g e t E r r o r V i s i b i l i t y S t a t u s + */ +static inline VisibilityStatus MessageHandling_getErrorVisibilityStatus( MessageHandling* _THIS ) +{ + return _THIS->errorVisibility; +} + + +/* + * g e t W a r n i n g V i s i b i l i t y S t a t u s + */ +static inline VisibilityStatus MessageHandling_getWarningVisibilityStatus( MessageHandling* _THIS ) +{ + return _THIS->warningVisibility; +} + + +/* + * g e t I n f o V i s i b i l i t y S t a t u s + */ +static inline VisibilityStatus MessageHandling_getInfoVisibilityStatus( MessageHandling* _THIS ) +{ + return _THIS->infoVisibility; +} + + +/* + * g e t O u t p u t F i l e + */ +static inline FILE* MessageHandling_getOutputFile( MessageHandling* _THIS ) +{ + return _THIS->outputFile; +} + + +/* + * g e t E r r o r C o u n t + */ +static inline int MessageHandling_getErrorCount( MessageHandling* _THIS ) +{ + return _THIS->errorCount; +} + + +/* + * s e t E r r o r V i s i b i l i t y S t a t u s + */ +static inline void MessageHandling_setErrorVisibilityStatus( MessageHandling* _THIS, VisibilityStatus _errorVisibility ) +{ + _THIS->errorVisibility = _errorVisibility; +} + + +/* + * s e t W a r n i n g V i s i b i l i t y S t a t u s + */ +static inline void MessageHandling_setWarningVisibilityStatus( MessageHandling* _THIS, VisibilityStatus _warningVisibility ) +{ + _THIS->warningVisibility = _warningVisibility; +} + + +/* + * s e t I n f o V i s i b i l i t y S t a t u s + */ +static inline void MessageHandling_setInfoVisibilityStatus( MessageHandling* _THIS, VisibilityStatus _infoVisibility ) +{ + _THIS->infoVisibility = _infoVisibility; +} + + +/* + * s e t O u t p u t F i l e + */ +static inline void MessageHandling_setOutputFile( MessageHandling* _THIS, FILE* _outputFile ) +{ + _THIS->outputFile = _outputFile; +} + + +/* + * s e t E r r o r C o u n t + */ +static inline returnValue MessageHandling_setErrorCount( MessageHandling* _THIS, int _errorCount ) +{ + if ( _errorCount >= 0 ) + { + _THIS->errorCount = _errorCount; + return SUCCESSFUL_RETURN; + } + else + return RET_INVALID_ARGUMENTS; +} + + +END_NAMESPACE_QPOASES + + +#endif /* QPOASES_MESSAGEHANDLING_H */ + + +/* + * end of file + */ diff --git a/phonelibs/acados/include/qpOASES_e/Options.h b/phonelibs/acados/include/qpOASES_e/Options.h new file mode 100644 index 0000000000..b471ee0668 --- /dev/null +++ b/phonelibs/acados/include/qpOASES_e/Options.h @@ -0,0 +1,153 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, + * Christian Kirches et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file include/qpOASES_e/Options.h + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches + * \version 3.1embedded + * \date 2007-2015 + * + * Declaration of the Options class designed to manage user-specified + * options for solving a QProblem. + */ + + +#ifndef QPOASES_OPTIONS_H +#define QPOASES_OPTIONS_H + + +#include + + +BEGIN_NAMESPACE_QPOASES + + +/** + * \brief Manages all user-specified options for solving QPs. + * + * This class manages all user-specified options used for solving + * quadratic programs. + * + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches + * \version 3.1embedded + * \date 2007-2015 + */ +typedef struct +{ + PrintLevel printLevel; /**< Print level. */ + + BooleanType enableRamping; /**< Specifies whether ramping shall be enabled or not. */ + BooleanType enableFarBounds; /**< Specifies whether far bounds shall be used or not. */ + BooleanType enableFlippingBounds; /**< Specifies whether flipping bounds shall be used or not. */ + BooleanType enableRegularisation; /**< Specifies whether Hessian matrix shall be regularised in case semi-definiteness is detected. */ + BooleanType enableFullLITests; /**< Specifies whether condition-hardened LI test shall be used or not. */ + BooleanType enableNZCTests; /**< Specifies whether nonzero curvature tests shall be used. */ + int enableDriftCorrection; /**< Specifies the frequency of drift corrections (0 = off). */ + int enableCholeskyRefactorisation; /**< Specifies the frequency of full refactorisation of proj. Hessian (otherwise updates). */ + BooleanType enableEqualities; /**< Specifies whether equalities shall be always treated as active constraints. */ + + real_t terminationTolerance; /**< Termination tolerance. */ + real_t boundTolerance; /**< Lower/upper (constraints') bound tolerance (an inequality constraint whose lower and upper bounds differ by less is regarded to be an equality constraint). */ + real_t boundRelaxation; /**< Offset for relaxing (constraints') bounds at beginning of an initial homotopy. It is also as initial value for far bounds. */ + real_t epsNum; /**< Numerator tolerance for ratio tests. */ + real_t epsDen; /**< Denominator tolerance for ratio tests. */ + real_t maxPrimalJump; /**< Maximum allowed jump in primal variables in nonzero curvature tests. */ + real_t maxDualJump; /**< Maximum allowed jump in dual variables in linear independence tests. */ + + real_t initialRamping; /**< Start value for Ramping Strategy. */ + real_t finalRamping; /**< Final value for Ramping Strategy. */ + real_t initialFarBounds; /**< Initial size of Far Bounds. */ + real_t growFarBounds; /**< Factor to grow Far Bounds. */ + SubjectToStatus initialStatusBounds; /**< Initial status of bounds at first iteration. */ + real_t epsFlipping; /**< Tolerance of squared Cholesky diagonal factor which triggers flipping bound. */ + int numRegularisationSteps; /**< Maximum number of successive regularisation steps. */ + real_t epsRegularisation; /**< Scaling factor of identity matrix used for Hessian regularisation. */ + int numRefinementSteps; /**< Maximum number of iterative refinement steps. */ + real_t epsIterRef; /**< Early termination tolerance for iterative refinement. */ + real_t epsLITests; /**< Tolerance for linear independence tests. */ + real_t epsNZCTests; /**< Tolerance for nonzero curvature tests. */ + + BooleanType enableDropInfeasibles; /**< ... */ + int dropBoundPriority; /**< ... */ + int dropEqConPriority; /**< ... */ + int dropIneqConPriority; /**< ... */ +} Options; + + +void OptionsCON( Options* _THIS + ); + +/** Copies all members from given rhs object. + * \return SUCCESSFUL_RETURN */ +void OptionsCPY( Options* FROM, + Options* TO + ); + + +/** Sets all options to default values. + * \return SUCCESSFUL_RETURN */ +returnValue Options_setToDefault( Options* _THIS + ); + +/** Sets all options to values resulting in maximum reliabilty. + * \return SUCCESSFUL_RETURN */ +returnValue Options_setToReliable( Options* _THIS + ); + +/** Sets all options to values resulting in minimum solution time. + * \return SUCCESSFUL_RETURN */ +returnValue Options_setToMPC( Options* _THIS + ); + +/** Same as setToMPC( ), for ensuring backwards compatibility. + * \return SUCCESSFUL_RETURN */ +returnValue Options_setToFast( Options* _THIS + ); + + +/** Ensures that all options have consistent values by automatically + * adjusting inconsistent ones. + * Note: This routine cannot (and does not try to) ensure that values + * are set to reasonable values that make the QP solution work! + * \return SUCCESSFUL_RETURN \n + * RET_OPTIONS_ADJUSTED */ +returnValue Options_ensureConsistency( Options* _THIS + ); + + +/** Prints values of all options. + * \return SUCCESSFUL_RETURN */ +returnValue Options_print( Options* _THIS + ); + + +END_NAMESPACE_QPOASES + + +#endif /* QPOASES_OPTIONS_H */ + + +/* + * end of file + */ diff --git a/phonelibs/acados/include/qpOASES_e/QProblem.h b/phonelibs/acados/include/qpOASES_e/QProblem.h new file mode 100644 index 0000000000..3c61a4d596 --- /dev/null +++ b/phonelibs/acados/include/qpOASES_e/QProblem.h @@ -0,0 +1,2369 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, + * Christian Kirches et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file include/qpOASES_e/QProblem.h + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches + * \version 3.1embedded + * \date 2007-2015 + * + * Declaration of the QProblem class which is able to use the newly + * developed online active set strategy for parametric quadratic programming. + */ + + + +#ifndef QPOASES_QPROBLEM_H +#define QPOASES_QPROBLEM_H + + +#include +#include +#include +#include +#include +#include + + +BEGIN_NAMESPACE_QPOASES + +typedef struct { + Bounds *auxiliaryBounds; + Constraints *auxiliaryConstraints; + + real_t *ub_new_far; + real_t *lb_new_far; + real_t *ubA_new_far; + real_t *lbA_new_far; + + real_t *g_new; + real_t *lb_new; + real_t *ub_new; + real_t *lbA_new; + real_t *ubA_new; + + real_t *g_new2; + real_t *lb_new2; + real_t *ub_new2; + real_t *lbA_new2; + real_t *ubA_new2; + + real_t *delta_xFX5; + real_t *delta_xFR5; + real_t *delta_yAC5; + real_t *delta_yFX5; + + real_t *Hx; + + real_t *_H; + + real_t *g_original; + real_t *lb_original; + real_t *ub_original; + real_t *lbA_original; + real_t *ubA_original; + + real_t *delta_xFR; + real_t *delta_xFX; + real_t *delta_yAC; + real_t *delta_yFX; + real_t *delta_g; + real_t *delta_lb; + real_t *delta_ub; + real_t *delta_lbA; + real_t *delta_ubA; + + real_t *gMod; + + real_t *aFR; + real_t *wZ; + + real_t *delta_g2; + real_t *delta_xFX2; + real_t *delta_xFR2; + real_t *delta_yAC2; + real_t *delta_yFX2; + real_t *nul; + real_t *Arow; + + real_t *xiC; + real_t *xiC_TMP; + real_t *xiB; + real_t *Arow2; + real_t *num; + + real_t *w; + real_t *tmp; + + real_t *delta_g3; + real_t *delta_xFX3; + real_t *delta_xFR3; + real_t *delta_yAC3; + real_t *delta_yFX3; + real_t *nul2; + + real_t *xiC2; + real_t *xiC_TMP2; + real_t *xiB2; + real_t *num2; + + real_t *Hz; + real_t *z; + real_t *ZHz; + real_t *r; + + real_t *tmp2; + real_t *Hz2; + real_t *z2; + real_t *r2; + real_t *rhs; + + real_t *delta_xFX4; + real_t *delta_xFR4; + real_t *delta_yAC4; + real_t *delta_yFX4; + real_t *nul3; + real_t *ek; + real_t *x_W; + real_t *As; + real_t *Ax_W; + + real_t *num3; + real_t *den; + real_t *delta_Ax_l; + real_t *delta_Ax_u; + real_t *delta_Ax; + real_t *delta_x; + + real_t *_A; + + real_t *grad; + real_t *AX; +} QProblem_ws; + +int QProblem_ws_calculateMemorySize( unsigned int nV, unsigned int nC ); + +char *QProblem_ws_assignMemory( unsigned int nV, unsigned int nC, QProblem_ws **mem, void *raw_memory ); + +QProblem_ws *QProblem_ws_createMemory( unsigned int nV, unsigned int nC ); + +/** + * \brief Implements the online active set strategy for QPs with general constraints. + * + * A class for setting up and solving quadratic programs. The main feature is + * the possibily to use the newly developed online active set strategy for + * parametric quadratic programming. + * + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches + * \version 3.1embedded + * \date 2007-2015 + */ +typedef struct +{ + QProblem_ws *ws; /**< Workspace */ + Bounds *bounds; /**< Data structure for problem's bounds. */ + Constraints *constraints; /**< Data structure for problem's constraints. */ + Flipper *flipper; /**< Struct for making a temporary copy of the matrix factorisations. */ + + DenseMatrix* H; /**< Hessian matrix pointer. */ + DenseMatrix* A; /**< Constraint matrix pointer. */ + + Options options; /**< Struct containing all user-defined options for solving QPs. */ + TabularOutput tabularOutput; /**< Struct storing information for tabular output (printLevel == PL_TABULAR). */ + + real_t *g; /**< Gradient. */ + + real_t *lb; /**< Lower bound vector (on variables). */ + real_t *ub; /**< Upper bound vector (on variables). */ + real_t *lbA; /**< Lower constraints' bound vector. */ + real_t *ubA; /**< Upper constraints' bound vector. */ + + real_t *R; /**< Cholesky factor of H (i.e. H = R^T*R). */ + + real_t *T; /**< Reverse triangular matrix, A = [0 T]*Q'. */ + real_t *Q; /**< Orthonormal quadratic matrix, A = [0 T]*Q'. */ + + real_t *Ax; /**< Stores the current A*x \n + * (for increased efficiency only). */ + real_t *Ax_l; /**< Stores the current distance to lower constraints' bounds A*x-lbA \n + * (for increased efficiency only). */ + real_t *Ax_u; /**< Stores the current distance to lower constraints' bounds ubA-A*x \n + * (for increased efficiency only). */ + + real_t *x; /**< Primal solution vector. */ + real_t *y; /**< Dual solution vector. */ + + real_t *delta_xFR_TMP; /**< Temporary for determineStepDirection */ + real_t *tempA; /**< Temporary for determineStepDirection. */ + real_t *tempB; /**< Temporary for determineStepDirection. */ + real_t *ZFR_delta_xFRz; /**< Temporary for determineStepDirection. */ + real_t *delta_xFRy; /**< Temporary for determineStepDirection. */ + real_t *delta_xFRz; /**< Temporary for determineStepDirection. */ + real_t *delta_yAC_TMP; /**< Temporary for determineStepDirection. */ + + ConstraintProduct constraintProduct; /**< Pointer to user-defined constraint product function. */ + + real_t tau; /**< Last homotopy step length. */ + real_t regVal; /**< Holds the offset used to regularise Hessian matrix (zero by default). */ + + real_t ramp0; /**< Start value for Ramping Strategy. */ + real_t ramp1; /**< Final value for Ramping Strategy. */ + + QProblemStatus status; /**< Current status of the solution process. */ + HessianType hessianType; /**< Type of Hessian matrix. */ + + BooleanType haveCholesky; /**< Flag indicating whether Cholesky decomposition has already been setup. */ + BooleanType infeasible; /**< QP infeasible? */ + BooleanType unbounded; /**< QP unbounded? */ + + int rampOffset; /**< Offset index for Ramping. */ + unsigned int count; /**< Counts the number of hotstart function calls (internal usage only!). */ + + int sizeT; /**< Matrix T is stored in a (sizeT x sizeT) array. */ +} QProblem; + +int QProblem_calculateMemorySize( unsigned int nV, unsigned int nC ); + +char *QProblem_assignMemory( unsigned int nV, unsigned int nC, QProblem **mem, void *raw_memory ); + +QProblem *QProblem_createMemory( unsigned int nV, unsigned int nC ); + + +/** Constructor which takes the QP dimension and Hessian type + * information. If the Hessian is the zero (i.e. HST_ZERO) or the + * identity matrix (i.e. HST_IDENTITY), respectively, no memory + * is allocated for it and a NULL pointer can be passed for it + * to the init() functions. */ +void QProblemCON( QProblem* _THIS, + int _nV, /**< Number of variables. */ + int _nC, /**< Number of constraints. */ + HessianType _hessianType /**< Type of Hessian matrix. */ + ); + +/** Copies all members from given rhs object. + * \return SUCCESSFUL_RETURN */ +void QProblemCPY( QProblem* FROM, + QProblem* TO + ); + + +/** Clears all data structures of QProblem except for QP data. + * \return SUCCESSFUL_RETURN \n + RET_RESET_FAILED */ +returnValue QProblem_reset( QProblem* _THIS ); + + +/** Initialises a QP problem with given QP data and tries to solve it + * using at most nWSR iterations. + * + * Note: This function internally calls solveInitialQP for initialisation! + * + * \return SUCCESSFUL_RETURN \n + RET_INIT_FAILED \n + RET_INIT_FAILED_CHOLESKY \n + RET_INIT_FAILED_TQ \n + RET_INIT_FAILED_HOTSTART \n + RET_INIT_FAILED_INFEASIBILITY \n + RET_INIT_FAILED_UNBOUNDEDNESS \n + RET_MAX_NWSR_REACHED \n + RET_INVALID_ARGUMENTS */ +returnValue QProblem_initM( QProblem* _THIS, + DenseMatrix *_H, /**< Hessian matrix. */ + const real_t* const _g, /**< Gradient vector. */ + DenseMatrix *_A, /**< Constraint matrix. */ + const real_t* const _lb, /**< Lower bound vector (on variables). \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const _ub, /**< Upper bound vector (on variables). \n + If no upper bounds exist, a NULL pointer can be passed. */ + const real_t* const _lbA, /**< Lower constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const real_t* const _ubA, /**< Upper constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. + Output: Number of performed working set recalculations. */ + real_t* const cputime /**< Input: Maximum CPU time allowed for QP initialisation. \n + Output: CPU time spent for QP initialisation (if pointer passed). */ + ); + + +/** Initialises a QP problem with given QP data and tries to solve it + * using at most nWSR iterations. + * + * Note: This function internally calls solveInitialQP for initialisation! + * + * \return SUCCESSFUL_RETURN \n + RET_INIT_FAILED \n + RET_INIT_FAILED_CHOLESKY \n + RET_INIT_FAILED_TQ \n + RET_INIT_FAILED_HOTSTART \n + RET_INIT_FAILED_INFEASIBILITY \n + RET_INIT_FAILED_UNBOUNDEDNESS \n + RET_MAX_NWSR_REACHED \n + RET_INVALID_ARGUMENTS */ +returnValue QProblem_init( QProblem* _THIS, + real_t* const _H, /**< Hessian matrix. \n + If Hessian matrix is trivial, a NULL pointer can be passed. */ + const real_t* const _g, /**< Gradient vector. */ + real_t* const _A, /**< Constraint matrix. */ + const real_t* const _lb, /**< Lower bound vector (on variables). \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const _ub, /**< Upper bound vector (on variables). \n + If no upper bounds exist, a NULL pointer can be passed. */ + const real_t* const _lbA, /**< Lower constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const real_t* const _ubA, /**< Upper constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. + Output: Number of performed working set recalculations. */ + real_t* const cputime /**< Input: Maximum CPU time allowed for QP initialisation. \n + Output: CPU time spent for QP initialisation (if pointer passed). */ + ); + +/** Initialises a QP problem with given QP data to be read from files and tries to solve it + * using at most nWSR iterations. + * + * Note: This function internally calls solveInitialQP for initialisation! + * + * \return SUCCESSFUL_RETURN \n + RET_INIT_FAILED \n + RET_INIT_FAILED_CHOLESKY \n + RET_INIT_FAILED_TQ \n + RET_INIT_FAILED_HOTSTART \n + RET_INIT_FAILED_INFEASIBILITY \n + RET_INIT_FAILED_UNBOUNDEDNESS \n + RET_MAX_NWSR_REACHED \n + RET_UNABLE_TO_READ_FILE */ +returnValue QProblem_initF( QProblem* _THIS, + const char* const H_file, /**< Name of file where Hessian matrix is stored. \n + If Hessian matrix is trivial, a NULL pointer can be passed. */ + const char* const g_file, /**< Name of file where gradient vector is stored. */ + const char* const A_file, /**< Name of file where constraint matrix is stored. */ + const char* const lb_file, /**< Name of file where lower bound vector. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const char* const ub_file, /**< Name of file where upper bound vector. \n + If no upper bounds exist, a NULL pointer can be passed. */ + const char* const lbA_file, /**< Name of file where lower constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const char* const ubA_file, /**< Name of file where upper constraints' bound vector. \n + If no upper constraints' bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. + Output: Number of performed working set recalculations. */ + real_t* const cputime /**< Input: Maximum CPU time allowed for QP initialisation. \n + Output: CPU time spent for QP initialisation (if pointer passed). */ + ); + +/** Initialises a QP problem with given QP data and tries to solve it + * using at most nWSR iterations. Depending on the parameter constellation it: \n + * 1. 0, 0, 0 : starts with xOpt = 0, yOpt = 0 and gB/gC empty (or all implicit equality bounds), \n + * 2. xOpt, 0, 0 : starts with xOpt, yOpt = 0 and obtain gB/gC by "clipping", \n + * 3. 0, yOpt, 0 : starts with xOpt = 0, yOpt and obtain gB/gC from yOpt != 0, \n + * 4. 0, 0, gB/gC: starts with xOpt = 0, yOpt = 0 and gB/gC, \n + * 5. xOpt, yOpt, 0 : starts with xOpt, yOpt and obtain gB/gC from yOpt != 0, \n + * 6. xOpt, 0, gB/gC: starts with xOpt, yOpt = 0 and gB/gC, \n + * 7. xOpt, yOpt, gB/gC: starts with xOpt, yOpt and gB/gC (assume them to be consistent!) + * + * Note: This function internally calls solveInitialQP for initialisation! + * + * \return SUCCESSFUL_RETURN \n + RET_INIT_FAILED \n + RET_INIT_FAILED_CHOLESKY \n + RET_INIT_FAILED_TQ \n + RET_INIT_FAILED_HOTSTART \n + RET_INIT_FAILED_INFEASIBILITY \n + RET_INIT_FAILED_UNBOUNDEDNESS \n + RET_MAX_NWSR_REACHED \n + RET_INVALID_ARGUMENTS */ +returnValue QProblem_initMW( QProblem* _THIS, + DenseMatrix *_H, /**< Hessian matrix. \n + If Hessian matrix is trivial, a NULL pointer can be passed. */ + const real_t* const _g, /**< Gradient vector. */ + DenseMatrix *_A, /**< Constraint matrix. */ + const real_t* const _lb, /**< Lower bound vector (on variables). \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const _ub, /**< Upper bound vector (on variables). \n + If no upper bounds exist, a NULL pointer can be passed. */ + const real_t* const _lbA, /**< Lower constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const real_t* const _ubA, /**< Upper constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. + * Output: Number of performed working set recalculations. */ + real_t* const cputime, /**< Input: Maximum CPU time allowed for QP initialisation. \n + Output: CPU time spent for QP initialisation. */ + const real_t* const xOpt, /**< Optimal primal solution vector. \n + (If a null pointer is passed, the old primal solution is kept!) */ + const real_t* const yOpt, /**< Optimal dual solution vector. \n + (If a null pointer is passed, the old dual solution is kept!) */ + Bounds* const guessedBounds, /**< Optimal working set of bounds for solution (xOpt,yOpt). */ + Constraints* const guessedConstraints, /**< Optimal working set of constraints for solution (xOpt,yOpt). */ + const real_t* const _R /**< Pre-computed (upper triangular) Cholesky factor of Hessian matrix. + The Cholesky factor must be stored in a real_t array of size nV*nV + in row-major format. Note: Only used if xOpt/yOpt and gB are NULL! \n + (If a null pointer is passed, Cholesky decomposition is computed internally!) */ + ); + +/** Initialises a QP problem with given QP data and tries to solve it + * using at most nWSR iterations. Depending on the parameter constellation it: \n + * 1. 0, 0, 0 : starts with xOpt = 0, yOpt = 0 and gB/gC empty (or all implicit equality bounds), \n + * 2. xOpt, 0, 0 : starts with xOpt, yOpt = 0 and obtain gB/gC by "clipping", \n + * 3. 0, yOpt, 0 : starts with xOpt = 0, yOpt and obtain gB/gC from yOpt != 0, \n + * 4. 0, 0, gB/gC: starts with xOpt = 0, yOpt = 0 and gB/gC, \n + * 5. xOpt, yOpt, 0 : starts with xOpt, yOpt and obtain gB/gC from yOpt != 0, \n + * 6. xOpt, 0, gB/gC: starts with xOpt, yOpt = 0 and gB/gC, \n + * 7. xOpt, yOpt, gB/gC: starts with xOpt, yOpt and gB/gC (assume them to be consistent!) + * + * Note: This function internally calls solveInitialQP for initialisation! + * + * \return SUCCESSFUL_RETURN \n + RET_INIT_FAILED \n + RET_INIT_FAILED_CHOLESKY \n + RET_INIT_FAILED_TQ \n + RET_INIT_FAILED_HOTSTART \n + RET_INIT_FAILED_INFEASIBILITY \n + RET_INIT_FAILED_UNBOUNDEDNESS \n + RET_MAX_NWSR_REACHED \n + RET_INVALID_ARGUMENTS */ +returnValue QProblem_initW( QProblem* _THIS, + real_t* const _H, /**< Hessian matrix. \n + If Hessian matrix is trivial, a NULL pointer can be passed. */ + const real_t* const _g, /**< Gradient vector. */ + real_t* const _A, /**< Constraint matrix. */ + const real_t* const _lb, /**< Lower bound vector (on variables). \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const _ub, /**< Upper bound vector (on variables). \n + If no upper bounds exist, a NULL pointer can be passed. */ + const real_t* const _lbA, /**< Lower constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const real_t* const _ubA, /**< Upper constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. + * Output: Number of performed working set recalculations. */ + real_t* const cputime, /**< Input: Maximum CPU time allowed for QP initialisation. \n + Output: CPU time spent for QP initialisation. */ + const real_t* const xOpt, /**< Optimal primal solution vector. \n + (If a null pointer is passed, the old primal solution is kept!) */ + const real_t* const yOpt, /**< Optimal dual solution vector. \n + (If a null pointer is passed, the old dual solution is kept!) */ + Bounds* const guessedBounds, /**< Optimal working set of bounds for solution (xOpt,yOpt). */ + Constraints* const guessedConstraints, /**< Optimal working set of constraints for solution (xOpt,yOpt). */ + const real_t* const _R /**< Pre-computed (upper triangular) Cholesky factor of Hessian matrix. + The Cholesky factor must be stored in a real_t array of size nV*nV + in row-major format. Note: Only used if xOpt/yOpt and gB are NULL! \n + (If a null pointer is passed, Cholesky decomposition is computed internally!) */ + ); + +/** Initialises a QP problem with given QP data to be ream from files and tries to solve it + * using at most nWSR iterations. Depending on the parameter constellation it: \n + * 1. 0, 0, 0 : starts with xOpt = 0, yOpt = 0 and gB/gC empty (or all implicit equality bounds), \n + * 2. xOpt, 0, 0 : starts with xOpt, yOpt = 0 and obtain gB/gC by "clipping", \n + * 3. 0, yOpt, 0 : starts with xOpt = 0, yOpt and obtain gB/gC from yOpt != 0, \n + * 4. 0, 0, gB/gC: starts with xOpt = 0, yOpt = 0 and gB/gC, \n + * 5. xOpt, yOpt, 0 : starts with xOpt, yOpt and obtain gB/gC from yOpt != 0, \n + * 6. xOpt, 0, gB/gC: starts with xOpt, yOpt = 0 and gB/gC, \n + * 7. xOpt, yOpt, gB/gC: starts with xOpt, yOpt and gB/gC (assume them to be consistent!) + * + * Note: This function internally calls solveInitialQP for initialisation! + * + * \return SUCCESSFUL_RETURN \n + RET_INIT_FAILED \n + RET_INIT_FAILED_CHOLESKY \n + RET_INIT_FAILED_TQ \n + RET_INIT_FAILED_HOTSTART \n + RET_INIT_FAILED_INFEASIBILITY \n + RET_INIT_FAILED_UNBOUNDEDNESS \n + RET_MAX_NWSR_REACHED \n + RET_UNABLE_TO_READ_FILE */ +returnValue QProblem_initFW( QProblem* _THIS, + const char* const H_file, /**< Name of file where Hessian matrix is stored. \n + If Hessian matrix is trivial, a NULL pointer can be passed. */ + const char* const g_file, /**< Name of file where gradient vector is stored. */ + const char* const A_file, /**< Name of file where constraint matrix is stored. */ + const char* const lb_file, /**< Name of file where lower bound vector. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const char* const ub_file, /**< Name of file where upper bound vector. \n + If no upper bounds exist, a NULL pointer can be passed. */ + const char* const lbA_file, /**< Name of file where lower constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const char* const ubA_file, /**< Name of file where upper constraints' bound vector. \n + If no upper constraints' bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. + Output: Number of performed working set recalculations. */ + real_t* const cputime, /**< Input: Maximum CPU time allowed for QP initialisation. \n + Output: CPU time spent for QP initialisation. */ + const real_t* const xOpt, /**< Optimal primal solution vector. \n + (If a null pointer is passed, the old primal solution is kept!) */ + const real_t* const yOpt, /**< Optimal dual solution vector. \n + (If a null pointer is passed, the old dual solution is kept!) */ + Bounds* const guessedBounds, /**< Optimal working set of bounds for solution (xOpt,yOpt). */ + Constraints* const guessedConstraints, /**< Optimal working set of constraints for solution (xOpt,yOpt). */ + const char* const R_file /**< Pre-computed (upper triangular) Cholesky factor of Hessian matrix. + The Cholesky factor must be stored in a real_t array of size nV*nV + in row-major format. Note: Only used if xOpt/yOpt and gB are NULL! \n + (If a null pointer is passed, Cholesky decomposition is computed internally!) */ + ); + +/** Solves an initialised QP sequence using the online active set strategy. + * QP solution is started from previous solution. + * + * Note: This function internally calls solveQP/solveRegularisedQP + * for solving an initialised QP! + * + * \return SUCCESSFUL_RETURN \n + RET_MAX_NWSR_REACHED \n + RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n + RET_HOTSTART_FAILED \n + RET_SHIFT_DETERMINATION_FAILED \n + RET_STEPDIRECTION_DETERMINATION_FAILED \n + RET_STEPLENGTH_DETERMINATION_FAILED \n + RET_HOMOTOPY_STEP_FAILED \n + RET_HOTSTART_STOPPED_INFEASIBILITY \n + RET_HOTSTART_STOPPED_UNBOUNDEDNESS */ +returnValue QProblem_hotstart( QProblem* _THIS, + const real_t* const g_new, /**< Gradient of neighbouring QP to be solved. */ + const real_t* const lb_new, /**< Lower bounds of neighbouring QP to be solved. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const ub_new, /**< Upper bounds of neighbouring QP to be solved. \n + If no upper bounds exist, a NULL pointer can be passed. */ + const real_t* const lbA_new, /**< Lower constraints' bounds of neighbouring QP to be solved. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const real_t* const ubA_new, /**< Upper constraints' bounds of neighbouring QP to be solved. \n + If no upper constraints' bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations; \n + Output: Number of performed working set recalculations. */ + real_t* const cputime /**< Input: Maximum CPU time allowed for QP solution. \n + Output: CPU time spent for QP solution (or to perform nWSR iterations). */ + ); + +/** Solves an initialised QP sequence using the online active set strategy, + * where QP data is read from files. QP solution is started from previous solution. + * + * Note: This function internally calls solveQP/solveRegularisedQP + * for solving an initialised QP! + * + * \return SUCCESSFUL_RETURN \n + RET_MAX_NWSR_REACHED \n + RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n + RET_HOTSTART_FAILED \n + RET_SHIFT_DETERMINATION_FAILED \n + RET_STEPDIRECTION_DETERMINATION_FAILED \n + RET_STEPLENGTH_DETERMINATION_FAILED \n + RET_HOMOTOPY_STEP_FAILED \n + RET_HOTSTART_STOPPED_INFEASIBILITY \n + RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n + RET_UNABLE_TO_READ_FILE \n + RET_INVALID_ARGUMENTS */ +returnValue QProblem_hotstartF( QProblem* _THIS, + const char* const g_file, /**< Name of file where gradient, of neighbouring QP to be solved, is stored. */ + const char* const lb_file, /**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const char* const ub_file, /**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n + If no upper bounds exist, a NULL pointer can be passed. */ + const char* const lbA_file, /**< Name of file where lower constraints' bounds, of neighbouring QP to be solved, is stored. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const char* const ubA_file, /**< Name of file where upper constraints' bounds, of neighbouring QP to be solved, is stored. \n + If no upper constraints' bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations; \n + Output: Number of performed working set recalculations. */ + real_t* const cputime /**< Input: Maximum CPU time allowed for QP solution. \n + Output: CPU time spent for QP solution (or to perform nWSR iterations). */ + ); + +/** Solves an initialised QP sequence using the online active set strategy. + * By default, QP solution is started from previous solution. If a guess + * for the working set is provided, an initialised homotopy is performed. + * + * Note: This function internally calls solveQP/solveRegularisedQP + * for solving an initialised QP! + * + * \return SUCCESSFUL_RETURN \n + RET_MAX_NWSR_REACHED \n + RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n + RET_HOTSTART_FAILED \n + RET_SHIFT_DETERMINATION_FAILED \n + RET_STEPDIRECTION_DETERMINATION_FAILED \n + RET_STEPLENGTH_DETERMINATION_FAILED \n + RET_HOMOTOPY_STEP_FAILED \n + RET_HOTSTART_STOPPED_INFEASIBILITY \n + RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n + RET_SETUP_AUXILIARYQP_FAILED */ +returnValue QProblem_hotstartW( QProblem* _THIS, + const real_t* const g_new, /**< Gradient of neighbouring QP to be solved. */ + const real_t* const lb_new, /**< Lower bounds of neighbouring QP to be solved. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const ub_new, /**< Upper bounds of neighbouring QP to be solved. \n + If no upper bounds exist, a NULL pointer can be passed. */ + const real_t* const lbA_new, /**< Lower constraints' bounds of neighbouring QP to be solved. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const real_t* const ubA_new, /**< Upper constraints' bounds of neighbouring QP to be solved. \n + If no upper constraints' bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations; \n + Output: Number of performed working set recalculations. */ + real_t* const cputime, /**< Input: Maximum CPU time allowed for QP solution. \n + Output: CPU time spent for QP solution (or to perform nWSR iterations). */ + Bounds* const guessedBounds, /**< Optimal working set of bounds for solution (xOpt,yOpt). \n + (If a null pointer is passed, the previous working set of bounds is kept!) */ + Constraints* const guessedConstraints /**< Optimal working set of constraints for solution (xOpt,yOpt). \n + (If a null pointer is passed, the previous working set of constraints is kept!) */ + ); + +/** Solves an initialised QP sequence using the online active set strategy, + * where QP data is read from files. + * By default, QP solution is started from previous solution. If a guess + * for the working set is provided, an initialised homotopy is performed. + * + * Note: This function internally calls solveQP/solveRegularisedQP + * for solving an initialised QP! + * + * \return SUCCESSFUL_RETURN \n + RET_MAX_NWSR_REACHED \n + RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n + RET_HOTSTART_FAILED \n + RET_SHIFT_DETERMINATION_FAILED \n + RET_STEPDIRECTION_DETERMINATION_FAILED \n + RET_STEPLENGTH_DETERMINATION_FAILED \n + RET_HOMOTOPY_STEP_FAILED \n + RET_HOTSTART_STOPPED_INFEASIBILITY \n + RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n + RET_SETUP_AUXILIARYQP_FAILED \n + RET_UNABLE_TO_READ_FILE \n + RET_INVALID_ARGUMENTS */ +returnValue QProblem_hotstartFW( QProblem* _THIS, + const char* const g_file, /**< Name of file where gradient, of neighbouring QP to be solved, is stored. */ + const char* const lb_file, /**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const char* const ub_file, /**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n + If no upper bounds exist, a NULL pointer can be passed. */ + const char* const lbA_file, /**< Name of file where lower constraints' bounds, of neighbouring QP to be solved, is stored. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const char* const ubA_file, /**< Name of file where upper constraints' bounds, of neighbouring QP to be solved, is stored. \n + If no upper constraints' bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations; \n + Output: Number of performed working set recalculations. */ + real_t* const cputime, /**< Input: Maximum CPU time allowed for QP solution. \n + Output: CPU time spent for QP solution (or to perform nWSR iterations). */ + Bounds* const guessedBounds, /**< Optimal working set of bounds for solution (xOpt,yOpt). \n + (If a null pointer is passed, the previous working set of bounds is kept!) */ + Constraints* const guessedConstraints /**< Optimal working set of constraints for solution (xOpt,yOpt). \n + (If a null pointer is passed, the previous working set of constraints is kept!) */ + ); + + +/** Solves using the current working set + * \return SUCCESSFUL_RETURN \n + * RET_STEPDIRECTION_FAILED_TQ \n + * RET_STEPDIRECTION_FAILED_CHOLESKY \n + * RET_INVALID_ARGUMENTS */ +returnValue QProblem_solveCurrentEQP ( QProblem* _THIS, + const int n_rhs, /**< Number of consecutive right hand sides */ + const real_t* g_in, /**< Gradient of neighbouring QP to be solved. */ + const real_t* lb_in, /**< Lower bounds of neighbouring QP to be solved. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* ub_in, /**< Upper bounds of neighbouring QP to be solved. \n + If no upper bounds exist, a NULL pointer can be passed. */ + const real_t* lbA_in, /**< Lower constraints' bounds of neighbouring QP to be solved. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const real_t* ubA_in, /**< Upper constraints' bounds of neighbouring QP to be solved. \n */ + real_t* x_out, /**< Output: Primal solution */ + real_t* y_out /**< Output: Dual solution */ + ); + + + +/** Returns current constraints object of the QP (deep copy). + * \return SUCCESSFUL_RETURN \n + RET_QPOBJECT_NOT_SETUP */ +static inline returnValue QProblem_getConstraints( QProblem* _THIS, + Constraints* _constraints /** Output: Constraints object. */ + ); + + +/** Returns the number of constraints. + * \return Number of constraints. */ +static inline int QProblem_getNC( QProblem* _THIS ); + +/** Returns the number of (implicitly defined) equality constraints. + * \return Number of (implicitly defined) equality constraints. */ +static inline int QProblem_getNEC( QProblem* _THIS ); + +/** Returns the number of active constraints. + * \return Number of active constraints. */ +static inline int QProblem_getNAC( QProblem* _THIS ); + +/** Returns the number of inactive constraints. + * \return Number of inactive constraints. */ +static inline int QProblem_getNIAC( QProblem* _THIS ); + +/** Returns the dimension of null space. + * \return Dimension of null space. */ +int QProblem_getNZ( QProblem* _THIS ); + + +/** Returns the dual solution vector (deep copy). + * \return SUCCESSFUL_RETURN \n + RET_QP_NOT_SOLVED */ +returnValue QProblem_getDualSolution( QProblem* _THIS, + real_t* const yOpt /**< Output: Dual solution vector (if QP has been solved). */ + ); + + +/** Defines user-defined routine for calculating the constraint product A*x + * \return SUCCESSFUL_RETURN \n */ +returnValue QProblem_setConstraintProduct( QProblem* _THIS, + ConstraintProduct _constraintProduct + ); + + +/** Prints concise list of properties of the current QP. + * \return SUCCESSFUL_RETURN \n */ +returnValue QProblem_printProperties( QProblem* _THIS ); + + + +/** Writes a vector with the state of the working set +* \return SUCCESSFUL_RETURN */ +returnValue QProblem_getWorkingSet( QProblem* _THIS, + real_t* workingSet /** Output: array containing state of the working set. */ + ); + +/** Writes a vector with the state of the working set of bounds + * \return SUCCESSFUL_RETURN \n + * RET_INVALID_ARGUMENTS */ +returnValue QProblem_getWorkingSetBounds( QProblem* _THIS, + real_t* workingSetB /** Output: array containing state of the working set of bounds. */ + ); + +/** Writes a vector with the state of the working set of constraints + * \return SUCCESSFUL_RETURN \n + * RET_INVALID_ARGUMENTS */ +returnValue QProblem_getWorkingSetConstraints( QProblem* _THIS, + real_t* workingSetC /** Output: array containing state of the working set of constraints. */ + ); + + +/** Returns current bounds object of the QP (deep copy). + * \return SUCCESSFUL_RETURN \n + RET_QPOBJECT_NOT_SETUP */ +static inline returnValue QProblem_getBounds( QProblem* _THIS, + Bounds* _bounds /** Output: Bounds object. */ + ); + + +/** Returns the number of variables. + * \return Number of variables. */ +static inline int QProblem_getNV( QProblem* _THIS ); + +/** Returns the number of free variables. + * \return Number of free variables. */ +static inline int QProblem_getNFR( QProblem* _THIS ); + +/** Returns the number of fixed variables. + * \return Number of fixed variables. */ +static inline int QProblem_getNFX( QProblem* _THIS ); + +/** Returns the number of implicitly fixed variables. + * \return Number of implicitly fixed variables. */ +static inline int QProblem_getNFV( QProblem* _THIS ); + + +/** Returns the optimal objective function value. + * \return finite value: Optimal objective function value (QP was solved) \n + +infinity: QP was not yet solved */ +real_t QProblem_getObjVal( QProblem* _THIS ); + +/** Returns the objective function value at an arbitrary point x. + * \return Objective function value at point x */ +real_t QProblem_getObjValX( QProblem* _THIS, + const real_t* const _x /**< Point at which the objective function shall be evaluated. */ + ); + +/** Returns the primal solution vector. + * \return SUCCESSFUL_RETURN \n + RET_QP_NOT_SOLVED */ +returnValue QProblem_getPrimalSolution( QProblem* _THIS, + real_t* const xOpt /**< Output: Primal solution vector (if QP has been solved). */ + ); + + +/** Returns status of the solution process. + * \return Status of solution process. */ +static inline QProblemStatus QProblem_getStatus( QProblem* _THIS ); + + +/** Returns if the QProblem object is initialised. + * \return BT_TRUE: QProblem initialised \n + BT_FALSE: QProblem not initialised */ +static inline BooleanType QProblem_isInitialised( QProblem* _THIS ); + +/** Returns if the QP has been solved. + * \return BT_TRUE: QProblem solved \n + BT_FALSE: QProblem not solved */ +static inline BooleanType QProblem_isSolved( QProblem* _THIS ); + +/** Returns if the QP is infeasible. + * \return BT_TRUE: QP infeasible \n + BT_FALSE: QP feasible (or not known to be infeasible!) */ +static inline BooleanType QProblem_isInfeasible( QProblem* _THIS ); + +/** Returns if the QP is unbounded. + * \return BT_TRUE: QP unbounded \n + BT_FALSE: QP unbounded (or not known to be unbounded!) */ +static inline BooleanType QProblem_isUnbounded( QProblem* _THIS ); + + +/** Returns Hessian type flag (type is not determined due to _THIS call!). + * \return Hessian type. */ +static inline HessianType QProblem_getHessianType( QProblem* _THIS ); + +/** Changes the print level. + * \return SUCCESSFUL_RETURN */ +static inline returnValue QProblem_setHessianType( QProblem* _THIS, + HessianType _hessianType /**< New Hessian type. */ + ); + +/** Returns if the QP has been internally regularised. + * \return BT_TRUE: Hessian is internally regularised for QP solution \n + BT_FALSE: No internal Hessian regularisation is used for QP solution */ +static inline BooleanType QProblem_usingRegularisation( QProblem* _THIS ); + +/** Returns current options struct. + * \return Current options struct. */ +static inline Options QProblem_getOptions( QProblem* _THIS ); + +/** Overrides current options with given ones. + * \return SUCCESSFUL_RETURN */ +static inline returnValue QProblem_setOptions( QProblem* _THIS, + Options _options /**< New options. */ + ); + +/** Returns the print level. + * \return Print level. */ +static inline PrintLevel QProblem_getPrintLevel( QProblem* _THIS ); + +/** Changes the print level. + * \return SUCCESSFUL_RETURN */ +returnValue QProblem_setPrintLevel( QProblem* _THIS, + PrintLevel _printlevel /**< New print level. */ + ); + + +/** Returns the current number of QP problems solved. + * \return Number of QP problems solved. */ +static inline unsigned int QProblem_getCount( QProblem* _THIS ); + +/** Resets QP problem counter (to zero). + * \return SUCCESSFUL_RETURN. */ +static inline returnValue QProblem_resetCounter( QProblem* _THIS ); + + +/** Prints a list of all options and their current values. + * \return SUCCESSFUL_RETURN \n */ +returnValue QProblem_printOptions( QProblem* _THIS ); + + +/** Solves a QProblem whose QP data is assumed to be stored in the member variables. + * A guess for its primal/dual optimal solution vectors and the corresponding + * working sets of bounds and constraints can be provided. + * Note: This function is internally called by all init functions! + * \return SUCCESSFUL_RETURN \n + RET_INIT_FAILED \n + RET_INIT_FAILED_CHOLESKY \n + RET_INIT_FAILED_TQ \n + RET_INIT_FAILED_HOTSTART \n + RET_INIT_FAILED_INFEASIBILITY \n + RET_INIT_FAILED_UNBOUNDEDNESS \n + RET_MAX_NWSR_REACHED */ +returnValue QProblem_solveInitialQP( QProblem* _THIS, + const real_t* const xOpt, /**< Optimal primal solution vector.*/ + const real_t* const yOpt, /**< Optimal dual solution vector. */ + Bounds* const guessedBounds, /**< Optimal working set of bounds for solution (xOpt,yOpt). */ + Constraints* const guessedConstraints, /**< Optimal working set of constraints for solution (xOpt,yOpt). */ + const real_t* const _R, /**< Pre-computed (upper triangular) Cholesky factor of Hessian matrix. */ + int* nWSR, /**< Input: Maximum number of working set recalculations; \n + * Output: Number of performed working set recalculations. */ + real_t* const cputime /**< Input: Maximum CPU time allowed for QP solution. \n + * Output: CPU time spent for QP solution (or to perform nWSR iterations). */ + ); + +/** Solves QProblem using online active set strategy. + * Note: This function is internally called by all hotstart functions! + * \return SUCCESSFUL_RETURN \n + RET_MAX_NWSR_REACHED \n + RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n + RET_HOTSTART_FAILED \n + RET_SHIFT_DETERMINATION_FAILED \n + RET_STEPDIRECTION_DETERMINATION_FAILED \n + RET_STEPLENGTH_DETERMINATION_FAILED \n + RET_HOMOTOPY_STEP_FAILED \n + RET_HOTSTART_STOPPED_INFEASIBILITY \n + RET_HOTSTART_STOPPED_UNBOUNDEDNESS */ +returnValue QProblem_solveQP( QProblem* _THIS, + const real_t* const g_new, /**< Gradient of neighbouring QP to be solved. */ + const real_t* const lb_new, /**< Lower bounds of neighbouring QP to be solved. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const ub_new, /**< Upper bounds of neighbouring QP to be solved. \n + If no upper bounds exist, a NULL pointer can be passed. */ + const real_t* const lbA_new, /**< Lower constraints' bounds of neighbouring QP to be solved. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const real_t* const ubA_new, /**< Upper constraints' bounds of neighbouring QP to be solved. \n + If no upper constraints' bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations; \n + Output: Number of performed working set recalculations. */ + real_t* const cputime, /**< Input: Maximum CPU time allowed for QP solution. \n + Output: CPU time spent for QP solution (or to perform nWSR iterations). */ + int nWSRperformed, /**< Number of working set recalculations already performed to solve + this QP within previous solveQP() calls. This number is + always zero, except for successive calls from solveRegularisedQP() + or when using the far bound strategy. */ + BooleanType isFirstCall /**< Indicating whether this is the first call for current QP. */ + ); + + +/** Solves QProblem using online active set strategy. + * Note: This function is internally called by all hotstart functions! + * \return SUCCESSFUL_RETURN \n + RET_MAX_NWSR_REACHED \n + RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n + RET_HOTSTART_FAILED \n + RET_SHIFT_DETERMINATION_FAILED \n + RET_STEPDIRECTION_DETERMINATION_FAILED \n + RET_STEPLENGTH_DETERMINATION_FAILED \n + RET_HOMOTOPY_STEP_FAILED \n + RET_HOTSTART_STOPPED_INFEASIBILITY \n + RET_HOTSTART_STOPPED_UNBOUNDEDNESS */ +returnValue QProblem_solveRegularisedQP( QProblem* _THIS, + const real_t* const g_new, /**< Gradient of neighbouring QP to be solved. */ + const real_t* const lb_new, /**< Lower bounds of neighbouring QP to be solved. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const ub_new, /**< Upper bounds of neighbouring QP to be solved. \n + If no upper bounds exist, a NULL pointer can be passed. */ + const real_t* const lbA_new, /**< Lower constraints' bounds of neighbouring QP to be solved. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const real_t* const ubA_new, /**< Upper constraints' bounds of neighbouring QP to be solved. \n + If no upper constraints' bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations; \n + Output: Number of performed working set recalculations. */ + real_t* const cputime, /**< Input: Maximum CPU time allowed for QP solution. \n + Output: CPU time spent for QP solution (or to perform nWSR iterations). */ + int nWSRperformed, /**< Number of working set recalculations already performed to solve + this QP within previous solveRegularisedQP() calls. This number is + always zero, except for successive calls when using the far bound strategy. */ + BooleanType isFirstCall /**< Indicating whether this is the first call for current QP. */ + ); + + +/** Determines type of existing constraints and bounds (i.e. implicitly fixed, unbounded etc.). + * \return SUCCESSFUL_RETURN \n + RET_SETUPSUBJECTTOTYPE_FAILED */ +returnValue QProblem_setupSubjectToType( QProblem* _THIS ); + +/** Determines type of new constraints and bounds (i.e. implicitly fixed, unbounded etc.). + * \return SUCCESSFUL_RETURN \n + RET_SETUPSUBJECTTOTYPE_FAILED */ +returnValue QProblem_setupSubjectToTypeNew( QProblem* _THIS, + const real_t* const lb_new, /**< New lower bounds. */ + const real_t* const ub_new, /**< New upper bounds. */ + const real_t* const lbA_new, /**< New lower constraints' bounds. */ + const real_t* const ubA_new /**< New upper constraints' bounds. */ + ); + +/** Computes the Cholesky decomposition of the projected Hessian (i.e. R^T*R = Z^T*H*Z). + * Note: If Hessian turns out not to be positive definite, the Hessian type + * is set to HST_SEMIDEF accordingly. + * \return SUCCESSFUL_RETURN \n + * RET_HESSIAN_NOT_SPD \n + * RET_INDEXLIST_CORRUPTED */ +returnValue QProblem_computeProjectedCholesky( QProblem* _THIS ); + +/** Computes initial Cholesky decomposition of the projected Hessian making + * use of the function setupCholeskyDecomposition() or setupCholeskyDecompositionProjected(). + * \return SUCCESSFUL_RETURN \n + * RET_HESSIAN_NOT_SPD \n + * RET_INDEXLIST_CORRUPTED */ +returnValue QProblem_setupInitialCholesky( QProblem* _THIS ); + +/** Initialises TQ factorisation of A (i.e. A*Q = [0 T]) if NO constraint is active. + * \return SUCCESSFUL_RETURN \n + RET_INDEXLIST_CORRUPTED */ +returnValue QProblem_setupTQfactorisation( QProblem* _THIS ); + + +/** Obtains the desired working set for the auxiliary initial QP in + * accordance with the user specifications + * (assumes that member AX has already been initialised!) + * \return SUCCESSFUL_RETURN \n + RET_OBTAINING_WORKINGSET_FAILED \n + RET_INVALID_ARGUMENTS */ +returnValue QProblem_obtainAuxiliaryWorkingSet( QProblem* _THIS, + const real_t* const xOpt, /**< Optimal primal solution vector. + * If a NULL pointer is passed, all entries are assumed to be zero. */ + const real_t* const yOpt, /**< Optimal dual solution vector. + * If a NULL pointer is passed, all entries are assumed to be zero. */ + Bounds* const guessedBounds, /**< Guessed working set of bounds for solution (xOpt,yOpt). */ + Constraints* const guessedConstraints, /**< Guessed working set for solution (xOpt,yOpt). */ + Bounds* auxiliaryBounds, /**< Input: Allocated bound object. \n + * Ouput: Working set of constraints for auxiliary QP. */ + Constraints* auxiliaryConstraints /**< Input: Allocated bound object. \n + * Ouput: Working set for auxiliary QP. */ + ); + +/** Sets up bound and constraints data structures according to auxiliaryBounds/Constraints. + * (If the working set shall be setup afresh, make sure that + * bounds and constraints data structure have been resetted + * and the TQ factorisation has been initialised!) + * \return SUCCESSFUL_RETURN \n + RET_SETUP_WORKINGSET_FAILED \n + RET_INVALID_ARGUMENTS \n + RET_UNKNOWN_BUG */ +returnValue QProblem_setupAuxiliaryWorkingSet( QProblem* _THIS, + Bounds* const auxiliaryBounds, /**< Working set of bounds for auxiliary QP. */ + Constraints* const auxiliaryConstraints, /**< Working set of constraints for auxiliary QP. */ + BooleanType setupAfresh /**< Flag indicating if given working set shall be + * setup afresh or by updating the current one. */ + ); + +/** Sets up the optimal primal/dual solution of the auxiliary initial QP. + * \return SUCCESSFUL_RETURN */ +returnValue QProblem_setupAuxiliaryQPsolution( QProblem* _THIS, + const real_t* const xOpt, /**< Optimal primal solution vector. + * If a NULL pointer is passed, all entries are set to zero. */ + const real_t* const yOpt /**< Optimal dual solution vector. + * If a NULL pointer is passed, all entries are set to zero. */ + ); + +/** Sets up gradient of the auxiliary initial QP for given + * optimal primal/dual solution and given initial working set + * (assumes that members X, Y and BOUNDS, CONSTRAINTS have already been initialised!). + * \return SUCCESSFUL_RETURN */ +returnValue QProblem_setupAuxiliaryQPgradient( QProblem* _THIS ); + +/** Sets up (constraints') bounds of the auxiliary initial QP for given + * optimal primal/dual solution and given initial working set + * (assumes that members X, Y and BOUNDS, CONSTRAINTS have already been initialised!). + * \return SUCCESSFUL_RETURN \n + RET_UNKNOWN_BUG */ +returnValue QProblem_setupAuxiliaryQPbounds( QProblem* _THIS, + Bounds* const auxiliaryBounds, /**< Working set of bounds for auxiliary QP. */ + Constraints* const auxiliaryConstraints, /**< Working set of constraints for auxiliary QP. */ + BooleanType useRelaxation /**< Flag indicating if inactive (constraints') bounds shall be relaxed. */ + ); + + +/** Adds a constraint to active set. + * \return SUCCESSFUL_RETURN \n + RET_ADDCONSTRAINT_FAILED \n + RET_ADDCONSTRAINT_FAILED_INFEASIBILITY \n + RET_ENSURELI_FAILED */ +returnValue QProblem_addConstraint( QProblem* _THIS, + int number, /**< Number of constraint to be added to active set. */ + SubjectToStatus C_status, /**< Status of new active constraint. */ + BooleanType updateCholesky, /**< Flag indicating if Cholesky decomposition shall be updated. */ + BooleanType ensureLI /**< Ensure linear independence by exchange rules by default. */ + ); + +/** Checks if new active constraint to be added is linearly dependent from + * from row of the active constraints matrix. + * \return RET_LINEARLY_DEPENDENT \n + RET_LINEARLY_INDEPENDENT \n + RET_INDEXLIST_CORRUPTED */ +returnValue QProblem_addConstraint_checkLI( QProblem* _THIS, + int number /**< Number of constraint to be added to active set. */ + ); + +/** Ensures linear independence of constraint matrix when a new constraint is added. + * To _THIS end a bound or constraint is removed simultaneously if necessary. + * \return SUCCESSFUL_RETURN \n + RET_LI_RESOLVED \n + RET_ENSURELI_FAILED \n + RET_ENSURELI_FAILED_TQ \n + RET_ENSURELI_FAILED_NOINDEX \n + RET_REMOVE_FROM_ACTIVESET */ +returnValue QProblem_addConstraint_ensureLI( QProblem* _THIS, + int number, /**< Number of constraint to be added to active set. */ + SubjectToStatus C_status /**< Status of new active bound. */ + ); + +/** Adds a bound to active set. + * \return SUCCESSFUL_RETURN \n + RET_ADDBOUND_FAILED \n + RET_ADDBOUND_FAILED_INFEASIBILITY \n + RET_ENSURELI_FAILED */ +returnValue QProblem_addBound( QProblem* _THIS, + int number, /**< Number of bound to be added to active set. */ + SubjectToStatus B_status, /**< Status of new active bound. */ + BooleanType updateCholesky, /**< Flag indicating if Cholesky decomposition shall be updated. */ + BooleanType ensureLI /**< Ensure linear independence by exchange rules by default. */ + ); + +/** Checks if new active bound to be added is linearly dependent from + * from row of the active constraints matrix. + * \return RET_LINEARLY_DEPENDENT \n + RET_LINEARLY_INDEPENDENT */ +returnValue QProblem_addBound_checkLI( QProblem* _THIS, + int number /**< Number of bound to be added to active set. */ + ); + +/** Ensures linear independence of constraint matrix when a new bound is added. + * To _THIS end a bound or constraint is removed simultaneously if necessary. + * \return SUCCESSFUL_RETURN \n + RET_LI_RESOLVED \n + RET_ENSURELI_FAILED \n + RET_ENSURELI_FAILED_TQ \n + RET_ENSURELI_FAILED_NOINDEX \n + RET_REMOVE_FROM_ACTIVESET */ +returnValue QProblem_addBound_ensureLI( QProblem* _THIS, + int number, /**< Number of bound to be added to active set. */ + SubjectToStatus B_status /**< Status of new active bound. */ + ); + +/** Removes a constraint from active set. + * \return SUCCESSFUL_RETURN \n + RET_CONSTRAINT_NOT_ACTIVE \n + RET_REMOVECONSTRAINT_FAILED \n + RET_HESSIAN_NOT_SPD */ +returnValue QProblem_removeConstraint( QProblem* _THIS, + int number, /**< Number of constraint to be removed from active set. */ + BooleanType updateCholesky, /**< Flag indicating if Cholesky decomposition shall be updated. */ + BooleanType allowFlipping, /**< Flag indicating if flipping bounds are allowed. */ + BooleanType ensureNZC /**< Flag indicating if non-zero curvature is ensured by exchange rules. */ + ); + +/** Removes a bounds from active set. + * \return SUCCESSFUL_RETURN \n + RET_BOUND_NOT_ACTIVE \n + RET_HESSIAN_NOT_SPD \n + RET_REMOVEBOUND_FAILED */ +returnValue QProblem_removeBound( QProblem* _THIS, + int number, /**< Number of bound to be removed from active set. */ + BooleanType updateCholesky, /**< Flag indicating if Cholesky decomposition shall be updated. */ + BooleanType allowFlipping, /**< Flag indicating if flipping bounds are allowed. */ + BooleanType ensureNZC /**< Flag indicating if non-zero curvature is ensured by exchange rules. */ + ); + + +/** Performs robustified ratio test yield the maximum possible step length + * along the homotopy path. + * \return SUCCESSFUL_RETURN */ +returnValue QProblem_performPlainRatioTest( QProblem* _THIS, + int nIdx, /**< Number of ratios to be checked. */ + const int* const idxList, /**< Array containing the indices of all ratios to be checked. */ + const real_t* const num, /**< Array containing all numerators for performing the ratio test. */ + const real_t* const den, /**< Array containing all denominators for performing the ratio test. */ + real_t epsNum, /**< Numerator tolerance. */ + real_t epsDen, /**< Denominator tolerance. */ + real_t* t, /**< Output: Maximum possible step length along the homotopy path. */ + int* BC_idx /**< Output: Index of blocking constraint. */ + ); + + +/** Ensure non-zero curvature by primal jump. + * \return SUCCESSFUL_RETURN \n + * RET_HOTSTART_STOPPED_UNBOUNDEDNESS */ +returnValue QProblem_ensureNonzeroCurvature( QProblem* _THIS, + BooleanType removeBoundNotConstraint, /**< SubjectTo to be removed is a bound. */ + int remIdx, /**< Index of bound/constraint to be removed. */ + BooleanType* exchangeHappened, /**< Output: Exchange was necessary to ensure. */ + BooleanType* addBoundNotConstraint, /**< SubjectTo to be added is a bound. */ + int* addIdx, /**< Index of bound/constraint to be added. */ + SubjectToStatus* addStatus /**< Status of bound/constraint to be added. */ + ); + + +/** Solves the system Ta = b or T^Ta = b where T is a reverse upper triangular matrix. + * \return SUCCESSFUL_RETURN \n + RET_DIV_BY_ZERO */ +returnValue QProblem_backsolveT( QProblem* _THIS, + const real_t* const b, /**< Right hand side vector. */ + BooleanType transposed, /**< Indicates if the transposed system shall be solved. */ + real_t* const a /**< Output: Solution vector */ + ); + + +/** Determines step direction of the shift of the QP data. + * \return SUCCESSFUL_RETURN */ +returnValue QProblem_determineDataShift( QProblem* _THIS, + const real_t* const g_new, /**< New gradient vector. */ + const real_t* const lbA_new, /**< New lower constraints' bounds. */ + const real_t* const ubA_new, /**< New upper constraints' bounds. */ + const real_t* const lb_new, /**< New lower bounds. */ + const real_t* const ub_new, /**< New upper bounds. */ + real_t* const delta_g, /**< Output: Step direction of gradient vector. */ + real_t* const delta_lbA, /**< Output: Step direction of lower constraints' bounds. */ + real_t* const delta_ubA, /**< Output: Step direction of upper constraints' bounds. */ + real_t* const delta_lb, /**< Output: Step direction of lower bounds. */ + real_t* const delta_ub, /**< Output: Step direction of upper bounds. */ + BooleanType* Delta_bC_isZero, /**< Output: Indicates if active constraints' bounds are to be shifted. */ + BooleanType* Delta_bB_isZero /**< Output: Indicates if active bounds are to be shifted. */ + ); + +/** Determines step direction of the homotopy path. + * \return SUCCESSFUL_RETURN \n + RET_STEPDIRECTION_FAILED_TQ \n + RET_STEPDIRECTION_FAILED_CHOLESKY */ +returnValue QProblem_determineStepDirection( QProblem* _THIS, + const real_t* const delta_g, /**< Step direction of gradient vector. */ + const real_t* const delta_lbA, /**< Step direction of lower constraints' bounds. */ + const real_t* const delta_ubA, /**< Step direction of upper constraints' bounds. */ + const real_t* const delta_lb, /**< Step direction of lower bounds. */ + const real_t* const delta_ub, /**< Step direction of upper bounds. */ + BooleanType Delta_bC_isZero, /**< Indicates if active constraints' bounds are to be shifted. */ + BooleanType Delta_bB_isZero, /**< Indicates if active bounds are to be shifted. */ + real_t* const delta_xFX, /**< Output: Primal homotopy step direction of fixed variables. */ + real_t* const delta_xFR, /**< Output: Primal homotopy step direction of free variables. */ + real_t* const delta_yAC, /**< Output: Dual homotopy step direction of active constraints' multiplier. */ + real_t* const delta_yFX /**< Output: Dual homotopy step direction of fixed variables' multiplier. */ + ); + +/** Determines the maximum possible step length along the homotopy path + * and performs _THIS step (without changing working set). + * \return SUCCESSFUL_RETURN \n + * RET_ERROR_IN_CONSTRAINTPRODUCT \n + * RET_QP_INFEASIBLE */ +returnValue QProblem_performStep( QProblem* _THIS, + const real_t* const delta_g, /**< Step direction of gradient. */ + const real_t* const delta_lbA, /**< Step direction of lower constraints' bounds. */ + const real_t* const delta_ubA, /**< Step direction of upper constraints' bounds. */ + const real_t* const delta_lb, /**< Step direction of lower bounds. */ + const real_t* const delta_ub, /**< Step direction of upper bounds. */ + const real_t* const delta_xFX, /**< Primal homotopy step direction of fixed variables. */ + const real_t* const delta_xFR, /**< Primal homotopy step direction of free variables. */ + const real_t* const delta_yAC, /**< Dual homotopy step direction of active constraints' multiplier. */ + const real_t* const delta_yFX, /**< Dual homotopy step direction of fixed variables' multiplier. */ + int* BC_idx, /**< Output: Index of blocking constraint. */ + SubjectToStatus* BC_status, /**< Output: Status of blocking constraint. */ + BooleanType* BC_isBound /**< Output: Indicates if blocking constraint is a bound. */ + ); + +/** Updates the active set. + * \return SUCCESSFUL_RETURN \n + RET_REMOVE_FROM_ACTIVESET_FAILED \n + RET_ADD_TO_ACTIVESET_FAILED */ +returnValue QProblem_changeActiveSet( QProblem* _THIS, + int BC_idx, /**< Index of blocking constraint. */ + SubjectToStatus BC_status, /**< Status of blocking constraint. */ + BooleanType BC_isBound /**< Indicates if blocking constraint is a bound. */ + ); + + +/** Compute relative length of homotopy in data space for termination + * criterion. + * \return Relative length in data space. */ +real_t QProblem_getRelativeHomotopyLength( QProblem* _THIS, + const real_t* const g_new, /**< Final gradient. */ + const real_t* const lb_new, /**< Final lower variable bounds. */ + const real_t* const ub_new, /**< Final upper variable bounds. */ + const real_t* const lbA_new, /**< Final lower constraint bounds. */ + const real_t* const ubA_new /**< Final upper constraint bounds. */ + ); + + +/** Ramping Strategy to avoid ties. Modifies homotopy start without + * changing current active set. + * \return SUCCESSFUL_RETURN */ +returnValue QProblem_performRamping( QProblem* _THIS ); + + +/** ... */ +returnValue QProblem_updateFarBounds( QProblem* _THIS, + real_t curFarBound, /**< ... */ + int nRamp, /**< ... */ + const real_t* const lb_new, /**< ... */ + real_t* const lb_new_far, /**< ... */ + const real_t* const ub_new, /**< ... */ + real_t* const ub_new_far, /**< ... */ + const real_t* const lbA_new, /**< ... */ + real_t* const lbA_new_far, /**< ... */ + const real_t* const ubA_new, /**< ... */ + real_t* const ubA_new_far /**< ... */ + ); + +/** ... */ +returnValue QProblemBCPY_updateFarBounds( QProblem* _THIS, + real_t curFarBound, /**< ... */ + int nRamp, /**< ... */ + const real_t* const lb_new, /**< ... */ + real_t* const lb_new_far, /**< ... */ + const real_t* const ub_new, /**< ... */ + real_t* const ub_new_far /**< ... */ + ); + + + +/** Performs robustified ratio test yield the maximum possible step length + * along the homotopy path. + * \return SUCCESSFUL_RETURN */ +returnValue QProblem_performRatioTestC( QProblem* _THIS, + int nIdx, /**< Number of ratios to be checked. */ + const int* const idxList, /**< Array containing the indices of all ratios to be checked. */ + Constraints* const subjectTo, /**< Constraint object corresponding to ratios to be checked. */ + const real_t* const num, /**< Array containing all numerators for performing the ratio test. */ + const real_t* const den, /**< Array containing all denominators for performing the ratio test. */ + real_t epsNum, /**< Numerator tolerance. */ + real_t epsDen, /**< Denominator tolerance. */ + real_t* t, /**< Output: Maximum possible step length along the homotopy path. */ + int* BC_idx /**< Output: Index of blocking constraint. */ + ); + + +/** Drift correction at end of each active set iteration + * \return SUCCESSFUL_RETURN */ +returnValue QProblem_performDriftCorrection( QProblem* _THIS ); + + +/** Updates QP vectors, working sets and internal data structures in order to + start from an optimal solution corresponding to initial guesses of the working + set for bounds and constraints. + * \return SUCCESSFUL_RETURN \n + * RET_SETUP_AUXILIARYQP_FAILED \n + RET_INVALID_ARGUMENTS */ +returnValue QProblem_setupAuxiliaryQP( QProblem* _THIS, + Bounds* const guessedBounds, /**< Initial guess for working set of bounds. */ + Constraints* const guessedConstraints /**< Initial guess for working set of constraints. */ + ); + +/** Determines if it is more efficient to refactorise the matrices when + * hotstarting or not (i.e. better to update the existing factorisations). + * \return BT_TRUE iff matrices shall be refactorised afresh + */ +BooleanType QProblem_shallRefactorise( QProblem* _THIS, + Bounds* const guessedBounds, /**< Guessed new working set of bounds. */ + Constraints* const guessedConstraints /**< Guessed new working set of constraints. */ + ); + +/** Setups internal QP data. + * \return SUCCESSFUL_RETURN \n + RET_INVALID_ARGUMENTS \n + RET_UNKNONW_BUG */ +returnValue QProblem_setupQPdataM( QProblem* _THIS, + DenseMatrix *_H, /**< Hessian matrix. \n + If Hessian matrix is trivial,a NULL pointer can be passed. */ + const real_t* const _g, /**< Gradient vector. */ + DenseMatrix *_A, /**< Constraint matrix. */ + const real_t* const _lb, /**< Lower bound vector (on variables). \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const _ub, /**< Upper bound vector (on variables). \n + If no upper bounds exist, a NULL pointer can be passed. */ + const real_t* const _lbA, /**< Lower constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const real_t* const _ubA /**< Upper constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + ); + + +/** Sets up dense internal QP data. If the current Hessian is trivial + * (i.e. HST_ZERO or HST_IDENTITY) but a non-trivial one is given, + * memory for Hessian is allocated and it is set to the given one. + * \return SUCCESSFUL_RETURN \n + RET_INVALID_ARGUMENTS \n + RET_UNKNONW_BUG */ +returnValue QProblem_setupQPdata( QProblem* _THIS, + real_t* const _H, /**< Hessian matrix. \n + If Hessian matrix is trivial,a NULL pointer can be passed. */ + const real_t* const _g, /**< Gradient vector. */ + real_t* const _A, /**< Constraint matrix. */ + const real_t* const _lb, /**< Lower bound vector (on variables). \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const _ub, /**< Upper bound vector (on variables). \n + If no upper bounds exist, a NULL pointer can be passed. */ + const real_t* const _lbA, /**< Lower constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const real_t* const _ubA /**< Upper constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + ); + +/** Sets up internal QP data by loading it from files. If the current Hessian + * is trivial (i.e. HST_ZERO or HST_IDENTITY) but a non-trivial one is given, + * memory for Hessian is allocated and it is set to the given one. + * \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_OPEN_FILE \n + RET_UNABLE_TO_READ_FILE \n + RET_INVALID_ARGUMENTS \n + RET_UNKNONW_BUG */ +returnValue QProblem_setupQPdataFromFile( QProblem* _THIS, + const char* const H_file, /**< Name of file where Hessian matrix, of neighbouring QP to be solved, is stored. \n + If Hessian matrix is trivial,a NULL pointer can be passed. */ + const char* const g_file, /**< Name of file where gradient, of neighbouring QP to be solved, is stored. */ + const char* const A_file, /**< Name of file where constraint matrix, of neighbouring QP to be solved, is stored. */ + const char* const lb_file, /**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const char* const ub_file, /**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n + If no upper bounds exist, a NULL pointer can be passed. */ + const char* const lbA_file, /**< Name of file where lower constraints' bounds, of neighbouring QP to be solved, is stored. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const char* const ubA_file /**< Name of file where upper constraints' bounds, of neighbouring QP to be solved, is stored. \n + If no upper constraints' bounds exist, a NULL pointer can be passed. */ + ); + +/** Loads new QP vectors from files (internal members are not affected!). + * \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_OPEN_FILE \n + RET_UNABLE_TO_READ_FILE \n + RET_INVALID_ARGUMENTS */ +returnValue QProblem_loadQPvectorsFromFile( QProblem* _THIS, + const char* const g_file, /**< Name of file where gradient, of neighbouring QP to be solved, is stored. */ + const char* const lb_file, /**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const char* const ub_file, /**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n + If no upper bounds exist, a NULL pointer can be passed. */ + const char* const lbA_file, /**< Name of file where lower constraints' bounds, of neighbouring QP to be solved, is stored. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const char* const ubA_file, /**< Name of file where upper constraints' bounds, of neighbouring QP to be solved, is stored. \n + If no upper constraints' bounds exist, a NULL pointer can be passed. */ + real_t* const g_new, /**< Output: Gradient of neighbouring QP to be solved. */ + real_t* const lb_new, /**< Output: Lower bounds of neighbouring QP to be solved */ + real_t* const ub_new, /**< Output: Upper bounds of neighbouring QP to be solved */ + real_t* const lbA_new, /**< Output: Lower constraints' bounds of neighbouring QP to be solved */ + real_t* const ubA_new /**< Output: Upper constraints' bounds of neighbouring QP to be solved */ + ); + + +/** Prints concise information on the current iteration. + * \return SUCCESSFUL_RETURN \n */ +returnValue QProblem_printIteration( QProblem* _THIS, + int iter, /**< Number of current iteration. */ + int BC_idx, /**< Index of blocking constraint. */ + SubjectToStatus BC_status, /**< Status of blocking constraint. */ + BooleanType BC_isBound, /**< Indicates if blocking constraint is a bound. */ + real_t homotopyLength, /**< Current homotopy distance. */ + BooleanType isFirstCall /**< Indicating whether this is the first call for current QP. */ + ); + + +/** Sets constraint matrix of the QP. \n + Note: Also internal vector Ax is recomputed! + * \return SUCCESSFUL_RETURN \n + * RET_INVALID_ARGUMENTS */ +static inline returnValue QProblem_setAM( QProblem* _THIS, + DenseMatrix *A_new /**< New constraint matrix. */ + ); + +/** Sets dense constraint matrix of the QP. \n + Note: Also internal vector Ax is recomputed! + * \return SUCCESSFUL_RETURN \n + * RET_INVALID_ARGUMENTS */ +static inline returnValue QProblem_setA( QProblem* _THIS, + real_t* const A_new /**< New dense constraint matrix (with correct dimension!). */ + ); + + +/** Sets constraints' lower bound vector of the QP. + * \return SUCCESSFUL_RETURN \n + * RET_QPOBJECT_NOT_SETUP */ +static inline returnValue QProblem_setLBA( QProblem* _THIS, + const real_t* const lbA_new /**< New constraints' lower bound vector (with correct dimension!). */ + ); + +/** Changes single entry of lower constraints' bound vector of the QP. + * \return SUCCESSFUL_RETURN \n + * RET_QPOBJECT_NOT_SETUP \n + * RET_INDEX_OUT_OF_BOUNDS */ +static inline returnValue QProblem_setLBAn( QProblem* _THIS, + int number, /**< Number of entry to be changed. */ + real_t value /**< New value for entry of lower constraints' bound vector (with correct dimension!). */ + ); + +/** Sets constraints' upper bound vector of the QP. + * \return SUCCESSFUL_RETURN \n + * RET_QPOBJECT_NOT_SETUP */ +static inline returnValue QProblem_setUBA( QProblem* _THIS, + const real_t* const ubA_new /**< New constraints' upper bound vector (with correct dimension!). */ + ); + +/** Changes single entry of upper constraints' bound vector of the QP. + * \return SUCCESSFUL_RETURN \n + * RET_QPOBJECT_NOT_SETUP \n + * RET_INDEX_OUT_OF_BOUNDS */ +static inline returnValue QProblem_setUBAn( QProblem* _THIS, + int number, /**< Number of entry to be changed. */ + real_t value /**< New value for entry of upper constraints' bound vector (with correct dimension!). */ + ); + + +/** Decides if lower bounds are smaller than upper bounds + * + * \return SUCCESSFUL_RETURN \n + * RET_QP_INFEASIBLE */ +returnValue QProblem_areBoundsConsistent( QProblem* _THIS, + const real_t* const lb, /**< Vector of lower bounds*/ + const real_t* const ub, /**< Vector of upper bounds*/ + const real_t* const lbA, /**< Vector of lower constraints*/ + const real_t* const ubA /**< Vector of upper constraints*/ + ); + + +/** Drops the blocking bound/constraint that led to infeasibility, or finds another + * bound/constraint to drop according to drop priorities. + * \return SUCCESSFUL_RETURN \n + */ +returnValue QProblem_dropInfeasibles ( QProblem* _THIS, + int BC_number, /**< Number of the bound or constraint to be added */ + SubjectToStatus BC_status, /**< New status of the bound or constraint to be added */ + BooleanType BC_isBound, /**< Whether a bound or a constraint is to be added */ + real_t *xiB, + real_t *xiC + ); + + +/** If Hessian type has been set by the user, nothing is done. + * Otherwise the Hessian type is set to HST_IDENTITY, HST_ZERO, or + * HST_POSDEF (default), respectively. + * \return SUCCESSFUL_RETURN \n + RET_HESSIAN_INDEFINITE */ +returnValue QProblem_determineHessianType( QProblem* _THIS ); + +/** Computes the Cholesky decomposition of the (simply projected) Hessian + * (i.e. R^T*R = Z^T*H*Z). It only works in the case where Z is a simple + * projection matrix! + * Note: If Hessian turns out not to be positive definite, the Hessian type + * is set to HST_SEMIDEF accordingly. + * \return SUCCESSFUL_RETURN \n + * RET_HESSIAN_NOT_SPD \n + * RET_INDEXLIST_CORRUPTED */ +returnValue QProblemBCPY_computeCholesky( QProblem* _THIS ); + +/** Obtains the desired working set for the auxiliary initial QP in + * accordance with the user specifications + * \return SUCCESSFUL_RETURN \n + RET_OBTAINING_WORKINGSET_FAILED \n + RET_INVALID_ARGUMENTS */ +returnValue QProblemBCPY_obtainAuxiliaryWorkingSet( QProblem* _THIS, + const real_t* const xOpt, /**< Optimal primal solution vector. + * If a NULL pointer is passed, all entries are assumed to be zero. */ + const real_t* const yOpt, /**< Optimal dual solution vector. + * If a NULL pointer is passed, all entries are assumed to be zero. */ + Bounds* const guessedBounds, /**< Guessed working set for solution (xOpt,yOpt). */ + Bounds* auxiliaryBounds /**< Input: Allocated bound object. \n + * Output: Working set for auxiliary QP. */ + ); + + +/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix. + * \return SUCCESSFUL_RETURN \n + RET_DIV_BY_ZERO */ +returnValue QProblem_backsolveR( QProblem* _THIS, + const real_t* const b, /**< Right hand side vector. */ + BooleanType transposed, /**< Indicates if the transposed system shall be solved. */ + real_t* const a /**< Output: Solution vector */ + ); + +/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix. \n + * Special variant for the case that _THIS function is called from within "removeBound()". + * \return SUCCESSFUL_RETURN \n + RET_DIV_BY_ZERO */ +returnValue QProblem_backsolveRrem( QProblem* _THIS, + const real_t* const b, /**< Right hand side vector. */ + BooleanType transposed, /**< Indicates if the transposed system shall be solved. */ + BooleanType removingBound, /**< Indicates if function is called from "removeBound()". */ + real_t* const a /**< Output: Solution vector */ + ); + + +/** Determines step direction of the shift of the QP data. + * \return SUCCESSFUL_RETURN */ +returnValue QProblemBCPY_determineDataShift( QProblem* _THIS, + const real_t* const g_new, /**< New gradient vector. */ + const real_t* const lb_new, /**< New lower bounds. */ + const real_t* const ub_new, /**< New upper bounds. */ + real_t* const delta_g, /**< Output: Step direction of gradient vector. */ + real_t* const delta_lb, /**< Output: Step direction of lower bounds. */ + real_t* const delta_ub, /**< Output: Step direction of upper bounds. */ + BooleanType* Delta_bB_isZero /**< Output: Indicates if active bounds are to be shifted. */ + ); + + +/** Sets up internal QP data. + * \return SUCCESSFUL_RETURN \n + RET_INVALID_ARGUMENTS */ +returnValue QProblemBCPY_setupQPdataM( QProblem* _THIS, + DenseMatrix *_H, /**< Hessian matrix.*/ + const real_t* const _g, /**< Gradient vector. */ + const real_t* const _lb, /**< Lower bounds (on variables). \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const _ub /**< Upper bounds (on variables). \n + If no upper bounds exist, a NULL pointer can be passed. */ + ); + +/** Sets up internal QP data. If the current Hessian is trivial + * (i.e. HST_ZERO or HST_IDENTITY) but a non-trivial one is given, + * memory for Hessian is allocated and it is set to the given one. + * \return SUCCESSFUL_RETURN \n + RET_INVALID_ARGUMENTS \n + RET_NO_HESSIAN_SPECIFIED */ +returnValue QProblemBCPY_setupQPdata( QProblem* _THIS, + real_t* const _H, /**< Hessian matrix. \n + If Hessian matrix is trivial,a NULL pointer can be passed. */ + const real_t* const _g, /**< Gradient vector. */ + const real_t* const _lb, /**< Lower bounds (on variables). \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const _ub /**< Upper bounds (on variables). \n + If no upper bounds exist, a NULL pointer can be passed. */ + ); + +/** Sets up internal QP data by loading it from files. If the current Hessian + * is trivial (i.e. HST_ZERO or HST_IDENTITY) but a non-trivial one is given, + * memory for Hessian is allocated and it is set to the given one. + * \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_OPEN_FILE \n + RET_UNABLE_TO_READ_FILE \n + RET_INVALID_ARGUMENTS \n + RET_NO_HESSIAN_SPECIFIED */ +returnValue QProblemBCPY_setupQPdataFromFile( QProblem* _THIS, + const char* const H_file, /**< Name of file where Hessian matrix, of neighbouring QP to be solved, is stored. \n + If Hessian matrix is trivial,a NULL pointer can be passed. */ + const char* const g_file, /**< Name of file where gradient, of neighbouring QP to be solved, is stored. */ + const char* const lb_file, /**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const char* const ub_file /**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n + If no upper bounds exist, a NULL pointer can be passed. */ + ); + +/** Loads new QP vectors from files (internal members are not affected!). + * \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_OPEN_FILE \n + RET_UNABLE_TO_READ_FILE \n + RET_INVALID_ARGUMENTS */ +returnValue QProblemBCPY_loadQPvectorsFromFile( QProblem* _THIS, + const char* const g_file, /**< Name of file where gradient, of neighbouring QP to be solved, is stored. */ + const char* const lb_file, /**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const char* const ub_file, /**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n + If no upper bounds exist, a NULL pointer can be passed. */ + real_t* const g_new, /**< Output: Gradient of neighbouring QP to be solved. */ + real_t* const lb_new, /**< Output: Lower bounds of neighbouring QP to be solved */ + real_t* const ub_new /**< Output: Upper bounds of neighbouring QP to be solved */ + ); + + +/** Sets internal infeasibility flag and throws given error in case the far bound + * strategy is not enabled (as QP might actually not be infeasible in _THIS case). + * \return RET_HOTSTART_STOPPED_INFEASIBILITY \n + RET_ENSURELI_FAILED_CYCLING \n + RET_ENSURELI_FAILED_NOINDEX */ +returnValue QProblem_setInfeasibilityFlag( QProblem* _THIS, + returnValue returnvalue, /**< Returnvalue to be tunneled. */ + BooleanType doThrowError /**< Flag forcing to throw an error. */ + ); + + +/** Determines if next QP iteration can be performed within given CPU time limit. + * \return BT_TRUE: CPU time limit is exceeded, stop QP solution. \n + BT_FALSE: Sufficient CPU time for next QP iteration. */ +BooleanType QProblem_isCPUtimeLimitExceeded( QProblem* _THIS, + const real_t* const cputime, /**< Maximum CPU time allowed for QP solution. */ + real_t starttime, /**< Start time of current QP solution. */ + int nWSR /**< Number of working set recalculations performed so far. */ + ); + + +/** Regularise Hessian matrix by adding a scaled identity matrix to it. + * \return SUCCESSFUL_RETURN \n + RET_HESSIAN_ALREADY_REGULARISED */ +returnValue QProblem_regulariseHessian( QProblem* _THIS ); + + +/** Sets Hessian matrix of the QP. + * \return SUCCESSFUL_RETURN */ +static inline returnValue QProblem_setHM( QProblem* _THIS, + DenseMatrix* H_new /**< New Hessian matrix. */ + ); + +/** Sets dense Hessian matrix of the QP. + * If a null pointer is passed and + * a) hessianType is HST_IDENTITY, nothing is done, + * b) hessianType is not HST_IDENTITY, Hessian matrix is set to zero. + * \return SUCCESSFUL_RETURN */ +static inline returnValue QProblem_setH( QProblem* _THIS, + real_t* const H_new /**< New dense Hessian matrix (with correct dimension!). */ + ); + +/** Changes gradient vector of the QP. + * \return SUCCESSFUL_RETURN \n + * RET_INVALID_ARGUMENTS */ +static inline returnValue QProblem_setG( QProblem* _THIS, + const real_t* const g_new /**< New gradient vector (with correct dimension!). */ + ); + +/** Changes lower bound vector of the QP. + * \return SUCCESSFUL_RETURN \n + * RET_INVALID_ARGUMENTS */ +static inline returnValue QProblem_setLB( QProblem* _THIS, + const real_t* const lb_new /**< New lower bound vector (with correct dimension!). */ + ); + +/** Changes single entry of lower bound vector of the QP. + * \return SUCCESSFUL_RETURN \n + RET_INDEX_OUT_OF_BOUNDS */ +static inline returnValue QProblem_setLBn( QProblem* _THIS, + int number, /**< Number of entry to be changed. */ + real_t value /**< New value for entry of lower bound vector. */ + ); + +/** Changes upper bound vector of the QP. + * \return SUCCESSFUL_RETURN \n + * RET_INVALID_ARGUMENTS */ +static inline returnValue QProblem_setUB( QProblem* _THIS, + const real_t* const ub_new /**< New upper bound vector (with correct dimension!). */ + ); + +/** Changes single entry of upper bound vector of the QP. + * \return SUCCESSFUL_RETURN \n + RET_INDEX_OUT_OF_BOUNDS */ +static inline returnValue QProblem_setUBn( QProblem* _THIS, + int number, /**< Number of entry to be changed. */ + real_t value /**< New value for entry of upper bound vector. */ + ); + + + +/** Compute relative length of homotopy in data space for termination + * criterion. + * \return Relative length in data space. */ +real_t QProblemBCPY_getRelativeHomotopyLength( QProblem* _THIS, + const real_t* const g_new, /**< Final gradient. */ + const real_t* const lb_new, /**< Final lower variable bounds. */ + const real_t* const ub_new /**< Final upper variable bounds. */ + ); + + + +/** Performs robustified ratio test yield the maximum possible step length + * along the homotopy path. + * \return SUCCESSFUL_RETURN */ +returnValue QProblem_performRatioTestB( QProblem* _THIS, + int nIdx, /**< Number of ratios to be checked. */ + const int* const idxList, /**< Array containing the indices of all ratios to be checked. */ + Bounds* const subjectTo, /**< Bound object corresponding to ratios to be checked. */ + const real_t* const num, /**< Array containing all numerators for performing the ratio test. */ + const real_t* const den, /**< Array containing all denominators for performing the ratio test. */ + real_t epsNum, /**< Numerator tolerance. */ + real_t epsDen, /**< Denominator tolerance. */ + real_t* t, /**< Output: Maximum possible step length along the homotopy path. */ + int* BC_idx /**< Output: Index of blocking constraint. */ + ); + +/** Checks whether given ratio is blocking, i.e. limits the maximum step length + * along the homotopy path to a value lower than given one. + * \return SUCCESSFUL_RETURN */ +static inline BooleanType QProblem_isBlocking( QProblem* _THIS, + real_t num, /**< Numerator for performing the ratio test. */ + real_t den, /**< Denominator for performing the ratio test. */ + real_t epsNum, /**< Numerator tolerance. */ + real_t epsDen, /**< Denominator tolerance. */ + real_t* t /**< Input: Current maximum step length along the homotopy path, + * Output: Updated maximum possible step length along the homotopy path. */ + ); + + +/** ... + * \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_OPEN_FILE */ +returnValue QProblem_writeQpDataIntoMatFile( QProblem* _THIS, + const char* const filename /**< Mat file name. */ + ); + +/** ... +* \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_OPEN_FILE */ +returnValue QProblem_writeQpWorkspaceIntoMatFile( QProblem* _THIS, + const char* const filename /**< Mat file name. */ + ); + + +/* + * g e t B o u n d s + */ +static inline returnValue QProblem_getBounds( QProblem* _THIS, Bounds* _bounds ) +{ + int nV = QProblem_getNV( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + _bounds = _THIS->bounds; + + return SUCCESSFUL_RETURN; +} + + +/* + * g e t N V + */ +static inline int QProblem_getNV( QProblem* _THIS ) +{ + return Bounds_getNV( _THIS->bounds ); +} + + +/* + * g e t N F R + */ +static inline int QProblem_getNFR( QProblem* _THIS ) +{ + return Bounds_getNFR( _THIS->bounds ); +} + + +/* + * g e t N F X + */ +static inline int QProblem_getNFX( QProblem* _THIS ) +{ + return Bounds_getNFX( _THIS->bounds ); +} + + +/* + * g e t N F V + */ +static inline int QProblem_getNFV( QProblem* _THIS ) +{ + return Bounds_getNFV( _THIS->bounds ); +} + + +/* + * g e t S t a t u s + */ +static inline QProblemStatus QProblem_getStatus( QProblem* _THIS ) +{ + return _THIS->status; +} + + +/* + * i s I n i t i a l i s e d + */ +static inline BooleanType QProblem_isInitialised( QProblem* _THIS ) +{ + if ( _THIS->status == QPS_NOTINITIALISED ) + return BT_FALSE; + else + return BT_TRUE; +} + + +/* + * i s S o l v e d + */ +static inline BooleanType QProblem_isSolved( QProblem* _THIS ) +{ + if ( _THIS->status == QPS_SOLVED ) + return BT_TRUE; + else + return BT_FALSE; +} + + +/* + * i s I n f e a s i b l e + */ +static inline BooleanType QProblem_isInfeasible( QProblem* _THIS ) +{ + return _THIS->infeasible; +} + + +/* + * i s U n b o u n d e d + */ +static inline BooleanType QProblem_isUnbounded( QProblem* _THIS ) +{ + return _THIS->unbounded; +} + + +/* + * g e t H e s s i a n T y p e + */ +static inline HessianType QProblem_getHessianType( QProblem* _THIS ) +{ + return _THIS->hessianType; +} + + +/* + * s e t H e s s i a n T y p e + */ +static inline returnValue QProblem_setHessianType( QProblem* _THIS, HessianType _hessianType ) +{ + _THIS->hessianType = _hessianType; + return SUCCESSFUL_RETURN; +} + + +/* + * u s i n g R e g u l a r i s a t i o n + */ +static inline BooleanType QProblem_usingRegularisation( QProblem* _THIS ) +{ + if ( _THIS->regVal > QPOASES_ZERO ) + return BT_TRUE; + else + return BT_FALSE; +} + + +/* + * g e t O p t i o n s + */ +static inline Options QProblem_getOptions( QProblem* _THIS ) +{ + return _THIS->options; +} + + +/* + * s e t O p t i o n s + */ +static inline returnValue QProblem_setOptions( QProblem* _THIS, + Options _options + ) +{ + OptionsCPY( &_options,&(_THIS->options) ); + Options_ensureConsistency( &(_THIS->options) ); + + QProblem_setPrintLevel( _THIS,_THIS->options.printLevel ); + + return SUCCESSFUL_RETURN; +} + + +/* + * g e t P r i n t L e v e l + */ +static inline PrintLevel QProblem_getPrintLevel( QProblem* _THIS ) +{ + return _THIS->options.printLevel; +} + + +/* + * g e t C o u n t + */ +static inline unsigned int QProblem_getCount( QProblem* _THIS ) +{ + return _THIS->count; +} + + +/* + * r e s e t C o u n t e r + */ +static inline returnValue QProblem_resetCounter( QProblem* _THIS ) +{ + _THIS->count = 0; + return SUCCESSFUL_RETURN; +} + + + +/***************************************************************************** + * P R O T E C T E D * + *****************************************************************************/ + + +/* + * s e t H + */ +static inline returnValue QProblem_setHM( QProblem* _THIS, DenseMatrix* H_new ) +{ + if ( H_new == 0 ) + return QProblem_setH( _THIS,(real_t*)0 ); + else + return QProblem_setH( _THIS,DenseMatrix_getVal(H_new) ); +} + + +/* + * s e t H + */ +static inline returnValue QProblem_setH( QProblem* _THIS, real_t* const H_new ) +{ + /* if null pointer is passed, Hessian is set to zero matrix + * (or stays identity matrix) */ + if ( H_new == 0 ) + { + if ( _THIS->hessianType == HST_IDENTITY ) + return SUCCESSFUL_RETURN; + + _THIS->hessianType = HST_ZERO; + + _THIS->H = 0; + } + else + { + DenseMatrixCON( _THIS->H,QProblem_getNV( _THIS ),QProblem_getNV( _THIS ),QProblem_getNV( _THIS ),H_new ); + } + + return SUCCESSFUL_RETURN; +} + + +/* + * s e t G + */ +static inline returnValue QProblem_setG( QProblem* _THIS, const real_t* const g_new ) +{ + unsigned int nV = (unsigned int)QProblem_getNV( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + if ( g_new == 0 ) + return THROWERROR( RET_INVALID_ARGUMENTS ); + + memcpy( _THIS->g,g_new,nV*sizeof(real_t) ); + + return SUCCESSFUL_RETURN; +} + + +/* + * s e t L B + */ +static inline returnValue QProblem_setLB( QProblem* _THIS, const real_t* const lb_new ) +{ + unsigned int i; + unsigned int nV = (unsigned int)QProblem_getNV( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + if ( lb_new != 0 ) + { + memcpy( _THIS->lb,lb_new,nV*sizeof(real_t) ); + } + else + { + /* if no lower bounds are specified, set them to -infinity */ + for( i=0; ilb[i] = -QPOASES_INFTY; + } + + return SUCCESSFUL_RETURN; +} + + +/* + * s e t L B + */ +static inline returnValue QProblem_setLBn( QProblem* _THIS, int number, real_t value ) +{ + int nV = QProblem_getNV( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + if ( ( number >= 0 ) && ( number < nV ) ) + { + _THIS->lb[number] = value; + return SUCCESSFUL_RETURN; + } + else + { + return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); + } +} + + +/* + * s e t U B + */ +static inline returnValue QProblem_setUB( QProblem* _THIS, const real_t* const ub_new ) +{ + unsigned int i; + unsigned int nV = (unsigned int)QProblem_getNV( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + if ( ub_new != 0 ) + { + memcpy( _THIS->ub,ub_new,nV*sizeof(real_t) ); + } + else + { + /* if no upper bounds are specified, set them to infinity */ + for( i=0; iub[i] = QPOASES_INFTY; + } + + return SUCCESSFUL_RETURN; +} + + +/* + * s e t U B + */ +static inline returnValue QProblem_setUBn( QProblem* _THIS, int number, real_t value ) +{ + int nV = QProblem_getNV( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + if ( ( number >= 0 ) && ( number < nV ) ) + { + _THIS->ub[number] = value; + + return SUCCESSFUL_RETURN; + } + else + { + return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); + } +} + + + +/* + * i s B l o c k i n g + */ +static inline BooleanType QProblem_isBlocking( QProblem* _THIS, + real_t num, + real_t den, + real_t epsNum, + real_t epsDen, + real_t* t + ) +{ + if ( ( den >= epsDen ) && ( num >= epsNum ) ) + { + if ( num < (*t)*den ) + return BT_TRUE; + } + + return BT_FALSE; +} + + + +/* + * g e t C o n s t r a i n t s + */ +static inline returnValue QProblem_getConstraints( QProblem* _THIS, Constraints* _constraints ) +{ + int nV = QProblem_getNV( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + ConstraintsCPY( _THIS->constraints,_constraints ); + + return SUCCESSFUL_RETURN; +} + + + +/* + * g e t N C + */ +static inline int QProblem_getNC( QProblem* _THIS ) +{ + return Constraints_getNC( _THIS->constraints ); +} + + +/* + * g e t N E C + */ +static inline int QProblem_getNEC( QProblem* _THIS ) +{ + return Constraints_getNEC( _THIS->constraints ); +} + + +/* + * g e t N A C + */ +static inline int QProblem_getNAC( QProblem* _THIS ) +{ + return Constraints_getNAC( _THIS->constraints ); +} + + +/* + * g e t N I A C + */ +static inline int QProblem_getNIAC( QProblem* _THIS ) +{ + return Constraints_getNIAC( _THIS->constraints ); +} + + + +/***************************************************************************** + * P R O T E C T E D * + *****************************************************************************/ + + +/* + * s e t A + */ +static inline returnValue QProblem_setAM( QProblem* _THIS, DenseMatrix *A_new ) +{ + if ( A_new == 0 ) + return QProblem_setA( _THIS,(real_t*)0 ); + else + return QProblem_setA( _THIS,DenseMatrix_getVal(A_new) ); +} + + +/* + * s e t A + */ +static inline returnValue QProblem_setA( QProblem* _THIS, real_t* const A_new ) +{ + int j; + int nV = QProblem_getNV( _THIS ); + int nC = QProblem_getNC( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + if ( A_new == 0 ) + return THROWERROR( RET_INVALID_ARGUMENTS ); + + DenseMatrixCON( _THIS->A,QProblem_getNC( _THIS ),QProblem_getNV( _THIS ),QProblem_getNV( _THIS ),A_new ); + + DenseMatrix_times( _THIS->A,1, 1.0, _THIS->x, nV, 0.0, _THIS->Ax, nC); + + for( j=0; jAx_u[j] = _THIS->ubA[j] - _THIS->Ax[j]; + _THIS->Ax_l[j] = _THIS->Ax[j] - _THIS->lbA[j]; + + /* (ckirches) disable constraints with empty rows */ + if ( qpOASES_isZero( DenseMatrix_getRowNorm( _THIS->A,j,2 ),QPOASES_ZERO ) == BT_TRUE ) + Constraints_setType( _THIS->constraints,j,ST_DISABLED ); + } + + return SUCCESSFUL_RETURN; +} + + +/* + * s e t L B A + */ +static inline returnValue QProblem_setLBA( QProblem* _THIS, const real_t* const lbA_new ) +{ + unsigned int i; + unsigned int nV = (unsigned int)QProblem_getNV( _THIS ); + unsigned int nC = (unsigned int)QProblem_getNC( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + if ( lbA_new != 0 ) + { + memcpy( _THIS->lbA,lbA_new,nC*sizeof(real_t) ); + } + else + { + /* if no lower constraints' bounds are specified, set them to -infinity */ + for( i=0; ilbA[i] = -QPOASES_INFTY; + } + + return SUCCESSFUL_RETURN; +} + + +/* + * s e t L B A + */ +static inline returnValue QProblem_setLBAn( QProblem* _THIS, int number, real_t value ) +{ + int nV = QProblem_getNV( _THIS ); + int nC = QProblem_getNC( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + if ( ( number >= 0 ) && ( number < nC ) ) + { + _THIS->lbA[number] = value; + return SUCCESSFUL_RETURN; + } + else + return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); +} + + +/* + * s e t U B A + */ +static inline returnValue QProblem_setUBA( QProblem* _THIS, const real_t* const ubA_new ) +{ + unsigned int i; + unsigned int nV = (unsigned int)QProblem_getNV( _THIS ); + unsigned int nC = (unsigned int)QProblem_getNC( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + if ( ubA_new != 0 ) + { + memcpy( _THIS->ubA,ubA_new,nC*sizeof(real_t) ); + } + else + { + /* if no upper constraints' bounds are specified, set them to infinity */ + for( i=0; iubA[i] = QPOASES_INFTY; + } + + return SUCCESSFUL_RETURN; +} + + +/* + * s e t U B A + */ +static inline returnValue QProblem_setUBAn( QProblem* _THIS, int number, real_t value ) +{ + int nV = QProblem_getNV( _THIS ); + int nC = QProblem_getNC( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + if ( ( number >= 0 ) && ( number < nC ) ) + { + _THIS->ubA[number] = value; + return SUCCESSFUL_RETURN; + } + else + return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); +} + + +END_NAMESPACE_QPOASES + + +#endif /* QPOASES_QPROBLEM_H */ + + +/* + * end of file + */ diff --git a/phonelibs/acados/include/qpOASES_e/QProblemB.h b/phonelibs/acados/include/qpOASES_e/QProblemB.h new file mode 100644 index 0000000000..ee5157dda7 --- /dev/null +++ b/phonelibs/acados/include/qpOASES_e/QProblemB.h @@ -0,0 +1,1641 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, + * Christian Kirches et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file include/qpOASES_e/QProblemB.h + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches + * \version 3.1embedded + * \date 2007-2015 + * + * Declaration of the QProblemB class which is able to use the newly + * developed online active set strategy for parametric quadratic programming + * for problems with (simple) bounds only. + */ + + + +#ifndef QPOASES_QPROBLEMB_H +#define QPOASES_QPROBLEMB_H + + +#include +#include +#include +#include + + +BEGIN_NAMESPACE_QPOASES + +typedef struct { + Bounds *emptyBounds; + Bounds *auxiliaryBounds; + + real_t *ub_new_far; + real_t *lb_new_far; + + real_t *g_new; + real_t *lb_new; + real_t *ub_new; + + real_t *g_new2; + real_t *lb_new2; + real_t *ub_new2; + + real_t *Hx; + + real_t *_H; + + real_t *g_original; + real_t *lb_original; + real_t *ub_original; + + real_t *delta_xFR; + real_t *delta_xFX; + real_t *delta_yFX; + real_t *delta_g; + real_t *delta_lb; + real_t *delta_ub; + + real_t *gMod; + + real_t *num; + real_t *den; + + real_t *rhs; + real_t *r; +} QProblemB_ws; + +int QProblemB_ws_calculateMemorySize( unsigned int nV ); + +char *QProblemB_ws_assignMemory( unsigned int nV, QProblemB_ws **mem, void *raw_memory ); + +QProblemB_ws *QProblemB_ws_createMemory( unsigned int nV ); + + +/** + * \brief Implements the online active set strategy for box-constrained QPs. + * + * Class for setting up and solving quadratic programs with bounds (= box constraints) only. + * The main feature is the possibility to use the newly developed online active set strategy + * for parametric quadratic programming. + * + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches + * \version 3.1embedded + * \date 2007-2015 + */ +typedef struct +{ + QProblemB_ws *ws; + Bounds *bounds; /**< Data structure for problem's bounds. */ + Flipper *flipper; /**< Struct for making a temporary copy of the matrix factorisations. */ + + DenseMatrix* H; /**< Hessian matrix pointer. */ + + Options options; /**< Struct containing all user-defined options for solving QPs. */ + TabularOutput tabularOutput; /**< Struct storing information for tabular output (printLevel == PL_TABULAR). */ + + real_t *g; /**< Gradient. */ + real_t *lb; /**< Lower bound vector (on variables). */ + real_t *ub; /**< Upper bound vector (on variables). */ + + real_t *R; /**< Cholesky factor of H (i.e. H = R^T*R). */ + + real_t *x; /**< Primal solution vector. */ + real_t *y; /**< Dual solution vector. */ + + real_t *delta_xFR_TMP; /**< Temporary for determineStepDirection */ + + real_t tau; /**< Last homotopy step length. */ + real_t regVal; /**< Holds the offset used to regularise Hessian matrix (zero by default). */ + + real_t ramp0; /**< Start value for Ramping Strategy. */ + real_t ramp1; /**< Final value for Ramping Strategy. */ + + QProblemStatus status; /**< Current status of the solution process. */ + HessianType hessianType; /**< Type of Hessian matrix. */ + + BooleanType haveCholesky; /**< Flag indicating whether Cholesky decomposition has already been setup. */ + BooleanType infeasible; /**< QP infeasible? */ + BooleanType unbounded; /**< QP unbounded? */ + + int rampOffset; /**< Offset index for Ramping. */ + unsigned int count; /**< Counts the number of hotstart function calls (internal usage only!). */ +} QProblemB; + +int QProblemB_calculateMemorySize( unsigned int nV ); + +char *QProblemB_assignMemory( unsigned int nV, QProblemB **mem, void *raw_memory ); + +QProblemB *QProblemB_createMemory( unsigned int nV ); + + +/** Constructor which takes the QP dimension and Hessian type + * information. If the Hessian is the zero (i.e. HST_ZERO) or the + * identity matrix (i.e. HST_IDENTITY), respectively, no memory + * is allocated for it and a NULL pointer can be passed for it + * to the init() functions. */ +void QProblemBCON( QProblemB* _THIS, + int _nV, /**< Number of variables. */ + HessianType _hessianType /**< Type of Hessian matrix. */ + ); + +void QProblemBCPY( QProblemB* FROM, + QProblemB* TO + ); + + +/** Clears all data structures of QProblemB except for QP data. + * \return SUCCESSFUL_RETURN \n + RET_RESET_FAILED */ +returnValue QProblemB_reset( QProblemB* _THIS ); + + +/** Initialises a simply bounded QP problem with given QP data and tries to solve it + * using at most nWSR iterations. + * + * Note: This function internally calls solveInitialQP for initialisation! + * + * \return SUCCESSFUL_RETURN \n + RET_INIT_FAILED \n + RET_INIT_FAILED_CHOLESKY \n + RET_INIT_FAILED_HOTSTART \n + RET_INIT_FAILED_INFEASIBILITY \n + RET_INIT_FAILED_UNBOUNDEDNESS \n + RET_MAX_NWSR_REACHED \n + RET_INVALID_ARGUMENTS */ +returnValue QProblemB_initM( QProblemB* _THIS, + DenseMatrix *_H, /**< Hessian matrix. */ + const real_t* const _g, /**< Gradient vector. */ + const real_t* const _lb, /**< Lower bounds (on variables). \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const _ub, /**< Upper bounds (on variables). \n + If no upper bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. \n + Output: Number of performed working set recalculations. */ + real_t* const cputime /**< Input: Maximum CPU time allowed for QP initialisation. \n + Output: CPU time spent for QP initialisation (if pointer passed). */ + ); + +/** Initialises a simply bounded QP problem with given QP data and tries to solve it + * using at most nWSR iterations. + * + * Note: This function internally calls solveInitialQP for initialisation! + * + * \return SUCCESSFUL_RETURN \n + RET_INIT_FAILED \n + RET_INIT_FAILED_CHOLESKY \n + RET_INIT_FAILED_HOTSTART \n + RET_INIT_FAILED_INFEASIBILITY \n + RET_INIT_FAILED_UNBOUNDEDNESS \n + RET_MAX_NWSR_REACHED \n + RET_INVALID_ARGUMENTS */ +returnValue QProblemB_init( QProblemB* _THIS, + real_t* const _H, /**< Hessian matrix. \n + If Hessian matrix is trivial, a NULL pointer can be passed. */ + const real_t* const _g, /**< Gradient vector. */ + const real_t* const _lb, /**< Lower bounds (on variables). \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const _ub, /**< Upper bounds (on variables). \n + If no upper bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. \n + Output: Number of performed working set recalculations. */ + real_t* const cputime /**< Input: Maximum CPU time allowed for QP initialisation. \n + Output: CPU time spent for QP initialisation (if pointer passed). */ + ); + +/** Initialises a simply bounded QP problem with given QP data to be read from files and solves it + * using at most nWSR iterations. + * + * Note: This function internally calls solveInitialQP for initialisation! + * + * \return SUCCESSFUL_RETURN \n + RET_INIT_FAILED \n + RET_INIT_FAILED_CHOLESKY \n + RET_INIT_FAILED_HOTSTART \n + RET_INIT_FAILED_INFEASIBILITY \n + RET_INIT_FAILED_UNBOUNDEDNESS \n + RET_MAX_NWSR_REACHED \n + RET_UNABLE_TO_READ_FILE */ +returnValue QProblemB_initF( QProblemB* _THIS, + const char* const H_file, /**< Name of file where Hessian matrix is stored. \n + If Hessian matrix is trivial, a NULL pointer can be passed. */ + const char* const g_file, /**< Name of file where gradient vector is stored. */ + const char* const lb_file, /**< Name of file where lower bound vector. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const char* const ub_file, /**< Name of file where upper bound vector. \n + If no upper bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. \n + Output: Number of performed working set recalculations. */ + real_t* const cputime /**< Input: Maximum CPU time allowed for QP initialisation. \n + Output: CPU time spent for QP initialisation (if pointer passed). */ + ); + +/** Initialises a simply bounded QP problem with given QP data and tries to solve it + * using at most nWSR iterations. Depending on the parameter constellation it: \n + * 1. 0, 0, 0 : starts with xOpt = 0, yOpt = 0 and gB empty (or all implicit equality bounds), \n + * 2. xOpt, 0, 0 : starts with xOpt, yOpt = 0 and obtain gB by "clipping", \n + * 3. 0, yOpt, 0 : starts with xOpt = 0, yOpt and obtain gB from yOpt != 0, \n + * 4. 0, 0, gB: starts with xOpt = 0, yOpt = 0 and gB, \n + * 5. xOpt, yOpt, 0 : starts with xOpt, yOpt and obtain gB from yOpt != 0, \n + * 6. xOpt, 0, gB: starts with xOpt, yOpt = 0 and gB, \n + * 7. xOpt, yOpt, gB: starts with xOpt, yOpt and gB (assume them to be consistent!) + * + * Note: This function internally calls solveInitialQP for initialisation! + * + * \return SUCCESSFUL_RETURN \n + RET_INIT_FAILED \n + RET_INIT_FAILED_CHOLESKY \n + RET_INIT_FAILED_HOTSTART \n + RET_INIT_FAILED_INFEASIBILITY \n + RET_INIT_FAILED_UNBOUNDEDNESS \n + RET_MAX_NWSR_REACHED \n + RET_INVALID_ARGUMENTS */ +returnValue QProblemB_initMW( QProblemB* _THIS, + DenseMatrix *_H, /**< Hessian matrix. */ + const real_t* const _g, /**< Gradient vector. */ + const real_t* const _lb, /**< Lower bounds (on variables). \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const _ub, /**< Upper bounds (on variables). \n + If no upper bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. \n + Output: Number of performed working set recalculations. */ + real_t* const cputime, /**< Input: Maximum CPU time allowed for QP initialisation. \n + Output: CPU time spent for QP initialisation. */ + const real_t* const xOpt, /**< Optimal primal solution vector. A NULL pointer can be passed. \n + (If a null pointer is passed, the old primal solution is kept!) */ + const real_t* const yOpt, /**< Optimal dual solution vector. A NULL pointer can be passed. \n + (If a null pointer is passed, the old dual solution is kept!) */ + Bounds* const guessedBounds, /**< Optimal working set of bounds for solution (xOpt,yOpt). \n + (If a null pointer is passed, all bounds are assumed inactive!) */ + const real_t* const _R /**< Pre-computed (upper triangular) Cholesky factor of Hessian matrix. + The Cholesky factor must be stored in a real_t array of size nV*nV + in row-major format. Note: Only used if xOpt/yOpt and gB are NULL! \n + (If a null pointer is passed, Cholesky decomposition is computed internally!) */ + ); + +/** Initialises a simply bounded QP problem with given QP data and tries to solve it + * using at most nWSR iterations. Depending on the parameter constellation it: \n + * 1. 0, 0, 0 : starts with xOpt = 0, yOpt = 0 and gB empty (or all implicit equality bounds), \n + * 2. xOpt, 0, 0 : starts with xOpt, yOpt = 0 and obtain gB by "clipping", \n + * 3. 0, yOpt, 0 : starts with xOpt = 0, yOpt and obtain gB from yOpt != 0, \n + * 4. 0, 0, gB: starts with xOpt = 0, yOpt = 0 and gB, \n + * 5. xOpt, yOpt, 0 : starts with xOpt, yOpt and obtain gB from yOpt != 0, \n + * 6. xOpt, 0, gB: starts with xOpt, yOpt = 0 and gB, \n + * 7. xOpt, yOpt, gB: starts with xOpt, yOpt and gB (assume them to be consistent!) + * + * Note: This function internally calls solveInitialQP for initialisation! + * + * \return SUCCESSFUL_RETURN \n + RET_INIT_FAILED \n + RET_INIT_FAILED_CHOLESKY \n + RET_INIT_FAILED_HOTSTART \n + RET_INIT_FAILED_INFEASIBILITY \n + RET_INIT_FAILED_UNBOUNDEDNESS \n + RET_MAX_NWSR_REACHED \n + RET_INVALID_ARGUMENTS */ +returnValue QProblemB_initW( QProblemB* _THIS, + real_t* const _H, /**< Hessian matrix. \n + If Hessian matrix is trivial, a NULL pointer can be passed. */ + const real_t* const _g, /**< Gradient vector. */ + const real_t* const _lb, /**< Lower bounds (on variables). \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const _ub, /**< Upper bounds (on variables). \n + If no upper bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. \n + Output: Number of performed working set recalculations. */ + real_t* const cputime, /**< Input: Maximum CPU time allowed for QP initialisation. \n + Output: CPU time spent for QP initialisation. */ + const real_t* const xOpt, /**< Optimal primal solution vector. A NULL pointer can be passed. \n + (If a null pointer is passed, the old primal solution is kept!) */ + const real_t* const yOpt, /**< Optimal dual solution vector. A NULL pointer can be passed. \n + (If a null pointer is passed, the old dual solution is kept!) */ + Bounds* const guessedBounds, /**< Optimal working set of bounds for solution (xOpt,yOpt). \n + (If a null pointer is passed, all bounds are assumed inactive!) */ + const real_t* const _R /**< Pre-computed (upper triangular) Cholesky factor of Hessian matrix. + The Cholesky factor must be stored in a real_t array of size nV*nV + in row-major format. Note: Only used if xOpt/yOpt and gB are NULL! \n + (If a null pointer is passed, Cholesky decomposition is computed internally!) */ + ); + +/** Initialises a simply bounded QP problem with given QP data to be read from files and solves it + * using at most nWSR iterations. Depending on the parameter constellation it: \n + * 1. 0, 0, 0 : starts with xOpt = 0, yOpt = 0 and gB empty (or all implicit equality bounds), \n + * 2. xOpt, 0, 0 : starts with xOpt, yOpt = 0 and obtain gB by "clipping", \n + * 3. 0, yOpt, 0 : starts with xOpt = 0, yOpt and obtain gB from yOpt != 0, \n + * 4. 0, 0, gB: starts with xOpt = 0, yOpt = 0 and gB, \n + * 5. xOpt, yOpt, 0 : starts with xOpt, yOpt and obtain gB from yOpt != 0, \n + * 6. xOpt, 0, gB: starts with xOpt, yOpt = 0 and gB, \n + * 7. xOpt, yOpt, gB: starts with xOpt, yOpt and gB (assume them to be consistent!) + * + * Note: This function internally calls solveInitialQP for initialisation! + * + * \return SUCCESSFUL_RETURN \n + RET_INIT_FAILED \n + RET_INIT_FAILED_CHOLESKY \n + RET_INIT_FAILED_HOTSTART \n + RET_INIT_FAILED_INFEASIBILITY \n + RET_INIT_FAILED_UNBOUNDEDNESS \n + RET_MAX_NWSR_REACHED \n + RET_UNABLE_TO_READ_FILE */ +returnValue QProblemB_initFW( QProblemB* _THIS, + const char* const H_file, /**< Name of file where Hessian matrix is stored. \n + If Hessian matrix is trivial, a NULL pointer can be passed. */ + const char* const g_file, /**< Name of file where gradient vector is stored. */ + const char* const lb_file, /**< Name of file where lower bound vector. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const char* const ub_file, /**< Name of file where upper bound vector. \n + If no upper bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. \n + Output: Number of performed working set recalculations. */ + real_t* const cputime, /**< Input: Maximum CPU time allowed for QP initialisation. \n + Output: CPU time spent for QP initialisation. */ + const real_t* const xOpt, /**< Optimal primal solution vector. A NULL pointer can be passed. \n + (If a null pointer is passed, the old primal solution is kept!) */ + const real_t* const yOpt, /**< Optimal dual solution vector. A NULL pointer can be passed. \n + (If a null pointer is passed, the old dual solution is kept!) */ + Bounds* const guessedBounds, /**< Optimal working set of bounds for solution (xOpt,yOpt). \n + (If a null pointer is passed, all bounds are assumed inactive!) */ + const char* const R_file /**< Name of the file where a pre-computed (upper triangular) Cholesky factor + of the Hessian matrix is stored. \n + (If a null pointer is passed, Cholesky decomposition is computed internally!) */ + ); + + +/** Solves an initialised QP sequence using the online active set strategy. + * By default, QP solution is started from previous solution. + * + * Note: This function internally calls solveQP/solveRegularisedQP + * for solving an initialised QP! + * + * \return SUCCESSFUL_RETURN \n + RET_MAX_NWSR_REACHED \n + RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n + RET_HOTSTART_FAILED \n + RET_SHIFT_DETERMINATION_FAILED \n + RET_STEPDIRECTION_DETERMINATION_FAILED \n + RET_STEPLENGTH_DETERMINATION_FAILED \n + RET_HOMOTOPY_STEP_FAILED \n + RET_HOTSTART_STOPPED_INFEASIBILITY \n + RET_HOTSTART_STOPPED_UNBOUNDEDNESS */ +returnValue QProblemB_hotstart( QProblemB* _THIS, + const real_t* const g_new, /**< Gradient of neighbouring QP to be solved. */ + const real_t* const lb_new, /**< Lower bounds of neighbouring QP to be solved. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const ub_new, /**< Upper bounds of neighbouring QP to be solved. \n + If no upper bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations; \n + Output: Number of performed working set recalculations. */ + real_t* const cputime /**< Input: Maximum CPU time allowed for QP solution. \n + Output: CPU time spent for QP solution (or to perform nWSR iterations). */ + ); + +/** Solves an initialised QP sequence using the online active set strategy, + * where QP data is read from files. QP solution is started from previous solution. + * + * Note: This function internally calls solveQP/solveRegularisedQP + * for solving an initialised QP! + * + * \return SUCCESSFUL_RETURN \n + RET_MAX_NWSR_REACHED \n + RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n + RET_HOTSTART_FAILED \n + RET_SHIFT_DETERMINATION_FAILED \n + RET_STEPDIRECTION_DETERMINATION_FAILED \n + RET_STEPLENGTH_DETERMINATION_FAILED \n + RET_HOMOTOPY_STEP_FAILED \n + RET_HOTSTART_STOPPED_INFEASIBILITY \n + RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n + RET_UNABLE_TO_READ_FILE \n + RET_INVALID_ARGUMENTS */ +returnValue QProblemB_hotstartF( QProblemB* _THIS, + const char* const g_file, /**< Name of file where gradient, of neighbouring QP to be solved, is stored. */ + const char* const lb_file, /**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const char* const ub_file, /**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n + If no upper bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations; \n + Output: Number of performed working set recalculations. */ + real_t* const cputime /**< Input: Maximum CPU time allowed for QP solution. \n + Output: CPU time spent for QP solution (or to perform nWSR iterations). */ + ); + +/** Solves an initialised QP sequence using the online active set strategy. + * By default, QP solution is started from previous solution. If a guess + * for the working set is provided, an initialised homotopy is performed. + * + * Note: This function internally calls solveQP/solveRegularisedQP + * for solving an initialised QP! + * + * \return SUCCESSFUL_RETURN \n + RET_MAX_NWSR_REACHED \n + RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n + RET_HOTSTART_FAILED \n + RET_SHIFT_DETERMINATION_FAILED \n + RET_STEPDIRECTION_DETERMINATION_FAILED \n + RET_STEPLENGTH_DETERMINATION_FAILED \n + RET_HOMOTOPY_STEP_FAILED \n + RET_HOTSTART_STOPPED_INFEASIBILITY \n + RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n + RET_SETUP_AUXILIARYQP_FAILED */ +returnValue QProblemB_hotstartW( QProblemB* _THIS, + const real_t* const g_new, /**< Gradient of neighbouring QP to be solved. */ + const real_t* const lb_new, /**< Lower bounds of neighbouring QP to be solved. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const ub_new, /**< Upper bounds of neighbouring QP to be solved. \n + If no upper bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations; \n + Output: Number of performed working set recalculations. */ + real_t* const cputime, /**< Input: Maximum CPU time allowed for QP solution. \n + Output: CPU time spent for QP solution (or to perform nWSR iterations). */ + Bounds* const guessedBounds /**< Optimal working set of bounds for solution (xOpt,yOpt). \n + (If a null pointer is passed, the previous working set is kept!) */ + ); + +/** Solves an initialised QP sequence using the online active set strategy, + * where QP data is read from files. + * By default, QP solution is started from previous solution. If a guess + * for the working set is provided, an initialised homotopy is performed. + * + * Note: This function internally calls solveQP/solveRegularisedQP + * for solving an initialised QP! + * + * \return SUCCESSFUL_RETURN \n + RET_MAX_NWSR_REACHED \n + RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n + RET_HOTSTART_FAILED \n + RET_SHIFT_DETERMINATION_FAILED \n + RET_STEPDIRECTION_DETERMINATION_FAILED \n + RET_STEPLENGTH_DETERMINATION_FAILED \n + RET_HOMOTOPY_STEP_FAILED \n + RET_HOTSTART_STOPPED_INFEASIBILITY \n + RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n + RET_UNABLE_TO_READ_FILE \n + RET_INVALID_ARGUMENTS \n + RET_SETUP_AUXILIARYQP_FAILED */ +returnValue QProblemB_hotstartFW( QProblemB* _THIS, + const char* const g_file, /**< Name of file where gradient, of neighbouring QP to be solved, is stored. */ + const char* const lb_file, /**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const char* const ub_file, /**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n + If no upper bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations; \n + Output: Number of performed working set recalculations. */ + real_t* const cputime, /**< Input: Maximum CPU time allowed for QP solution. \n + Output: CPU time spent for QP solution (or to perform nWSR iterations). */ + Bounds* const guessedBounds /**< Optimal working set of bounds for solution (xOpt,yOpt). \n + (If a null pointer is passed, the previous working set is kept!) */ + ); + + +/** Writes a vector with the state of the working set + * \return SUCCESSFUL_RETURN \n + * RET_INVALID_ARGUMENTS */ +returnValue QProblemB_getWorkingSet( QProblemB* _THIS, + real_t* workingSet /** Output: array containing state of the working set. */ + ); + +/** Writes a vector with the state of the working set of bounds + * \return SUCCESSFUL_RETURN \n + * RET_INVALID_ARGUMENTS */ +returnValue QProblemB_getWorkingSetBounds( QProblemB* _THIS, + real_t* workingSetB /** Output: array containing state of the working set of bounds. */ + ); + +/** Writes a vector with the state of the working set of constraints + * \return SUCCESSFUL_RETURN \n + * RET_INVALID_ARGUMENTS */ +returnValue QProblemB_getWorkingSetConstraints( QProblemB* _THIS, + real_t* workingSetC /** Output: array containing state of the working set of constraints. */ + ); + + +/** Returns current bounds object of the QP (deep copy). + * \return SUCCESSFUL_RETURN \n + RET_QPOBJECT_NOT_SETUP */ +static inline returnValue QProblemB_getBounds( QProblemB* _THIS, + Bounds* _bounds /** Output: Bounds object. */ + ); + + +/** Returns the number of variables. + * \return Number of variables. */ +static inline int QProblemB_getNV( QProblemB* _THIS ); + +/** Returns the number of free variables. + * \return Number of free variables. */ +static inline int QProblemB_getNFR( QProblemB* _THIS ); + +/** Returns the number of fixed variables. + * \return Number of fixed variables. */ +static inline int QProblemB_getNFX( QProblemB* _THIS ); + +/** Returns the number of implicitly fixed variables. + * \return Number of implicitly fixed variables. */ +static inline int QProblemB_getNFV( QProblemB* _THIS ); + +/** Returns the dimension of null space. + * \return Dimension of null space. */ +int QProblemB_getNZ( QProblemB* _THIS ); + + +/** Returns the optimal objective function value. + * \return finite value: Optimal objective function value (QP was solved) \n + +infinity: QP was not yet solved */ +real_t QProblemB_getObjVal( QProblemB* _THIS ); + +/** Returns the objective function value at an arbitrary point x. + * \return Objective function value at point x */ +real_t QProblemB_getObjValX( QProblemB* _THIS, + const real_t* const _x /**< Point at which the objective function shall be evaluated. */ + ); + +/** Returns the primal solution vector. + * \return SUCCESSFUL_RETURN \n + RET_QP_NOT_SOLVED */ +returnValue QProblemB_getPrimalSolution( QProblemB* _THIS, + real_t* const xOpt /**< Output: Primal solution vector (if QP has been solved). */ + ); + +/** Returns the dual solution vector. + * \return SUCCESSFUL_RETURN \n + RET_QP_NOT_SOLVED */ +returnValue QProblemB_getDualSolution( QProblemB* _THIS, + real_t* const yOpt /**< Output: Dual solution vector (if QP has been solved). */ + ); + + +/** Returns status of the solution process. + * \return Status of solution process. */ +static inline QProblemStatus QProblemB_getStatus( QProblemB* _THIS ); + + +/** Returns if the QProblem object is initialised. + * \return BT_TRUE: QProblemB initialised \n + BT_FALSE: QProblemB not initialised */ +static inline BooleanType QProblemB_isInitialised( QProblemB* _THIS ); + +/** Returns if the QP has been solved. + * \return BT_TRUE: QProblemB solved \n + BT_FALSE: QProblemB not solved */ +static inline BooleanType QProblemB_isSolved( QProblemB* _THIS ); + +/** Returns if the QP is infeasible. + * \return BT_TRUE: QP infeasible \n + BT_FALSE: QP feasible (or not known to be infeasible!) */ +static inline BooleanType QProblemB_isInfeasible( QProblemB* _THIS ); + +/** Returns if the QP is unbounded. + * \return BT_TRUE: QP unbounded \n + BT_FALSE: QP unbounded (or not known to be unbounded!) */ +static inline BooleanType QProblemB_isUnbounded( QProblemB* _THIS ); + + +/** Returns Hessian type flag (type is not determined due to _THIS call!). + * \return Hessian type. */ +static inline HessianType QProblemB_getHessianType( QProblemB* _THIS ); + +/** Changes the print level. + * \return SUCCESSFUL_RETURN */ +static inline returnValue QProblemB_setHessianType( QProblemB* _THIS, + HessianType _hessianType /**< New Hessian type. */ + ); + +/** Returns if the QP has been internally regularised. + * \return BT_TRUE: Hessian is internally regularised for QP solution \n + BT_FALSE: No internal Hessian regularisation is used for QP solution */ +static inline BooleanType QProblemB_usingRegularisation( QProblemB* _THIS ); + +/** Returns current options struct. + * \return Current options struct. */ +static inline Options QProblemB_getOptions( QProblemB* _THIS ); + +/** Overrides current options with given ones. + * \return SUCCESSFUL_RETURN */ +static inline returnValue QProblemB_setOptions( QProblemB* _THIS, + Options _options /**< New options. */ + ); + +/** Returns the print level. + * \return Print level. */ +static inline PrintLevel QProblemB_getPrintLevel( QProblemB* _THIS ); + +/** Changes the print level. + * \return SUCCESSFUL_RETURN */ +returnValue QProblemB_setPrintLevel( QProblemB* _THIS, + PrintLevel _printlevel /**< New print level. */ + ); + +/** Returns the current number of QP problems solved. + * \return Number of QP problems solved. */ +static inline unsigned int QProblemB_getCount( QProblemB* _THIS ); + +/** Resets QP problem counter (to zero). + * \return SUCCESSFUL_RETURN. */ +static inline returnValue QProblemB_resetCounter( QProblemB* _THIS ); + + +/** Prints concise list of properties of the current QP. + * \return SUCCESSFUL_RETURN \n */ +returnValue QProblemB_printProperties( QProblemB* _THIS ); + +/** Prints a list of all options and their current values. + * \return SUCCESSFUL_RETURN \n */ +returnValue QProblemB_printOptions( QProblemB* _THIS ); + + +/** If Hessian type has been set by the user, nothing is done. + * Otherwise the Hessian type is set to HST_IDENTITY, HST_ZERO, or + * HST_POSDEF (default), respectively. + * \return SUCCESSFUL_RETURN \n + RET_HESSIAN_INDEFINITE */ +returnValue QProblemB_determineHessianType( QProblemB* _THIS ); + +/** Determines type of existing constraints and bounds (i.e. implicitly fixed, unbounded etc.). + * \return SUCCESSFUL_RETURN \n + RET_SETUPSUBJECTTOTYPE_FAILED */ +returnValue QProblemB_setupSubjectToType( QProblemB* _THIS ); + +/** Determines type of new constraints and bounds (i.e. implicitly fixed, unbounded etc.). + * \return SUCCESSFUL_RETURN \n + RET_SETUPSUBJECTTOTYPE_FAILED */ +returnValue QProblemB_setupSubjectToTypeNew( QProblemB* _THIS, + const real_t* const lb_new, /**< New lower bounds. */ + const real_t* const ub_new /**< New upper bounds. */ + ); + +/** Computes the Cholesky decomposition of the (simply projected) Hessian + * (i.e. R^T*R = Z^T*H*Z). It only works in the case where Z is a simple + * projection matrix! + * Note: If Hessian turns out not to be positive definite, the Hessian type + * is set to HST_SEMIDEF accordingly. + * \return SUCCESSFUL_RETURN \n + * RET_HESSIAN_NOT_SPD \n + * RET_INDEXLIST_CORRUPTED */ +returnValue QProblemB_computeCholesky( QProblemB* _THIS ); + +/** Computes initial Cholesky decomposition of the projected Hessian making + * use of the function setupCholeskyDecomposition() or setupCholeskyDecompositionProjected(). + * \return SUCCESSFUL_RETURN \n + * RET_HESSIAN_NOT_SPD \n + * RET_INDEXLIST_CORRUPTED */ +returnValue QProblemB_setupInitialCholesky( QProblemB* _THIS ); + + +/** Obtains the desired working set for the auxiliary initial QP in + * accordance with the user specifications + * \return SUCCESSFUL_RETURN \n + RET_OBTAINING_WORKINGSET_FAILED \n + RET_INVALID_ARGUMENTS */ +returnValue QProblemB_obtainAuxiliaryWorkingSet( QProblemB* _THIS, + const real_t* const xOpt, /**< Optimal primal solution vector. + * If a NULL pointer is passed, all entries are assumed to be zero. */ + const real_t* const yOpt, /**< Optimal dual solution vector. + * If a NULL pointer is passed, all entries are assumed to be zero. */ + Bounds* const guessedBounds, /**< Guessed working set for solution (xOpt,yOpt). */ + Bounds* auxiliaryBounds /**< Input: Allocated bound object. \n + * Ouput: Working set for auxiliary QP. */ + ); + +/** Decides if lower bounds are smaller than upper bounds + * + * \return SUCCESSFUL_RETURN \n + * RET_QP_INFEASIBLE */ +returnValue QProblemB_areBoundsConsistent( QProblemB* _THIS, + const real_t* const lb, /**< Vector of lower bounds*/ + const real_t* const ub /**< Vector of upper bounds*/ + ); + +/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix. + * \return SUCCESSFUL_RETURN \n + RET_DIV_BY_ZERO */ +returnValue QProblemB_backsolveR( QProblemB* _THIS, + const real_t* const b, /**< Right hand side vector. */ + BooleanType transposed, /**< Indicates if the transposed system shall be solved. */ + real_t* const a /**< Output: Solution vector */ + ); + +/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix. \n + * Special variant for the case that _THIS function is called from within "removeBound()". + * \return SUCCESSFUL_RETURN \n + RET_DIV_BY_ZERO */ +returnValue QProblemB_backsolveRrem( QProblemB* _THIS, + const real_t* const b, /**< Right hand side vector. */ + BooleanType transposed, /**< Indicates if the transposed system shall be solved. */ + BooleanType removingBound, /**< Indicates if function is called from "removeBound()". */ + real_t* const a /**< Output: Solution vector */ + ); + + +/** Determines step direction of the shift of the QP data. + * \return SUCCESSFUL_RETURN */ +returnValue QProblemB_determineDataShift( QProblemB* _THIS, + const real_t* const g_new, /**< New gradient vector. */ + const real_t* const lb_new, /**< New lower bounds. */ + const real_t* const ub_new, /**< New upper bounds. */ + real_t* const delta_g, /**< Output: Step direction of gradient vector. */ + real_t* const delta_lb, /**< Output: Step direction of lower bounds. */ + real_t* const delta_ub, /**< Output: Step direction of upper bounds. */ + BooleanType* Delta_bB_isZero/**< Output: Indicates if active bounds are to be shifted. */ + ); + + +/** Sets up internal QP data. + * \return SUCCESSFUL_RETURN \n + RET_INVALID_ARGUMENTS */ +returnValue QProblemB_setupQPdataM( QProblemB* _THIS, + DenseMatrix *_H, /**< Hessian matrix.*/ + const real_t* const _g, /**< Gradient vector. */ + const real_t* const _lb, /**< Lower bounds (on variables). \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const _ub /**< Upper bounds (on variables). \n + If no upper bounds exist, a NULL pointer can be passed. */ + ); + +/** Sets up internal QP data. If the current Hessian is trivial + * (i.e. HST_ZERO or HST_IDENTITY) but a non-trivial one is given, + * memory for Hessian is allocated and it is set to the given one. + * \return SUCCESSFUL_RETURN \n + RET_INVALID_ARGUMENTS \n + RET_NO_HESSIAN_SPECIFIED */ +returnValue QProblemB_setupQPdata( QProblemB* _THIS, + real_t* const _H, /**< Hessian matrix. \n + If Hessian matrix is trivial,a NULL pointer can be passed. */ + const real_t* const _g, /**< Gradient vector. */ + const real_t* const _lb, /**< Lower bounds (on variables). \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const _ub /**< Upper bounds (on variables). \n + If no upper bounds exist, a NULL pointer can be passed. */ + ); + +/** Sets up internal QP data by loading it from files. If the current Hessian + * is trivial (i.e. HST_ZERO or HST_IDENTITY) but a non-trivial one is given, + * memory for Hessian is allocated and it is set to the given one. + * \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_OPEN_FILE \n + RET_UNABLE_TO_READ_FILE \n + RET_INVALID_ARGUMENTS \n + RET_NO_HESSIAN_SPECIFIED */ +returnValue QProblemB_setupQPdataFromFile( QProblemB* _THIS, + const char* const H_file, /**< Name of file where Hessian matrix, of neighbouring QP to be solved, is stored. \n + If Hessian matrix is trivial,a NULL pointer can be passed. */ + const char* const g_file, /**< Name of file where gradient, of neighbouring QP to be solved, is stored. */ + const char* const lb_file, /**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const char* const ub_file /**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n + If no upper bounds exist, a NULL pointer can be passed. */ + ); + +/** Loads new QP vectors from files (internal members are not affected!). + * \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_OPEN_FILE \n + RET_UNABLE_TO_READ_FILE \n + RET_INVALID_ARGUMENTS */ +returnValue QProblemB_loadQPvectorsFromFile( QProblemB* _THIS, + const char* const g_file, /**< Name of file where gradient, of neighbouring QP to be solved, is stored. */ + const char* const lb_file, /**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const char* const ub_file, /**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n + If no upper bounds exist, a NULL pointer can be passed. */ + real_t* const g_new, /**< Output: Gradient of neighbouring QP to be solved. */ + real_t* const lb_new, /**< Output: Lower bounds of neighbouring QP to be solved */ + real_t* const ub_new /**< Output: Upper bounds of neighbouring QP to be solved */ + ); + + +/** Sets internal infeasibility flag and throws given error in case the far bound + * strategy is not enabled (as QP might actually not be infeasible in _THIS case). + * \return RET_HOTSTART_STOPPED_INFEASIBILITY \n + RET_ENSURELI_FAILED_CYCLING \n + RET_ENSURELI_FAILED_NOINDEX */ +returnValue QProblemB_setInfeasibilityFlag( QProblemB* _THIS, + returnValue returnvalue, /**< Returnvalue to be tunneled. */ + BooleanType doThrowError /**< Flag forcing to throw an error. */ + ); + + +/** Determines if next QP iteration can be performed within given CPU time limit. + * \return BT_TRUE: CPU time limit is exceeded, stop QP solution. \n + BT_FALSE: Sufficient CPU time for next QP iteration. */ +BooleanType QProblemB_isCPUtimeLimitExceeded( QProblemB* _THIS, + const real_t* const cputime, /**< Maximum CPU time allowed for QP solution. */ + real_t starttime, /**< Start time of current QP solution. */ + int nWSR /**< Number of working set recalculations performed so far. */ + ); + + +/** Regularise Hessian matrix by adding a scaled identity matrix to it. + * \return SUCCESSFUL_RETURN \n + RET_HESSIAN_ALREADY_REGULARISED */ +returnValue QProblemB_regulariseHessian( QProblemB* _THIS ); + + +/** Sets Hessian matrix of the QP. + * \return SUCCESSFUL_RETURN */ +static inline returnValue QProblemB_setHM( QProblemB* _THIS, + DenseMatrix* H_new /**< New Hessian matrix. */ + ); + +/** Sets dense Hessian matrix of the QP. + * If a null pointer is passed and + * a) hessianType is HST_IDENTITY, nothing is done, + * b) hessianType is not HST_IDENTITY, Hessian matrix is set to zero. + * \return SUCCESSFUL_RETURN */ +static inline returnValue QProblemB_setH( QProblemB* _THIS, + real_t* const H_new /**< New dense Hessian matrix (with correct dimension!). */ + ); + +/** Changes gradient vector of the QP. + * \return SUCCESSFUL_RETURN \n + * RET_INVALID_ARGUMENTS */ +static inline returnValue QProblemB_setG( QProblemB* _THIS, + const real_t* const g_new /**< New gradient vector (with correct dimension!). */ + ); + +/** Changes lower bound vector of the QP. + * \return SUCCESSFUL_RETURN \n + * RET_QPOBJECT_NOT_SETUP */ +static inline returnValue QProblemB_setLB( QProblemB* _THIS, + const real_t* const lb_new /**< New lower bound vector (with correct dimension!). */ + ); + +/** Changes single entry of lower bound vector of the QP. + * \return SUCCESSFUL_RETURN \n + * RET_QPOBJECT_NOT_SETUP \n + * RET_INDEX_OUT_OF_BOUNDS */ +static inline returnValue QProblemB_setLBn( QProblemB* _THIS, + int number, /**< Number of entry to be changed. */ + real_t value /**< New value for entry of lower bound vector. */ + ); + +/** Changes upper bound vector of the QP. + * \return SUCCESSFUL_RETURN \n + * RET_QPOBJECT_NOT_SETUP */ +static inline returnValue QProblemB_setUB( QProblemB* _THIS, + const real_t* const ub_new /**< New upper bound vector (with correct dimension!). */ + ); + +/** Changes single entry of upper bound vector of the QP. + * \return SUCCESSFUL_RETURN \n + * RET_QPOBJECT_NOT_SETUP \n + * RET_INDEX_OUT_OF_BOUNDS */ +static inline returnValue QProblemB_setUBn( QProblemB* _THIS, + int number, /**< Number of entry to be changed. */ + real_t value /**< New value for entry of upper bound vector. */ + ); + + +/** Computes parameters for the Givens matrix G for which [x,y]*G = [z,0] + * \return SUCCESSFUL_RETURN */ +static inline void QProblemB_computeGivens( real_t xold, /**< Matrix entry to be normalised. */ + real_t yold, /**< Matrix entry to be annihilated. */ + real_t* xnew, /**< Output: Normalised matrix entry. */ + real_t* ynew, /**< Output: Annihilated matrix entry. */ + real_t* c, /**< Output: Cosine entry of Givens matrix. */ + real_t* s /**< Output: Sine entry of Givens matrix. */ + ); + +/** Applies Givens matrix determined by c and s (cf. computeGivens). + * \return SUCCESSFUL_RETURN */ +static inline void QProblemB_applyGivens( real_t c, /**< Cosine entry of Givens matrix. */ + real_t s, /**< Sine entry of Givens matrix. */ + real_t nu, /**< Further factor: s/(1+c). */ + real_t xold, /**< Matrix entry to be transformed corresponding to + * the normalised entry of the original matrix. */ + real_t yold, /**< Matrix entry to be transformed corresponding to + * the annihilated entry of the original matrix. */ + real_t* xnew, /**< Output: Transformed matrix entry corresponding to + * the normalised entry of the original matrix. */ + real_t* ynew /**< Output: Transformed matrix entry corresponding to + * the annihilated entry of the original matrix. */ + ); + + + +/** Compute relative length of homotopy in data space for termination + * criterion. + * \return Relative length in data space. */ +real_t QProblemB_getRelativeHomotopyLength( QProblemB* _THIS, + const real_t* const g_new, /**< Final gradient. */ + const real_t* const lb_new, /**< Final lower variable bounds. */ + const real_t* const ub_new /**< Final upper variable bounds. */ + ); + +/** Ramping Strategy to avoid ties. Modifies homotopy start without + * changing current active set. + * \return SUCCESSFUL_RETURN */ +returnValue QProblemB_performRamping( QProblemB* _THIS ); + + +/** ... */ +returnValue QProblemB_updateFarBounds( QProblemB* _THIS, + real_t curFarBound, /**< ... */ + int nRamp, /**< ... */ + const real_t* const lb_new, /**< ... */ + real_t* const lb_new_far, /**< ... */ + const real_t* const ub_new, /**< ... */ + real_t* const ub_new_far /**< ... */ + ); + + + +/** Performs robustified ratio test yield the maximum possible step length + * along the homotopy path. + * \return SUCCESSFUL_RETURN */ +returnValue QProblemB_performRatioTestB( QProblemB* _THIS, + int nIdx, /**< Number of ratios to be checked. */ + const int* const idxList, /**< Array containing the indices of all ratios to be checked. */ + Bounds* const subjectTo, /**< Bound object corresponding to ratios to be checked. */ + const real_t* const num, /**< Array containing all numerators for performing the ratio test. */ + const real_t* const den, /**< Array containing all denominators for performing the ratio test. */ + real_t epsNum, /**< Numerator tolerance. */ + real_t epsDen, /**< Denominator tolerance. */ + real_t* t, /**< Output: Maximum possible step length along the homotopy path. */ + int* BC_idx /**< Output: Index of blocking constraint. */ + ); + +/** Checks whether given ratio is blocking, i.e. limits the maximum step length + * along the homotopy path to a value lower than given one. + * \return SUCCESSFUL_RETURN */ +static inline BooleanType QProblemB_isBlocking( QProblemB* _THIS, + real_t num, /**< Numerator for performing the ratio test. */ + real_t den, /**< Denominator for performing the ratio test. */ + real_t epsNum, /**< Numerator tolerance. */ + real_t epsDen, /**< Denominator tolerance. */ + real_t* t /**< Input: Current maximum step length along the homotopy path, + * Output: Updated maximum possible step length along the homotopy path. */ + ); + + +/** Solves a QProblemB whose QP data is assumed to be stored in the member variables. + * A guess for its primal/dual optimal solution vectors and the corresponding + * optimal working set can be provided. + * Note: This function is internally called by all init functions! + * \return SUCCESSFUL_RETURN \n + RET_INIT_FAILED \n + RET_INIT_FAILED_CHOLESKY \n + RET_INIT_FAILED_HOTSTART \n + RET_INIT_FAILED_INFEASIBILITY \n + RET_INIT_FAILED_UNBOUNDEDNESS \n + RET_MAX_NWSR_REACHED */ +returnValue QProblemB_solveInitialQP( QProblemB* _THIS, + const real_t* const xOpt, /**< Optimal primal solution vector.*/ + const real_t* const yOpt, /**< Optimal dual solution vector. */ + Bounds* const guessedBounds, /**< Optimal working set of bounds for solution (xOpt,yOpt). */ + const real_t* const _R, /**< Pre-computed (upper triangular) Cholesky factor of Hessian matrix. */ + int* nWSR, /**< Input: Maximum number of working set recalculations; \n + * Output: Number of performed working set recalculations. */ + real_t* const cputime /**< Input: Maximum CPU time allowed for QP solution. \n + * Output: CPU time spent for QP solution (or to perform nWSR iterations). */ + ); + +/** Solves an initialised QProblemB using online active set strategy. + * Note: This function is internally called by all hotstart functions! + * \return SUCCESSFUL_RETURN \n + RET_MAX_NWSR_REACHED \n + RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n + RET_HOTSTART_FAILED \n + RET_SHIFT_DETERMINATION_FAILED \n + RET_STEPDIRECTION_DETERMINATION_FAILED \n + RET_STEPLENGTH_DETERMINATION_FAILED \n + RET_HOMOTOPY_STEP_FAILED \n + RET_HOTSTART_STOPPED_INFEASIBILITY \n + RET_HOTSTART_STOPPED_UNBOUNDEDNESS */ +returnValue QProblemB_solveQP( QProblemB* _THIS, + const real_t* const g_new, /**< Gradient of neighbouring QP to be solved. */ + const real_t* const lb_new, /**< Lower bounds of neighbouring QP to be solved. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const ub_new, /**< Upper bounds of neighbouring QP to be solved. \n + If no upper bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations; \n + Output: Number of performed working set recalculations. */ + real_t* const cputime, /**< Input: Maximum CPU time allowed for QP solution. \n + Output: CPU time spent for QP solution (or to perform nWSR iterations). */ + int nWSRperformed, /**< Number of working set recalculations already performed to solve + this QP within previous solveQP() calls. This number is + always zero, except for successive calls from solveRegularisedQP() + or when using the far bound strategy. */ + BooleanType isFirstCall /**< Indicating whether this is the first call for current QP. */ + ); + + +/** Solves an initialised QProblemB using online active set strategy. + * Note: This function is internally called by all hotstart functions! + * \return SUCCESSFUL_RETURN \n + RET_MAX_NWSR_REACHED \n + RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n + RET_HOTSTART_FAILED \n + RET_SHIFT_DETERMINATION_FAILED \n + RET_STEPDIRECTION_DETERMINATION_FAILED \n + RET_STEPLENGTH_DETERMINATION_FAILED \n + RET_HOMOTOPY_STEP_FAILED \n + RET_HOTSTART_STOPPED_INFEASIBILITY \n + RET_HOTSTART_STOPPED_UNBOUNDEDNESS */ +returnValue QProblemB_solveRegularisedQP( QProblemB* _THIS, + const real_t* const g_new, /**< Gradient of neighbouring QP to be solved. */ + const real_t* const lb_new, /**< Lower bounds of neighbouring QP to be solved. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const ub_new, /**< Upper bounds of neighbouring QP to be solved. \n + If no upper bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations; \n + Output: Number of performed working set recalculations. */ + real_t* const cputime, /**< Input: Maximum CPU time allowed for QP solution. \n + Output: CPU time spent for QP solution (or to perform nWSR iterations). */ + int nWSRperformed, /**< Number of working set recalculations already performed to solve + this QP within previous solveRegularisedQP() calls. This number is + always zero, except for successive calls when using the far bound strategy. */ + BooleanType isFirstCall /**< Indicating whether this is the first call for current QP. */ + ); + + +/** Sets up bound data structure according to auxiliaryBounds. + * (If the working set shall be setup afresh, make sure that + * bounds data structure has been resetted!) + * \return SUCCESSFUL_RETURN \n + RET_SETUP_WORKINGSET_FAILED \n + RET_INVALID_ARGUMENTS \n + RET_UNKNOWN_BUG */ +returnValue QProblemB_setupAuxiliaryWorkingSet( QProblemB* _THIS, + Bounds* const auxiliaryBounds, /**< Working set for auxiliary QP. */ + BooleanType setupAfresh /**< Flag indicating if given working set shall be + * setup afresh or by updating the current one. */ + ); + +/** Sets up the optimal primal/dual solution of the auxiliary initial QP. + * \return SUCCESSFUL_RETURN */ +returnValue QProblemB_setupAuxiliaryQPsolution( QProblemB* _THIS, + const real_t* const xOpt, /**< Optimal primal solution vector. + * If a NULL pointer is passed, all entries are set to zero. */ + const real_t* const yOpt /**< Optimal dual solution vector. + * If a NULL pointer is passed, all entries are set to zero. */ + ); + +/** Sets up gradient of the auxiliary initial QP for given + * optimal primal/dual solution and given initial working set + * (assumes that members X, Y and BOUNDS have already been (ialised!). + * \return SUCCESSFUL_RETURN */ +returnValue QProblemB_setupAuxiliaryQPgradient( QProblemB* _THIS ); + +/** Sets up bounds of the auxiliary initial QP for given + * optimal primal/dual solution and given initial working set + * (assumes that members X, Y and BOUNDS have already been initialised!). + * \return SUCCESSFUL_RETURN \n + RET_UNKNOWN_BUG */ +returnValue QProblemB_setupAuxiliaryQPbounds( QProblemB* _THIS, + BooleanType useRelaxation /**< Flag indicating if inactive bounds shall be relaxed. */ + ); + + +/** Updates QP vectors, working sets and internal data structures in order to + start from an optimal solution corresponding to initial guesses of the working + set for bounds + * \return SUCCESSFUL_RETURN \n + * RET_SETUP_AUXILIARYQP_FAILED */ +returnValue QProblemB_setupAuxiliaryQP( QProblemB* _THIS, + Bounds* const guessedBounds /**< Initial guess for working set of bounds. */ + ); + +/** Determines step direction of the homotopy path. + * \return SUCCESSFUL_RETURN \n + RET_STEPDIRECTION_FAILED_CHOLESKY */ +returnValue QProblemB_determineStepDirection( QProblemB* _THIS, + const real_t* const delta_g, /**< Step direction of gradient vector. */ + const real_t* const delta_lb, /**< Step direction of lower bounds. */ + const real_t* const delta_ub, /**< Step direction of upper bounds. */ + BooleanType Delta_bB_isZero, /**< Indicates if active bounds are to be shifted. */ + real_t* const delta_xFX, /**< Output: Primal homotopy step direction of fixed variables. */ + real_t* const delta_xFR, /**< Output: Primal homotopy step direction of free variables. */ + real_t* const delta_yFX /**< Output: Dual homotopy step direction of fixed variables' multiplier. */ + ); + +/** Determines the maximum possible step length along the homotopy path + * and performs _THIS step (without changing working set). + * \return SUCCESSFUL_RETURN \n + * RET_QP_INFEASIBLE \n + */ +returnValue QProblemB_performStep( QProblemB* _THIS, + const real_t* const delta_g, /**< Step direction of gradient. */ + const real_t* const delta_lb, /**< Step direction of lower bounds. */ + const real_t* const delta_ub, /**< Step direction of upper bounds. */ + const real_t* const delta_xFX, /**< Primal homotopy step direction of fixed variables. */ + const real_t* const delta_xFR, /**< Primal homotopy step direction of free variables. */ + const real_t* const delta_yFX, /**< Dual homotopy step direction of fixed variables' multiplier. */ + int* BC_idx, /**< Output: Index of blocking constraint. */ + SubjectToStatus* BC_status /**< Output: Status of blocking constraint. */ + ); + +/** Updates active set. + * \return SUCCESSFUL_RETURN \n + RET_REMOVE_FROM_ACTIVESET_FAILED \n + RET_ADD_TO_ACTIVESET_FAILED */ +returnValue QProblemB_changeActiveSet( QProblemB* _THIS, + int BC_idx, /**< Index of blocking constraint. */ + SubjectToStatus BC_status /**< Status of blocking constraint. */ + ); + +/** Drift correction at end of each active set iteration + * \return SUCCESSFUL_RETURN */ +returnValue QProblemB_performDriftCorrection( QProblemB* _THIS ); + +/** Determines if it is more efficient to refactorise the matrices when + * hotstarting or not (i.e. better to update the existing factorisations). + * \return BT_TRUE iff matrices shall be refactorised afresh + */ +BooleanType QProblemB_shallRefactorise( QProblemB* _THIS, + Bounds* const guessedBounds /**< Guessed new working set. */ + ); + + +/** Adds a bound to active set (specialised version for the case where no constraints exist). + * \return SUCCESSFUL_RETURN \n + RET_ADDBOUND_FAILED */ +returnValue QProblemB_addBound( QProblemB* _THIS, + int number, /**< Number of bound to be added to active set. */ + SubjectToStatus B_status, /**< Status of new active bound. */ + BooleanType updateCholesky /**< Flag indicating if Cholesky decomposition shall be updated. */ + ); + +/** Removes a bounds from active set (specialised version for the case where no constraints exist). + * \return SUCCESSFUL_RETURN \n + RET_HESSIAN_NOT_SPD \n + RET_REMOVEBOUND_FAILED */ +returnValue QProblemB_removeBound( QProblemB* _THIS, + int number, /**< Number of bound to be removed from active set. */ + BooleanType updateCholesky /**< Flag indicating if Cholesky decomposition shall be updated. */ + ); + + +/** Prints concise information on the current iteration. + * \return SUCCESSFUL_RETURN \n */ +returnValue QProblemB_printIteration( QProblemB* _THIS, + int iter, /**< Number of current iteration. */ + int BC_idx, /**< Index of blocking bound. */ + SubjectToStatus BC_status, /**< Status of blocking bound. */ + real_t homotopyLength, /**< Current homotopy distance. */ + BooleanType isFirstCall /**< Indicating whether this is the first call for current QP. */ + ); + + + +/* + * g e t B o u n d s + */ +static inline returnValue QProblemB_getBounds( QProblemB* _THIS, Bounds* _bounds ) +{ + int nV = QProblemB_getNV( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + _bounds = _THIS->bounds; + + return SUCCESSFUL_RETURN; +} + + +/* + * g e t N V + */ +static inline int QProblemB_getNV( QProblemB* _THIS ) +{ + return Bounds_getNV( _THIS->bounds ); +} + + +/* + * g e t N F R + */ +static inline int QProblemB_getNFR( QProblemB* _THIS ) +{ + return Bounds_getNFR( _THIS->bounds ); +} + + +/* + * g e t N F X + */ +static inline int QProblemB_getNFX( QProblemB* _THIS ) +{ + return Bounds_getNFX( _THIS->bounds ); +} + + +/* + * g e t N F V + */ +static inline int QProblemB_getNFV( QProblemB* _THIS ) +{ + return Bounds_getNFV( _THIS->bounds ); +} + + +/* + * g e t S t a t u s + */ +static inline QProblemStatus QProblemB_getStatus( QProblemB* _THIS ) +{ + return _THIS->status; +} + + +/* + * i s I n i t i a l i s e d + */ +static inline BooleanType QProblemB_isInitialised( QProblemB* _THIS ) +{ + if ( _THIS->status == QPS_NOTINITIALISED ) + return BT_FALSE; + else + return BT_TRUE; +} + + +/* + * i s S o l v e d + */ +static inline BooleanType QProblemB_isSolved( QProblemB* _THIS ) +{ + if ( _THIS->status == QPS_SOLVED ) + return BT_TRUE; + else + return BT_FALSE; +} + + +/* + * i s I n f e a s i b l e + */ +static inline BooleanType QProblemB_isInfeasible( QProblemB* _THIS ) +{ + return _THIS->infeasible; +} + + +/* + * i s U n b o u n d e d + */ +static inline BooleanType QProblemB_isUnbounded( QProblemB* _THIS ) +{ + return _THIS->unbounded; +} + + +/* + * g e t H e s s i a n T y p e + */ +static inline HessianType QProblemB_getHessianType( QProblemB* _THIS ) +{ + return _THIS->hessianType; +} + + +/* + * s e t H e s s i a n T y p e + */ +static inline returnValue QProblemB_setHessianType( QProblemB* _THIS, HessianType _hessianType ) +{ + _THIS->hessianType = _hessianType; + return SUCCESSFUL_RETURN; +} + + +/* + * u s i n g R e g u l a r i s a t i o n + */ +static inline BooleanType QProblemB_usingRegularisation( QProblemB* _THIS ) +{ + if ( _THIS->regVal > QPOASES_ZERO ) + return BT_TRUE; + else + return BT_FALSE; +} + + +/* + * g e t O p t i o n s + */ +static inline Options QProblemB_getOptions( QProblemB* _THIS ) +{ + return _THIS->options; +} + + +/* + * s e t O p t i o n s + */ +static inline returnValue QProblemB_setOptions( QProblemB* _THIS, + Options _options + ) +{ + OptionsCPY( &_options,&(_THIS->options) ); + Options_ensureConsistency( &(_THIS->options) ); + + QProblemB_setPrintLevel( _THIS,_THIS->options.printLevel ); + + return SUCCESSFUL_RETURN; +} + + +/* + * g e t P r i n t L e v e l + */ +static inline PrintLevel QProblemB_getPrintLevel( QProblemB* _THIS ) +{ + return _THIS->options.printLevel; +} + + + +/* + * g e t C o u n t + */ +static inline unsigned int QProblemB_getCount( QProblemB* _THIS ) +{ + return _THIS->count; +} + + +/* + * r e s e t C o u n t e r + */ +static inline returnValue QProblemB_resetCounter( QProblemB* _THIS ) +{ + _THIS->count = 0; + return SUCCESSFUL_RETURN; +} + + + +/***************************************************************************** + * P R O T E C T E D * + *****************************************************************************/ + + +/* + * s e t H + */ +static inline returnValue QProblemB_setHM( QProblemB* _THIS, DenseMatrix* H_new ) +{ + if ( H_new == 0 ) + return QProblemB_setH( _THIS,(real_t*)0 ); + else + return QProblemB_setH( _THIS,DenseMatrix_getVal(H_new) ); +} + + +/* + * s e t H + */ +static inline returnValue QProblemB_setH( QProblemB* _THIS, real_t* const H_new ) +{ + /* if null pointer is passed, Hessian is set to zero matrix + * (or stays identity matrix) */ + if ( H_new == 0 ) + { + if ( _THIS->hessianType == HST_IDENTITY ) + return SUCCESSFUL_RETURN; + + _THIS->hessianType = HST_ZERO; + + _THIS->H = 0; + } + else + { + DenseMatrixCON( _THIS->H,QProblemB_getNV( _THIS ),QProblemB_getNV( _THIS ),QProblemB_getNV( _THIS ),H_new ); + } + + return SUCCESSFUL_RETURN; +} + + +/* + * s e t G + */ +static inline returnValue QProblemB_setG( QProblemB* _THIS, const real_t* const g_new ) +{ + unsigned int nV = (unsigned int)QProblemB_getNV( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + if ( g_new == 0 ) + return THROWERROR( RET_INVALID_ARGUMENTS ); + + memcpy( _THIS->g,g_new,nV*sizeof(real_t) ); + + return SUCCESSFUL_RETURN; +} + + +/* + * s e t L B + */ +static inline returnValue QProblemB_setLB( QProblemB* _THIS, const real_t* const lb_new ) +{ + unsigned int i; + unsigned int nV = (unsigned int)QProblemB_getNV( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + if ( lb_new != 0 ) + { + memcpy( _THIS->lb,lb_new,nV*sizeof(real_t) ); + } + else + { + /* if no lower bounds are specified, set them to -infinity */ + for( i=0; ilb[i] = -QPOASES_INFTY; + } + + return SUCCESSFUL_RETURN; +} + + +/* + * s e t L B + */ +static inline returnValue QProblemB_setLBn( QProblemB* _THIS, int number, real_t value ) +{ + int nV = QProblemB_getNV( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + if ( ( number >= 0 ) && ( number < nV ) ) + { + _THIS->lb[number] = value; + return SUCCESSFUL_RETURN; + } + else + { + return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); + } +} + + +/* + * s e t U B + */ +static inline returnValue QProblemB_setUB( QProblemB* _THIS, const real_t* const ub_new ) +{ + unsigned int i; + unsigned int nV = (unsigned int)QProblemB_getNV( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + if ( ub_new != 0 ) + { + memcpy( _THIS->ub,ub_new,nV*sizeof(real_t) ); + } + else + { + /* if no upper bounds are specified, set them to infinity */ + for( i=0; iub[i] = QPOASES_INFTY; + } + + return SUCCESSFUL_RETURN; +} + + +/* + * s e t U B + */ +static inline returnValue QProblemB_setUBn( QProblemB* _THIS, int number, real_t value ) +{ + int nV = QProblemB_getNV( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + if ( ( number >= 0 ) && ( number < nV ) ) + { + _THIS->ub[number] = value; + + return SUCCESSFUL_RETURN; + } + else + { + return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); + } +} + + +/* + * c o m p u t e G i v e n s + */ +static inline void QProblemB_computeGivens( real_t xold, real_t yold, + real_t* xnew, real_t* ynew, real_t* c, real_t* s + ) +{ + real_t t, mu; + + if ( fabs( yold ) <= QPOASES_ZERO ) + { + *c = 1.0; + *s = 0.0; + + *xnew = xold; + *ynew = yold; + } + else + { + mu = fabs( xold ); + if ( fabs( yold ) > mu ) + mu = fabs( yold ); + + t = mu * sqrt( (xold/mu)*(xold/mu) + (yold/mu)*(yold/mu) ); + + if ( xold < 0.0 ) + t = -t; + + *c = xold/t; + *s = yold/t; + *xnew = t; + *ynew = 0.0; + } + + return; +} + + +/* + * a p p l y G i v e n s + */ +static inline void QProblemB_applyGivens( real_t c, real_t s, real_t nu, real_t xold, real_t yold, + real_t* xnew, real_t* ynew + ) +{ + #ifdef __USE_THREE_MULTS_GIVENS__ + + /* Givens plane rotation requiring only three multiplications, + * cf. Hammarling, S.: A note on modifications to the givens plane rotation. + * J. Inst. Maths Applics, 13:215-218, 1974. */ + *xnew = xold*c + yold*s; + *ynew = (*xnew+xold)*nu - yold; + + #else + + /* Usual Givens plane rotation requiring four multiplications. */ + *xnew = c*xold + s*yold; + *ynew = -s*xold + c*yold; + + #endif + + return; +} + + +/* + * i s B l o c k i n g + */ +static inline BooleanType QProblemB_isBlocking( QProblemB* _THIS, + real_t num, + real_t den, + real_t epsNum, + real_t epsDen, + real_t* t + ) +{ + if ( ( den >= epsDen ) && ( num >= epsNum ) ) + { + if ( num < (*t)*den ) + return BT_TRUE; + } + + return BT_FALSE; +} + + +END_NAMESPACE_QPOASES + + +#endif /* QPOASES_QPROBLEMB_H */ + + +/* + * end of file + */ diff --git a/phonelibs/acados/include/qpOASES_e/Types.h b/phonelibs/acados/include/qpOASES_e/Types.h new file mode 100644 index 0000000000..1e452097f8 --- /dev/null +++ b/phonelibs/acados/include/qpOASES_e/Types.h @@ -0,0 +1,310 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, + * Christian Kirches et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file include/qpOASES_e/Types.h + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches + * \version 3.1embedded + * \date 2007-2015 + * + * Declaration of all non-built-in types (except for classes). + */ + + +#ifndef QPOASES_TYPES_H +#define QPOASES_TYPES_H + +#ifdef USE_ACADOS_TYPES +#include "acados/utils/types.h" +#endif + +/* If your compiler does not support the snprintf() function, + * uncomment the following line and try to compile again. */ +/* #define __NO_SNPRINTF__ */ + + +/* Uncomment the following line for setting the __DSPACE__ flag. */ +/* #define __DSPACE__ */ + +/* Uncomment the following line for setting the __XPCTARGET__ flag. */ +/* #define __XPCTARGET__ */ + + +/* Uncomment the following line for setting the __NO_FMATH__ flag. */ +/* #define __NO_FMATH__ */ + +/* Uncomment the following line to enable debug information. */ +/* #define __DEBUG__ */ + +/* Uncomment the following line to enable suppress any kind of console output. */ +/* #define __SUPPRESSANYOUTPUT__ */ + + +/** Forces to always include all implicitly fixed bounds and all equality constraints + * into the initial working set when setting up an auxiliary QP. */ +#define __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__ + +/* Uncomment the following line to activate the use of an alternative Givens + * plane rotation requiring only three multiplications. */ +/* #define __USE_THREE_MULTS_GIVENS__ */ + +/* Uncomment the following line to activate the use of single precision arithmetic. */ +/* #define __USE_SINGLE_PRECISION__ */ + +/* The inline keyword is skipped by default as it is not part of the C90 standard. + * However, by uncommenting the following line, use of the inline keyword can be enforced. */ +/* #define __USE_INLINE__ */ + + +/* Work-around for Borland BCC 5.5 compiler. */ +#ifdef __BORLANDC__ +#if __BORLANDC__ < 0x0561 + #define __STDC__ 1 +#endif +#endif + + +/* Work-around for Microsoft compilers. */ +#ifdef _MSC_VER + #define __NO_SNPRINTF__ + #pragma warning( disable : 4061 4100 4250 4514 4996 ) +#endif + + +/* Apply pre-processor settings when using qpOASES within auto-generated code. */ +#ifdef __CODE_GENERATION__ + #define __NO_COPYRIGHT__ + #define __EXTERNAL_DIMENSIONS__ +#endif /* __CODE_GENERATION__ */ + + +/* Avoid using static variables declaration within functions. */ +#ifdef __NO_STATIC__ + #define myStatic +#else + #define myStatic static +#endif /* __NO_STATIC__ */ + + +/* Skip inline keyword if not specified otherwise. */ +#ifndef __USE_INLINE__ + #define inline +#endif + + +/* Avoid any printing on embedded platforms. */ +#if defined(__DSPACE__) || defined(__XPCTARGET__) + #define __SUPPRESSANYOUTPUT__ + #define __NO_SNPRINTF__ +#endif + + +#ifdef __NO_SNPRINTF__ + #if (!defined(_MSC_VER)) || defined(__DSPACE__) || defined(__XPCTARGET__) + /* If snprintf is not available, provide an empty implementation... */ + int snprintf( char* s, size_t n, const char* format, ... ); + #else + /* ... or substitute snprintf by _snprintf for Microsoft compilers. */ + #define snprintf _snprintf + #endif +#endif /* __NO_SNPRINTF__ */ + + +/** Macro for switching on/off the beginning of the qpOASES namespace definition. */ +#define BEGIN_NAMESPACE_QPOASES + +/** Macro for switching on/off the end of the qpOASES namespace definition. */ +#define END_NAMESPACE_QPOASES + +/** Macro for switching on/off the use of the qpOASES namespace. */ +#define USING_NAMESPACE_QPOASES + +/** Macro for switching on/off references to the qpOASES namespace. */ +#define REFER_NAMESPACE_QPOASES /*::*/ + + +/** Macro for accessing the Cholesky factor R. */ +#define RR( I,J ) _THIS->R[(I)+nV*(J)] + +/** Macro for accessing the orthonormal matrix Q of the QT factorisation. */ +#define QQ( I,J ) _THIS->Q[(I)+nV*(J)] + +/** Macro for accessing the triangular matrix T of the QT factorisation. */ +#define TT( I,J ) _THIS->T[(I)*nVC_min+(J)] + + + +BEGIN_NAMESPACE_QPOASES + + +/** Defines real_t for facilitating switching between double and float. */ + +#ifndef USE_ACADOS_TYPES +#ifndef __CODE_GENERATION__ + + #ifdef __USE_SINGLE_PRECISION__ + typedef float real_t; + #else + typedef double real_t; + #endif /* __USE_SINGLE_PRECISION__ */ + +#endif /* __CODE_GENERATION__ */ +#endif /* USE_ACADOS_TYPES */ + +/** Summarises all possible logical values. */ +typedef enum +{ + BT_FALSE = 0, /**< Logical value for "false". */ + BT_TRUE /**< Logical value for "true". */ +} BooleanType; + + +/** Summarises all possible print levels. Print levels are used to describe + * the desired amount of output during runtime of qpOASES. */ +typedef enum +{ + PL_DEBUG_ITER = -2, /**< Full tabular debugging output. */ + PL_TABULAR, /**< Tabular output. */ + PL_NONE, /**< No output. */ + PL_LOW, /**< Print error messages only. */ + PL_MEDIUM, /**< Print error and warning messages as well as concise info messages. */ + PL_HIGH /**< Print all messages with full details. */ +} PrintLevel; + + +/** Defines visibility status of a message. */ +typedef enum +{ + VS_HIDDEN, /**< Message not visible. */ + VS_VISIBLE /**< Message visible. */ +} VisibilityStatus; + + +/** Summarises all possible states of the (S)QProblem(B) object during the +solution process of a QP sequence. */ +typedef enum +{ + QPS_NOTINITIALISED, /**< QProblem object is freshly instantiated or reset. */ + QPS_PREPARINGAUXILIARYQP, /**< An auxiliary problem is currently setup, either at the very beginning + * via an initial homotopy or after changing the QP matrices. */ + QPS_AUXILIARYQPSOLVED, /**< An auxilary problem was solved, either at the very beginning + * via an initial homotopy or after changing the QP matrices. */ + QPS_PERFORMINGHOMOTOPY, /**< A homotopy according to the main idea of the online active + * set strategy is performed. */ + QPS_HOMOTOPYQPSOLVED, /**< An intermediate QP along the homotopy path was solved. */ + QPS_SOLVED /**< The solution of the actual QP was found. */ +} QProblemStatus; + + +/** Summarises all possible types of the QP's Hessian matrix. */ +typedef enum +{ + HST_ZERO, /**< Hessian is zero matrix (i.e. LP formulation). */ + HST_IDENTITY, /**< Hessian is identity matrix. */ + HST_POSDEF, /**< Hessian is (strictly) positive definite. */ + HST_POSDEF_NULLSPACE, /**< Hessian is positive definite on null space of active bounds/constraints. */ + HST_SEMIDEF, /**< Hessian is positive semi-definite. */ + HST_INDEF, /**< Hessian is indefinite. */ + HST_UNKNOWN /**< Hessian type is unknown. */ +} HessianType; + + +/** Summarises all possible types of bounds and constraints. */ +typedef enum +{ + ST_UNBOUNDED, /**< Bound/constraint is unbounded. */ + ST_BOUNDED, /**< Bound/constraint is bounded but not fixed. */ + ST_EQUALITY, /**< Bound/constraint is fixed (implicit equality bound/constraint). */ + ST_DISABLED, /**< Bound/constraint is disabled (i.e. ignored when solving QP). */ + ST_UNKNOWN /**< Type of bound/constraint unknown. */ +} SubjectToType; + + +/** Summarises all possible states of bounds and constraints. */ +typedef enum +{ + ST_LOWER = -1, /**< Bound/constraint is at its lower bound. */ + ST_INACTIVE, /**< Bound/constraint is inactive. */ + ST_UPPER, /**< Bound/constraint is at its upper bound. */ + ST_INFEASIBLE_LOWER, /**< (to be documented) */ + ST_INFEASIBLE_UPPER, /**< (to be documented) */ + ST_UNDEFINED /**< Status of bound/constraint undefined. */ +} SubjectToStatus; + + +/** + * \brief Stores internal information for tabular (debugging) output. + * + * Struct storing internal information for tabular (debugging) output + * when using the (S)QProblem(B) objects. + * + * \author Hans Joachim Ferreau + * \version 3.1embedded + * \date 2013-2015 + */ +typedef struct +{ + int idxAddB; /**< Index of bound that has been added to working set. */ + int idxRemB; /**< Index of bound that has been removed from working set. */ + int idxAddC; /**< Index of constraint that has been added to working set. */ + int idxRemC; /**< Index of constraint that has been removed from working set. */ + int excAddB; /**< Flag indicating whether a bound has been added to working set to keep a regular projected Hessian. */ + int excRemB; /**< Flag indicating whether a bound has been removed from working set to keep a regular projected Hessian. */ + int excAddC; /**< Flag indicating whether a constraint has been added to working set to keep a regular projected Hessian. */ + int excRemC; /**< Flag indicating whether a constraint has been removed from working set to keep a regular projected Hessian. */ +} TabularOutput; + +/** + * \brief Struct containing the variable header for mat file. + * + * Struct storing the header of a variable to be stored in + * Matlab's binary format (using the outdated Level 4 variant + * for simplictiy). + * + * Note, this code snippet has been inspired from the document + * "Matlab(R) MAT-file Format, R2013b" by MathWorks + * + * \author Hans Joachim Ferreau + * \version 3.1embedded + * \date 2013-2015 + */ +typedef struct +{ + long numericFormat; /**< Flag indicating numerical format. */ + long nRows; /**< Number of rows. */ + long nCols; /**< Number of rows. */ + long imaginaryPart; /**< (to be documented) */ + long nCharName; /**< Number of character in name. */ +} MatMatrixHeader; + + +END_NAMESPACE_QPOASES + + +#endif /* QPOASES_TYPES_H */ + + +/* + * end of file + */ diff --git a/phonelibs/acados/include/qpOASES_e/UnitTesting.h b/phonelibs/acados/include/qpOASES_e/UnitTesting.h new file mode 100644 index 0000000000..3fb31129a5 --- /dev/null +++ b/phonelibs/acados/include/qpOASES_e/UnitTesting.h @@ -0,0 +1,79 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, + * Christian Kirches et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file include/qpOASES_e/UnitTesting.h + * \author Hans Joachim Ferreau + * \version 3.1embedded + * \date 2014-2015 + * + * Definition of auxiliary functions/macros for unit testing. + */ + + +#ifndef QPOASES_UNIT_TESTING_H +#define QPOASES_UNIT_TESTING_H + + +#ifndef TEST_TOL_FACTOR +#define TEST_TOL_FACTOR 1 +#endif + + +/** Return value for tests that passed. */ +#define TEST_PASSED 0 + +/** Return value for tests that failed. */ +#define TEST_FAILED 1 + +/** Return value for tests that could not run due to missing external data. */ +#define TEST_DATA_NOT_FOUND 99 + + +/** Macro verifying that two numerical values are equal in order to pass unit test. */ +#define QPOASES_TEST_FOR_EQUAL( x,y ) if ( REFER_NAMESPACE_QPOASES isEqual( (x),(y) ) == BT_FALSE ) { return TEST_FAILED; } + +/** Macro verifying that two numerical values are close to each other in order to pass unit test. */ +#define QPOASES_TEST_FOR_NEAR( x,y ) if ( REFER_NAMESPACE_QPOASES getAbs((x)-(y)) / REFER_NAMESPACE_QPOASES getMax( 1.0,REFER_NAMESPACE_QPOASES getAbs(x) ) >= 1e-10 ) { return TEST_FAILED; } + +/** Macro verifying that first quantity is lower or equal than second one in order to pass unit test. */ +#define QPOASES_TEST_FOR_TOL( x,tol ) if ( (x) > (tol)*(TEST_TOL_FACTOR) ) { return TEST_FAILED; } + +/** Macro verifying that a logical expression holds in order to pass unit test. */ +#define QPOASES_TEST_FOR_TRUE( x ) if ( (x) == 0 ) { return TEST_FAILED; } + + + +BEGIN_NAMESPACE_QPOASES + + +END_NAMESPACE_QPOASES + + +#endif /* QPOASES_UNIT_TESTING_H */ + + +/* + * end of file + */ diff --git a/phonelibs/acados/include/qpOASES_e/Utils.h b/phonelibs/acados/include/qpOASES_e/Utils.h new file mode 100644 index 0000000000..75e45a56a0 --- /dev/null +++ b/phonelibs/acados/include/qpOASES_e/Utils.h @@ -0,0 +1,500 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, + * Christian Kirches et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file include/qpOASES_e/Utils.h + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches + * \version 3.1embedded + * \date 2007-2015 + * + * Declaration of some utilities for working with the different QProblem classes. + */ + + +#ifndef QPOASES_UTILS_H +#define QPOASES_UTILS_H + +#include + + +BEGIN_NAMESPACE_QPOASES + + +/** Prints a vector. + * \return SUCCESSFUL_RETURN */ +returnValue qpOASES_printV( const real_t* const v, /**< Vector to be printed. */ + int n /**< Length of vector. */ + ); + +/** Prints a permuted vector. + * \return SUCCESSFUL_RETURN */ +returnValue qpOASES_printPV( const real_t* const v, /**< Vector to be printed. */ + int n, /**< Length of vector. */ + const int* const V_idx /**< Pemutation vector. */ + ); + +/** Prints a named vector. + * \return SUCCESSFUL_RETURN */ +returnValue qpOASES_printNV( const real_t* const v, /**< Vector to be printed. */ + int n, /**< Length of vector. */ + const char* name /** Name of vector. */ + ); + +/** Prints a matrix. + * \return SUCCESSFUL_RETURN */ +returnValue qpOASES_printM( const real_t* const M, /**< Matrix to be printed. */ + int nrow, /**< Row number of matrix. */ + int ncol /**< Column number of matrix. */ + ); + +/** Prints a permuted matrix. + * \return SUCCESSFUL_RETURN */ +returnValue qpOASES_printPM( const real_t* const M, /**< Matrix to be printed. */ + int nrow, /**< Row number of matrix. */ + int ncol , /**< Column number of matrix. */ + const int* const ROW_idx, /**< Row pemutation vector. */ + const int* const COL_idx /**< Column pemutation vector. */ + ); + +/** Prints a named matrix. + * \return SUCCESSFUL_RETURN */ +returnValue qpOASES_printNM( const real_t* const M, /**< Matrix to be printed. */ + int nrow, /**< Row number of matrix. */ + int ncol, /**< Column number of matrix. */ + const char* name /** Name of matrix. */ + ); + +/** Prints an index array. + * \return SUCCESSFUL_RETURN */ +returnValue qpOASES_printI( const int* const _index, /**< Index array to be printed. */ + int n /**< Length of index array. */ + ); + +/** Prints a named index array. + * \return SUCCESSFUL_RETURN */ +returnValue qpOASES_printNI( const int* const _index, /**< Index array to be printed. */ + int n, /**< Length of index array. */ + const char* name /**< Name of index array. */ + ); + + +/** Prints a string to desired output target (useful also for MATLAB output!). + * \return SUCCESSFUL_RETURN */ +returnValue qpOASES_myPrintf( const char* s /**< String to be written. */ + ); + + +/** Prints qpOASES copyright notice. + * \return SUCCESSFUL_RETURN */ +returnValue qpOASES_printCopyrightNotice( ); + + +/** Reads a real_t matrix from file. + * \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_OPEN_FILE \n + RET_UNABLE_TO_READ_FILE */ +returnValue qpOASES_readFromFileM( real_t* data, /**< Matrix to be read from file. */ + int nrow, /**< Row number of matrix. */ + int ncol, /**< Column number of matrix. */ + const char* datafilename /**< Data file name. */ + ); + +/** Reads a real_t vector from file. + * \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_OPEN_FILE \n + RET_UNABLE_TO_READ_FILE */ +returnValue qpOASES_readFromFileV( real_t* data, /**< Vector to be read from file. */ + int n, /**< Length of vector. */ + const char* datafilename /**< Data file name. */ + ); + +/** Reads an integer (column) vector from file. + * \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_OPEN_FILE \n + RET_UNABLE_TO_READ_FILE */ +returnValue qpOASES_readFromFileI( int* data, /**< Vector to be read from file. */ + int n, /**< Length of vector. */ + const char* datafilename /**< Data file name. */ + ); + + +/** Writes a real_t matrix into a file. + * \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_OPEN_FILE */ +returnValue qpOASES_writeIntoFileM( const real_t* const data, /**< Matrix to be written into file. */ + int nrow, /**< Row number of matrix. */ + int ncol, /**< Column number of matrix. */ + const char* datafilename, /**< Data file name. */ + BooleanType append /**< Indicates if data shall be appended if the file already exists (otherwise it is overwritten). */ + ); + +/** Writes a real_t vector into a file. + * \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_OPEN_FILE */ +returnValue qpOASES_writeIntoFileV( const real_t* const data, /**< Vector to be written into file. */ + int n, /**< Length of vector. */ + const char* datafilename, /**< Data file name. */ + BooleanType append /**< Indicates if data shall be appended if the file already exists (otherwise it is overwritten). */ + ); + +/** Writes an integer (column) vector into a file. + * \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_OPEN_FILE */ +returnValue qpOASES_writeIntoFileI( const int* const integer, /**< Integer vector to be written into file. */ + int n, /**< Length of vector. */ + const char* datafilename, /**< Data file name. */ + BooleanType append /**< Indicates if integer shall be appended if the file already exists (otherwise it is overwritten). */ + ); + +/** Writes a real_t matrix/vector into a Matlab binary file. + * \return SUCCESSFUL_RETURN \n + RET_INVALID_ARGUMENTS + RET_UNABLE_TO_WRITE_FILE */ +returnValue qpOASES_writeIntoMatFile( FILE* const matFile, /**< Pointer to Matlab binary file. */ + const real_t* const data, /**< Data to be written into file. */ + int nRows, /**< Row number of matrix. */ + int nCols, /**< Column number of matrix. */ + const char* name /**< Matlab name of matrix/vector to be stored. */ + ); + +/** Writes in integer matrix/vector into a Matlab binary file. + * \return SUCCESSFUL_RETURN \n + RET_INVALID_ARGUMENTS + RET_UNABLE_TO_WRITE_FILE */ +returnValue qpOASES_writeIntoMatFileI( FILE* const matFile, /**< Pointer to Matlab binary file. */ + const int* const data, /**< Data to be written into file. */ + int nRows, /**< Row number of matrix. */ + int nCols, /**< Column number of matrix. */ + const char* name /**< Matlab name of matrix/vector to be stored. */ + ); + + +/** Returns the current system time. + * \return current system time */ +real_t qpOASES_getCPUtime( ); + + +/** Returns the N-norm of a vector. + * \return >= 0.0: successful */ +real_t qpOASES_getNorm( const real_t* const v, /**< Vector. */ + int n, /**< Vector's dimension. */ + int type /**< Norm type, 1: one-norm, 2: Euclidean norm. */ + ); + +/** Tests whether two real-valued arguments are (numerically) equal. + * \return BT_TRUE: arguments differ not more than TOL \n + BT_FALSE: arguments differ more than TOL */ +static inline BooleanType qpOASES_isEqual( real_t x, /**< First real number. */ + real_t y, /**< Second real number. */ + real_t TOL /**< Tolerance for comparison. */ + ); + + +/** Tests whether a real-valued argument is (numerically) zero. + * \return BT_TRUE: argument differs from 0.0 not more than TOL \n + BT_FALSE: argument differs from 0.0 more than TOL */ +static inline BooleanType qpOASES_isZero( real_t x, /**< Real number. */ + real_t TOL /**< Tolerance for comparison. */ + ); + + +/** Returns sign of a real-valued argument. + * \return 1.0: argument is non-negative \n + -1.0: argument is negative */ +static inline real_t qpOASES_getSign( real_t arg /**< real-valued argument whose sign is to be determined. */ + ); + + +/** Returns maximum of two integers. + * \return Maximum of two integers */ +static inline int qpOASES_getMaxI( int x, /**< First integer. */ + int y /**< Second integer. */ + ); + + +/** Returns minimum of two integers. + * \return Minimum of two integers */ +static inline int qpOASES_getMinI( int x, /**< First integer. */ + int y /**< Second integer. */ + ); + + +/** Returns maximum of two reals. + * \return Maximum of two reals */ +static inline real_t qpOASES_getMax( real_t x, /**< First real number. */ + real_t y /**< Second real number. */ + ); + + +/** Returns minimum of two reals. + * \return Minimum of two reals */ +static inline real_t qpOASES_getMin( real_t x, /**< First real number. */ + real_t y /**< Second real number. */ + ); + + +/** Returns the absolute value of a real_t-valued argument. + * \return Absolute value of a real_t-valued argument */ +static inline real_t qpOASES_getAbs( real_t x /**< real_t-valued argument. */ + ); + +/** Returns the square-root of a real number. + * \return Square-root of a real number */ +static inline real_t qpOASES_getSqrt( real_t x /**< Non-negative real number. */ + ); + + +/** Computes the maximum violation of the KKT optimality conditions + * of given iterate for given QP data. */ +returnValue qpOASES_getKktViolation( int nV, /**< Number of variables. */ + int nC, /**< Number of constraints. */ + const real_t* const H, /**< Hessian matrix (may be NULL if Hessian is zero or identity matrix). */ + const real_t* const g, /**< Gradient vector. */ + const real_t* const A, /**< Constraint matrix. */ + const real_t* const lb, /**< Lower bound vector (on variables). */ + const real_t* const ub, /**< Upper bound vector (on variables). */ + const real_t* const lbA, /**< Lower constraints' bound vector. */ + const real_t* const ubA, /**< Upper constraints' bound vector. */ + const real_t* const x, /**< Primal trial vector. */ + const real_t* const y, /**< Dual trial vector. */ + real_t* const _stat, /**< Output: maximum value of stationarity condition residual. */ + real_t* const feas, /**< Output: maximum value of primal feasibility violation. */ + real_t* const cmpl /**< Output: maximum value of complementarity residual. */ + ); + +/** Computes the maximum violation of the KKT optimality conditions + * of given iterate for given QP data. */ +returnValue qpOASES_getKktViolationSB( int nV, /**< Number of variables. */ + const real_t* const H, /**< Hessian matrix (may be NULL if Hessian is zero or identity matrix). */ + const real_t* const g, /**< Gradient vector. */ + const real_t* const lb, /**< Lower bound vector (on variables). */ + const real_t* const ub, /**< Upper bound vector (on variables). */ + const real_t* const x, /**< Primal trial vector. */ + const real_t* const y, /**< Dual trial vector. */ + real_t* const _stat, /**< Output: maximum value of stationarity condition residual. */ + real_t* const feas, /**< Output: maximum value of primal feasibility violation. */ + real_t* const cmpl /**< Output: maximum value of complementarity residual. */ + ); + + +/** Writes a value of BooleanType into a string. + * \return SUCCESSFUL_RETURN */ +returnValue qpOASES_convertBooleanTypeToString( BooleanType value, /**< Value to be written. */ + char* const string /**< Input: String of sufficient size, \n + Output: String containing value. */ + ); + +/** Writes a value of SubjectToStatus into a string. + * \return SUCCESSFUL_RETURN */ +returnValue qpOASES_convertSubjectToStatusToString( SubjectToStatus value, /**< Value to be written. */ + char* const string /**< Input: String of sufficient size, \n + Output: String containing value. */ + ); + +/** Writes a value of PrintLevel into a string. + * \return SUCCESSFUL_RETURN */ +returnValue qpOASES_convertPrintLevelToString( PrintLevel value, /**< Value to be written. */ + char* const string /**< Input: String of sufficient size, \n + Output: String containing value. */ + ); + + +/** Converts a returnValue from an QProblem(B) object into a more + * simple status flag. + * + * \return 0: QP problem solved + * 1: QP could not be solved within given number of iterations + * -1: QP could not be solved due to an internal error + * -2: QP is infeasible (and thus could not be solved) + * -3: QP is unbounded (and thus could not be solved) + */ +int qpOASES_getSimpleStatus( returnValue returnvalue, /**< ReturnValue to be analysed. */ + BooleanType doPrintStatus /**< Flag indicating whether simple status shall be printed to screen. */ + ); + +/** Normalises QP constraints. + * \return SUCCESSFUL_RETURN \n + * RET_INVALID_ARGUMENTS */ +returnValue qpOASES_normaliseConstraints( int nV, /**< Number of variables. */ + int nC, /**< Number of constraints. */ + real_t* A, /**< Input: Constraint matrix, \n + Output: Normalised constraint matrix. */ + real_t* lbA, /**< Input: Constraints' lower bound vector, \n + Output: Normalised constraints' lower bound vector. */ + real_t* ubA, /**< Input: Constraints' upper bound vector, \n + Output: Normalised constraints' upper bound vector. */ + int type /**< Norm type, 1: one-norm, 2: Euclidean norm. */ + ); + + +#ifdef __DEBUG__ +/** Writes matrix with given dimension into specified file. */ +void gdb_printmat( const char *fname, /**< File name. */ + real_t *M, /**< Matrix to be written. */ + int n, /**< Number of rows. */ + int m, /**< Number of columns. */ + int ldim /**< Leading dimension. */ + ); +#endif /* __DEBUG__ */ + + +#if defined(__DSPACE__) || defined(__XPCTARGET__) +void __cxa_pure_virtual( void ); +#endif /* __DSPACE__ || __XPCTARGET__*/ + + + +/* + * i s E q u a l + */ +static inline BooleanType qpOASES_isEqual( real_t x, + real_t y, + real_t TOL + ) +{ + if ( qpOASES_getAbs(x-y) <= TOL ) + return BT_TRUE; + else + return BT_FALSE; +} + + +/* + * i s Z e r o + */ +static inline BooleanType qpOASES_isZero( real_t x, + real_t TOL + ) +{ + if ( qpOASES_getAbs(x) <= TOL ) + return BT_TRUE; + else + return BT_FALSE; +} + + +/* + * g e t S i g n + */ +static inline real_t qpOASES_getSign( real_t arg + ) +{ + if ( arg >= 0.0 ) + return 1.0; + else + return -1.0; +} + + + +/* + * g e t M a x + */ +static inline int qpOASES_getMaxI( int x, + int y + ) +{ + return (yx) ? x : y; +} + + +/* + * g e t M a x + */ +static inline real_t qpOASES_getMax( real_t x, + real_t y + ) +{ + #ifdef __NO_FMATH__ + return (yx) ? x : y; + #else + return (y>x) ? x : y; + /*return fmin(x,y); seems to be slower */ + #endif +} + + +/* + * g e t A b s + */ +static inline real_t qpOASES_getAbs( real_t x + ) +{ + #ifdef __NO_FMATH__ + return (x>=0.0) ? x : -x; + #else + return fabs(x); + #endif +} + +/* + * g e t S q r t + */ +static inline real_t qpOASES_getSqrt( real_t x + ) +{ + #ifdef __NO_FMATH__ + return sqrt(x); /* put your custom sqrt-replacement here */ + #else + return sqrt(x); + #endif +} + + +END_NAMESPACE_QPOASES + + +#endif /* QPOASES_UTILS_H */ + + +/* + * end of file + */ diff --git a/phonelibs/acados/include/qpOASES_e/extras/OQPinterface.h b/phonelibs/acados/include/qpOASES_e/extras/OQPinterface.h new file mode 100644 index 0000000000..da59ea9db6 --- /dev/null +++ b/phonelibs/acados/include/qpOASES_e/extras/OQPinterface.h @@ -0,0 +1,227 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, + * Christian Kirches et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file include/qpOASES_e/extras/OQPinterface.h + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches + * \version 3.1embedded + * \date 2007-2015 + * + * Declaration of an interface comprising several utility functions + * for solving test problems from the Online QP Benchmark Collection + * (This collection is no longer maintained, see + * http://www.qpOASES.org/onlineQP for a backup). + */ + + +#ifndef QPOASES_OQPINTERFACE_H +#define QPOASES_OQPINTERFACE_H + + +#include +#include + +#include +#include + + +BEGIN_NAMESPACE_QPOASES + +typedef struct { + QProblem *qp; + + DenseMatrix *H; + DenseMatrix *A; + + real_t *x; + real_t *y; +} OQPbenchmark_ws; + +int OQPbenchmark_ws_calculateMemorySize( unsigned int nV, unsigned int nC ); + +char *OQPbenchmark_ws_assignMemory( unsigned int nV, unsigned int nC, OQPbenchmark_ws **mem, void *raw_memory ); + +OQPbenchmark_ws *OQPbenchmark_ws_createMemory( unsigned int nV, unsigned int nC ); + +typedef struct { + QProblemB *qp; + + DenseMatrix *H; + + real_t *x; + real_t *y; +} OQPbenchmarkB_ws; + +int OQPbenchmarkB_ws_calculateMemorySize( unsigned int nV ); + +char *OQPbenchmarkB_ws_assignMemory( unsigned int nV, OQPbenchmarkB_ws **mem, void *raw_memory ); + +OQPbenchmarkB_ws *OQPbenchmarkB_ws_createMemory( unsigned int nV ); + +typedef struct { + OQPbenchmark_ws *qp_ws; + OQPbenchmarkB_ws *qpB_ws; + + real_t *H; + real_t *g; + real_t *A; + real_t *lb; + real_t *ub; + real_t *lbA; + real_t *ubA; +} OQPinterface_ws; + +int OQPinterface_ws_calculateMemorySize( unsigned int nV, unsigned int nC, unsigned int nQP ); + +char *OQPinterface_ws_assignMemory( unsigned int nV, unsigned int nC, unsigned int nQP, OQPinterface_ws **mem, void *raw_memory ); + +OQPinterface_ws *OQPinterface_ws_createMemory( unsigned int nV, unsigned int nC, unsigned int nQP ); + +/** Reads dimensions of an Online QP Benchmark problem from file. + * \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_READ_FILE \n + RET_FILEDATA_INCONSISTENT */ +returnValue readOQPdimensions( const char* path, /**< Full path of the data files (without trailing slash!). */ + int* nQP, /**< Output: Number of QPs. */ + int* nV, /**< Output: Number of variables. */ + int* nC, /**< Output: Number of constraints. */ + int* nEC /**< Output: Number of equality constraints. */ + ); + +/** Reads data of an Online QP Benchmark problem from file. + * This function allocates the required memory for all data; after successfully calling it, + * you have to free this memory yourself! + * \return SUCCESSFUL_RETURN \n + RET_INVALID_ARGUMENTS \n + RET_UNABLE_TO_READ_FILE \n + RET_FILEDATA_INCONSISTENT */ +returnValue readOQPdata( const char* path, /**< Full path of the data files (without trailing slash!). */ + int* nQP, /**< Output: Number of QPs. */ + int* nV, /**< Output: Number of variables. */ + int* nC, /**< Output: Number of constraints. */ + int* nEC, /**< Output: Number of equality constraints. */ + real_t* H, /**< Output: Hessian matrix. */ + real_t* g, /**< Output: Sequence of gradient vectors. */ + real_t* A, /**< Output: Constraint matrix. */ + real_t* lb, /**< Output: Sequence of lower bound vectors (on variables). */ + real_t* ub, /**< Output: Sequence of upper bound vectors (on variables). */ + real_t* lbA, /**< Output: Sequence of lower constraints' bound vectors. */ + real_t* ubA, /**< Output: Sequence of upper constraints' bound vectors. */ + real_t* xOpt, /**< Output: Sequence of primal solution vectors + * (not read if a null pointer is passed). */ + real_t* yOpt, /**< Output: Sequence of dual solution vectors + * (not read if a null pointer is passed). */ + real_t* objOpt /**< Output: Sequence of optimal objective function values + * (not read if a null pointer is passed). */ + ); + + +/** Solves an Online QP Benchmark problem as specified by the arguments. + * The maximum deviations from the given optimal solution as well as the + * maximum CPU time to solve each QP are determined. + * \return SUCCESSFUL_RETURN \n + RET_BENCHMARK_ABORTED */ +returnValue solveOQPbenchmark( int nQP, /**< Number of QPs. */ + int nV, /**< Number of variables. */ + int nC, /**< Number of constraints. */ + int nEC, /**< Number of equality constraints. */ + real_t* _H, /**< Hessian matrix. */ + const real_t* const g, /**< Sequence of gradient vectors. */ + real_t* _A, /**< Constraint matrix. */ + const real_t* const lb, /**< Sequence of lower bound vectors (on variables). */ + const real_t* const ub, /**< Sequence of upper bound vectors (on variables). */ + const real_t* const lbA, /**< Sequence of lower constraints' bound vectors. */ + const real_t* const ubA, /**< Sequence of upper constraints' bound vectors. */ + BooleanType isSparse, /**< Shall convert matrices to sparse format before solution? */ + BooleanType useHotstarts, /**< Shall QP solution be hotstarted? */ + const Options* options, /**< QP solver options to be used while solving benchmark problems. */ + int maxAllowedNWSR, /**< Maximum number of working set recalculations to be performed. */ + real_t* maxNWSR, /**< Output: Maximum number of performed working set recalculations. */ + real_t* avgNWSR, /**< Output: Average number of performed working set recalculations. */ + real_t* maxCPUtime, /**< Output: Maximum CPU time required for solving each QP. */ + real_t* avgCPUtime, /**< Output: Average CPU time required for solving each QP. */ + real_t* maxStationarity, /**< Output: Maximum residual of stationarity condition. */ + real_t* maxFeasibility, /**< Output: Maximum residual of primal feasibility condition. */ + real_t* maxComplementarity, /**< Output: Maximum residual of complementarity condition. */ + OQPbenchmark_ws *work /**< Workspace. */ + ); + +/** Solves an Online QP Benchmark problem (without constraints) as specified + * by the arguments. The maximum deviations from the given optimal solution + * as well as the maximum CPU time to solve each QP are determined. + * \return SUCCESSFUL_RETURN \n + RET_BENCHMARK_ABORTED */ +returnValue solveOQPbenchmarkB( int nQP, /**< Number of QPs. */ + int nV, /**< Number of variables. */ + real_t* _H, /**< Hessian matrix. */ + const real_t* const g, /**< Sequence of gradient vectors. */ + const real_t* const lb, /**< Sequence of lower bound vectors (on variables). */ + const real_t* const ub, /**< Sequence of upper bound vectors (on variables). */ + BooleanType isSparse, /**< Shall convert matrices to sparse format before solution? */ + BooleanType useHotstarts, /**< Shall QP solution be hotstarted? */ + const Options* options, /**< QP solver options to be used while solving benchmark problems. */ + int maxAllowedNWSR, /**< Maximum number of working set recalculations to be performed. */ + real_t* maxNWSR, /**< Output: Maximum number of performed working set recalculations. */ + real_t* avgNWSR, /**< Output: Average number of performed working set recalculations. */ + real_t* maxCPUtime, /**< Output: Maximum CPU time required for solving each QP. */ + real_t* avgCPUtime, /**< Output: Average CPU time required for solving each QP. */ + real_t* maxStationarity, /**< Output: Maximum residual of stationarity condition. */ + real_t* maxFeasibility, /**< Output: Maximum residual of primal feasibility condition. */ + real_t* maxComplementarity, /**< Output: Maximum residual of complementarity condition. */ + OQPbenchmarkB_ws *work /**< Workspace. */ + ); + + +/** Runs an Online QP Benchmark problem and determines the maximum + * violation of the KKT optimality conditions as well as the + * maximum and average number of iterations and CPU time to solve + * each QP. + * + * \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_READ_BENCHMARK \n + RET_BENCHMARK_ABORTED */ +returnValue runOQPbenchmark( const char* path, /**< Full path of the benchmark files (without trailing slash!). */ + BooleanType isSparse, /**< Shall convert matrices to sparse format before solution? */ + BooleanType useHotstarts, /**< Shall QP solution be hotstarted? */ + const Options* options, /**< QP solver options to be used while solving benchmark problems. */ + int maxAllowedNWSR, /**< Maximum number of working set recalculations to be performed. */ + real_t* maxNWSR, /**< Output: Maximum number of performed working set recalculations. */ + real_t* avgNWSR, /**< Output: Average number of performed working set recalculations. */ + real_t* maxCPUtime, /**< Output: Maximum CPU time required for solving each QP. */ + real_t* avgCPUtime, /**< Output: Average CPU time required for solving each QP. */ + real_t* maxStationarity, /**< Output: Maximum residual of stationarity condition. */ + real_t* maxFeasibility, /**< Output: Maximum residual of primal feasibility condition. */ + real_t* maxComplementarity, /**< Output: Maximum residual of complementarity condition. */ + OQPinterface_ws* work /**< Workspace. */ + ); + +END_NAMESPACE_QPOASES + + +#endif /* QPOASES_OQPINTERFACE_H */ + + +/* + * end of file + */ diff --git a/phonelibs/acados/x86_64/lib/libacados.so b/phonelibs/acados/x86_64/lib/libacados.so new file mode 100644 index 0000000000..30d3b71ef1 Binary files /dev/null and b/phonelibs/acados/x86_64/lib/libacados.so differ diff --git a/phonelibs/acados/x86_64/lib/libblasfeo.so b/phonelibs/acados/x86_64/lib/libblasfeo.so new file mode 100644 index 0000000000..5eb0e988c9 Binary files /dev/null and b/phonelibs/acados/x86_64/lib/libblasfeo.so differ diff --git a/phonelibs/acados/x86_64/lib/libhpipm.so b/phonelibs/acados/x86_64/lib/libhpipm.so new file mode 100644 index 0000000000..9a79bfbd31 Binary files /dev/null and b/phonelibs/acados/x86_64/lib/libhpipm.so differ diff --git a/phonelibs/acados/x86_64/lib/libqpOASES_e.so b/phonelibs/acados/x86_64/lib/libqpOASES_e.so new file mode 120000 index 0000000000..14048625a1 --- /dev/null +++ b/phonelibs/acados/x86_64/lib/libqpOASES_e.so @@ -0,0 +1 @@ +libqpOASES_e.so.3.1 \ No newline at end of file diff --git a/phonelibs/acados/x86_64/lib/libqpOASES_e.so.3.1 b/phonelibs/acados/x86_64/lib/libqpOASES_e.so.3.1 new file mode 100644 index 0000000000..93967ae04a Binary files /dev/null and b/phonelibs/acados/x86_64/lib/libqpOASES_e.so.3.1 differ diff --git a/pyextra/acados_template/.gitignore b/pyextra/acados_template/.gitignore new file mode 100644 index 0000000000..c18dd8d83c --- /dev/null +++ b/pyextra/acados_template/.gitignore @@ -0,0 +1 @@ +__pycache__/ diff --git a/pyextra/acados_template/__init__.py b/pyextra/acados_template/__init__.py new file mode 100644 index 0000000000..f33b75bb7b --- /dev/null +++ b/pyextra/acados_template/__init__.py @@ -0,0 +1,43 @@ +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + +from .acados_model import * +from .generate_c_code_explicit_ode import * +from .generate_c_code_implicit_ode import * +from .generate_c_code_constraint import * +from .generate_c_code_nls_cost import * +from .acados_ocp import * +from .acados_sim import * +from .acados_ocp_solver import * +from .acados_sim_solver import * +from .utils import * diff --git a/pyextra/acados_template/acados_layout.json b/pyextra/acados_template/acados_layout.json new file mode 100644 index 0000000000..9458afb10c --- /dev/null +++ b/pyextra/acados_template/acados_layout.json @@ -0,0 +1,772 @@ +{ + "code_export_directory": [ + "str" + ], + "acados_include_path": [ + "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" + ] + }, + "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" + ], + "regularize_method": [ + "str" + ], + "integrator_type": [ + "str" + ], + "nlp_solver_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" + ], + "sim_method_num_stages": [ + "ndarray", + [ + "N" + ] + ], + "sim_method_num_steps": [ + "ndarray", + [ + "N" + ] + ], + "sim_method_newton_iter": [ + "int" + ], + "sim_method_jac_reuse": [ + "bool" + ], + "qp_solver_cond_N": [ + "int" + ], + "qp_solver_warm_start": [ + "int" + ], + "qp_solver_tol_stat": [ + "float" + ], + "qp_solver_tol_eq": [ + "float" + ], + "qp_solver_tol_ineq": [ + "float" + ], + "qp_solver_tol_comp": [ + "float" + ], + "qp_solver_iter_max": [ + "int" + ], + "nlp_solver_tol_stat": [ + "float" + ], + "nlp_solver_tol_eq": [ + "float" + ], + "nlp_solver_tol_ineq": [ + "float" + ], + "nlp_solver_tol_comp": [ + "float" + ], + "nlp_solver_max_iter": [ + "int" + ], + "print_level": [ + "int" + ], + "initialize_t_slacks": [ + "int" + ], + "exact_hess_cost": [ + "int" + ], + "exact_hess_constr": [ + "int" + ], + "exact_hess_dyn": [ + "int" + ], + "ext_cost_num_hess": [ + "int" + ], + "model_external_shared_lib_dir": [ + "str" + ], + "model_external_shared_lib_name": [ + "str" + ] + } +} diff --git a/pyextra/acados_template/acados_model.py b/pyextra/acados_template/acados_model.py new file mode 100644 index 0000000000..379ade0d95 --- /dev/null +++ b/pyextra/acados_template/acados_model.py @@ -0,0 +1,151 @@ +# +# 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 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` + + +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'] + + return out diff --git a/pyextra/acados_template/acados_ocp.py b/pyextra/acados_template/acados_ocp.py new file mode 100644 index 0000000000..eec9b7e09f --- /dev/null +++ b/pyextra/acados_template/acados_ocp.py @@ -0,0 +1,2822 @@ +# -*- coding: future_fstrings -*- +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + +import numpy as np +import os +from .acados_model import AcadosModel +from .utils import get_acados_path, J_to_idx, J_to_idx_slack + +class AcadosOcpDims: + """ + Class containing the dimensions of the optimal control problem. + """ + def __init__(self): + self.__nx = None + self.__nu = None + self.__nz = 0 + self.__np = 0 + self.__ny = 0 + self.__ny_e = 0 + self.__ny_0 = 0 + self.__nr = 0 + self.__nr_e = 0 + self.__nh = 0 + self.__nh_e = 0 + self.__nphi = 0 + self.__nphi_e = 0 + self.__nbx = 0 + self.__nbx_0 = 0 + self.__nbx_e = 0 + self.__nbu = 0 + self.__nsbx = 0 + self.__nsbx_e = 0 + self.__nsbu = 0 + self.__nsh = 0 + self.__nsh_e = 0 + self.__nsphi = 0 + self.__nsphi_e = 0 + self.__ns = 0 + self.__ns_e = 0 + self.__ng = 0 + self.__ng_e = 0 + self.__nsg = 0 + self.__nsg_e = 0 + self.__nbxe_0 = None + self.__N = None + + + @property + def nx(self): + """:math:`n_x` - number of states. + Type: int; default: None""" + return self.__nx + + @property + def nz(self): + """:math:`n_z` - number of algebraic variables. + Type: int; default: 0""" + return self.__nz + + @property + def nu(self): + """:math:`n_u` - number of inputs. + Type: int; default: None""" + return self.__nu + + @property + def np(self): + """:math:`n_p` - number of parameters. + Type: int; default: 0""" + return self.__np + + @property + def ny(self): + """:math:`n_y` - number of residuals in Lagrange term. + Type: int; default: 0""" + return self.__ny + + @property + def ny_0(self): + """:math:`n_{y}^0` - number of residuals in Mayer term. + Type: int; default: 0""" + return self.__ny_0 + + @property + def ny_e(self): + """:math:`n_{y}^e` - number of residuals in Mayer term. + Type: int; default: 0""" + return self.__ny_e + + @property + def nr(self): + """:math:`n_{\pi}` - dimension of the image of the inner nonlinear function in positive definite constraints. + Type: int; default: 0""" + return self.__nr + + @property + def nr_e(self): + """:math:`n_{\pi}^e` - dimension of the image of the inner nonlinear function in positive definite constraints. + Type: int; default: 0""" + return self.__nr_e + + @property + def nh(self): + """:math:`n_h` - number of nonlinear constraints. + Type: int; default: 0""" + return self.__nh + + @property + def nh_e(self): + """:math:`n_{h}^e` - number of nonlinear constraints at terminal shooting node N. + Type: int; default: 0""" + return self.__nh_e + + @property + def nphi(self): + """:math:`n_{\phi}` - number of convex-over-nonlinear constraints. + Type: int; default: 0""" + return self.__nphi + + @property + def nphi_e(self): + """:math:`n_{\phi}^e` - number of convex-over-nonlinear constraints at terminal shooting node N. + Type: int; default: 0""" + return self.__nphi_e + + @property + def nbx(self): + """:math:`n_{b_x}` - number of state bounds. + Type: int; default: 0""" + return self.__nbx + + @property + def nbxe_0(self): + """:math:`n_{be_{x0}}` - number of state bounds at initial shooting node that are equalities. + Type: int; default: None""" + return self.__nbxe_0 + + @property + def nbx_0(self): + """:math:`n_{b_{x0}}` - number of state bounds for initial state. + Type: int; default: 0""" + return self.__nbx_0 + + @property + def nbx_e(self): + """:math:`n_{b_x}` - number of state bounds at terminal shooting node N. + Type: int; default: 0""" + return self.__nbx_e + + @property + def nbu(self): + """:math:`n_{b_u}` - number of input bounds. + Type: int; default: 0""" + return self.__nbu + + @property + def nsbx(self): + """:math:`n_{{sb}_x}` - number of soft state bounds. + Type: int; default: 0""" + return self.__nsbx + + @property + def nsbx_e(self): + """:math:`n_{{sb}^e_{x}}` - number of soft state bounds at terminal shooting node N. + Type: int; default: 0""" + return self.__nsbx_e + + @property + def nsbu(self): + """:math:`n_{{sb}_u}` - number of soft input bounds. + Type: int; default: 0""" + return self.__nsbu + + @property + def nsg(self): + """:math:`n_{{sg}}` - number of soft general linear constraints. + Type: int; default: 0""" + return self.__nsg + + @property + def nsg_e(self): + """:math:`n_{{sg}^e}` - number of soft general linear constraints at terminal shooting node N. + Type: int; default: 0""" + return self.__nsg_e + + @property + def nsh(self): + """:math:`n_{{sh}}` - number of soft nonlinear constraints. + Type: int; default: 0""" + return self.__nsh + + @property + def nsh_e(self): + """:math:`n_{{sh}}^e` - number of soft nonlinear constraints at terminal shooting node N. + Type: int; default: 0""" + return self.__nsh_e + + @property + def nsphi(self): + """:math:`n_{{s\phi}}` - number of soft convex-over-nonlinear constraints. + Type: int; default: 0""" + return self.__nsphi + + @property + def nsphi_e(self): + """:math:`n_{{s\phi}^e}` - number of soft convex-over-nonlinear constraints at terminal shooting node N. + Type: int; default: 0""" + return self.__nsphi_e + + @property + def ns(self): + """:math:`n_{s}` - total number of slacks. + Type: int; default: 0""" + return self.__ns + + @property + def ns_e(self): + """:math:`n_{s}^e` - total number of slacks at terminal shooting node N. + Type: int; default: 0""" + return self.__ns_e + + @property + def ng(self): + """:math:`n_{g}` - number of general polytopic constraints. + Type: int; default: 0""" + return self.__ng + + @property + def ng_e(self): + """:math:`n_{g}^e` - number of general polytopic constraints at terminal shooting node N. + Type: int; default: 0""" + return self.__ng_e + + @property + def N(self): + """:math:`N` - prediction horizon. + Type: int; default: None""" + return self.__N + + @nx.setter + def nx(self, nx): + if type(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 type(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 type(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 type(np) == int and np > -1: + self.__np = np + else: + raise Exception('Invalid np value, expected nonnegative integer. Exiting.') + + @ny_0.setter + def ny_0(self, ny_0): + if isinstance(ny_0, int) and ny_0 > -1: + self.__ny_0 = ny_0 + else: + raise Exception('Invalid ny_0 value, expected nonnegative integer. Exiting.') + + @ny.setter + def ny(self, ny): + if isinstance(ny, int) and ny > -1: + self.__ny = ny + else: + raise Exception('Invalid ny value, expected nonnegative integer. Exiting.') + + @ny_e.setter + def ny_e(self, ny_e): + if type(ny_e) == int and ny_e > -1: + self.__ny_e = ny_e + else: + raise Exception('Invalid ny_e value, expected nonnegative integer. Exiting.') + + @nr.setter + def nr(self, nr): + if type(nr) == int and nr > -1: + self.__nr = nr + else: + raise Exception('Invalid nr value, expected nonnegative integer. Exiting.') + + @nr_e.setter + def nr_e(self, nr_e): + if type(nr_e) == int and nr_e > -1: + self.__nr_e = nr_e + else: + raise Exception('Invalid nr_e value, expected nonnegative integer. Exiting.') + + @nh.setter + def nh(self, nh): + if type(nh) == int and nh > -1: + self.__nh = nh + else: + raise Exception('Invalid nh value, expected nonnegative integer. Exiting.') + + @nh_e.setter + def nh_e(self, nh_e): + if type(nh_e) == int and nh_e > -1: + self.__nh_e = nh_e + else: + raise Exception('Invalid nh_e value, expected nonnegative integer. Exiting.') + + @nphi.setter + def nphi(self, nphi): + if type(nphi) == int and nphi > -1: + self.__nphi = nphi + else: + raise Exception('Invalid nphi value, expected nonnegative integer. Exiting.') + + @nphi_e.setter + def nphi_e(self, nphi_e): + if type(nphi_e) == int and nphi_e > -1: + self.__nphi_e = nphi_e + else: + raise Exception('Invalid nphi_e value, expected nonnegative integer. Exiting.') + + @nbx.setter + def nbx(self, nbx): + if isinstance(nbx, int) and nbx > -1: + self.__nbx = nbx + else: + raise Exception('Invalid nbx value, expected nonnegative integer. Exiting.') + + @nbxe_0.setter + def nbxe_0(self, nbxe_0): + if isinstance(nbxe_0, int) and nbxe_0 > -1: + self.__nbxe_0 = nbxe_0 + else: + raise Exception('Invalid nbxe_0 value, expected nonnegative integer. Exiting.') + + @nbx_0.setter + def nbx_0(self, nbx_0): + if type(nbx_0) == int and nbx_0 > -1: + self.__nbx_0 = nbx_0 + else: + raise Exception('Invalid nbx_0 value, expected nonnegative integer. Exiting.') + + @nbx_e.setter + def nbx_e(self, nbx_e): + if type(nbx_e) == int and nbx_e > -1: + self.__nbx_e = nbx_e + else: + raise Exception('Invalid nbx_e value, expected nonnegative integer. Exiting.') + + @nbu.setter + def nbu(self, nbu): + if type(nbu) == int and nbu > -1: + self.__nbu = nbu + else: + raise Exception('Invalid nbu value, expected nonnegative integer. Exiting.') + + @nsbx.setter + def nsbx(self, nsbx): + if type(nsbx) == int and nsbx > -1: + self.__nsbx = nsbx + else: + raise Exception('Invalid nsbx value, expected nonnegative integer. Exiting.') + + @nsbx_e.setter + def nsbx_e(self, nsbx_e): + if type(nsbx_e) == int and nsbx_e > -1: + self.__nsbx_e = nsbx_e + else: + raise Exception('Invalid nsbx_e value, expected nonnegative integer. Exiting.') + + @nsbu.setter + def nsbu(self, nsbu): + if type(nsbu) == int and nsbu > -1: + self.__nsbu = nsbu + else: + raise Exception('Invalid nsbu value, expected nonnegative integer. Exiting.') + + @nsg.setter + def nsg(self, nsg): + if isinstance(nsg, int) and nsg > -1: + self.__nsg = nsg + else: + raise Exception('Invalid nsg value, expected nonnegative integer. Exiting.') + + @nsg_e.setter + def nsg_e(self, nsg_e): + if isinstance(nsg_e, int) and nsg_e > -1: + self.__nsg_e = nsg_e + else: + raise Exception('Invalid nsg_e value, expected nonnegative integer. Exiting.') + + @nsh.setter + def nsh(self, nsh): + if isinstance(nsh, int) and nsh > -1: + self.__nsh = nsh + else: + raise Exception('Invalid nsh value, expected nonnegative integer. Exiting.') + + @nsh_e.setter + def nsh_e(self, nsh_e): + if isinstance(nsh_e, int) and nsh_e > -1: + self.__nsh_e = nsh_e + else: + raise Exception('Invalid nsh_e value, expected nonnegative integer. Exiting.') + + @nsphi.setter + def nsphi(self, nsphi): + if isinstance(nsphi, int) and nsphi > -1: + self.__nsphi = nsphi + else: + raise Exception('Invalid nsphi value, expected nonnegative integer. Exiting.') + + @nsphi_e.setter + def nsphi_e(self, nsphi_e): + if isinstance(nsphi_e, int) and nsphi_e > -1: + self.__nsphi_e = nsphi_e + else: + raise Exception('Invalid nsphi_e value, expected nonnegative integer. Exiting.') + + @ns.setter + def ns(self, ns): + if isinstance(ns, int) and ns > -1: + self.__ns = ns + else: + raise Exception('Invalid ns value, expected nonnegative integer. Exiting.') + + @ns_e.setter + def ns_e(self, ns_e): + if isinstance(ns_e, int) and ns_e > -1: + self.__ns_e = ns_e + else: + raise Exception('Invalid ns_e value, expected nonnegative integer. Exiting.') + + @ng.setter + def ng(self, ng): + if isinstance(ng, int) and ng > -1: + self.__ng = ng + else: + raise Exception('Invalid ng value, expected nonnegative integer. Exiting.') + + @ng_e.setter + def ng_e(self, ng_e): + if isinstance(ng_e, int) and ng_e > -1: + self.__ng_e = ng_e + else: + raise Exception('Invalid ng_e value, expected nonnegative integer. Exiting.') + + @N.setter + def N(self, N): + if isinstance(N, int) and N > 0: + self.__N = N + else: + raise Exception('Invalid N value, expected positive integer. Exiting.') + + def set(self, attr, value): + setattr(self, attr, value) + + +class AcadosOcpCost: + """ + Class containing the numerical data of the cost: + + In case of LINEAR_LS: + stage cost is + :math:`l(x,u,z) = || V_x \, x + V_u \, u + V_z \, z - y_\\text{ref}||^2_W`, + terminal cost is + :math:`m(x) = || V^e_x \, x - y_\\text{ref}^e||^2_{W^e}` + + In case of NONLINEAR_LS: + stage cost is + :math:`l(x,u,z) = || y(x,u,z) - y_\\text{ref}||^2_W`, + terminal cost is + :math:`m(x) = || y^e(x) - y_\\text{ref}^e||^2_{W^e}` + """ + def __init__(self): + # initial stage + self.__cost_type_0 = None + self.__W_0 = None + self.__Vx_0 = None + self.__Vu_0 = None + self.__Vz_0 = None + self.__yref_0 = None + self.__cost_ext_fun_type_0 = 'casadi' + # Lagrange term + self.__cost_type = 'LINEAR_LS' # cost type + self.__W = np.zeros((0,0)) + self.__Vx = np.zeros((0,0)) + self.__Vu = np.zeros((0,0)) + self.__Vz = np.zeros((0,0)) + self.__yref = np.array([]) + self.__Zl = np.array([]) + self.__Zu = np.array([]) + self.__zl = np.array([]) + self.__zu = np.array([]) + self.__cost_ext_fun_type = 'casadi' + # Mayer term + self.__cost_type_e = 'LINEAR_LS' + self.__W_e = np.zeros((0,0)) + self.__Vx_e = np.zeros((0,0)) + self.__yref_e = np.array([]) + self.__Zl_e = np.array([]) + self.__Zu_e = np.array([]) + self.__zl_e = np.array([]) + self.__zu_e = np.array([]) + self.__cost_ext_fun_type_e = 'casadi' + + # initial stage + @property + def cost_type_0(self): + """Cost type at initial shooting node (0) + -- string in {EXTERNAL, LINEAR_LS, NONLINEAR_LS} or :code:`None`. + Default: :code:`None`. + + .. note:: Cost at initial stage is the same as for intermediate shooting nodes if not set differently explicitly. + + .. note:: If :py:attr:`cost_type_0` is set to :code:`None` values in :py:attr:`W_0`, :py:attr:`Vx_0`, :py:attr:`Vu_0`, :py:attr:`Vz_0` and :py:attr:`yref_0` are ignored (set to :code:`None`). + """ + return self.__cost_type_0 + + @property + def W_0(self): + """:math:`W_0` - weight matrix at initial shooting node (0). + Default: :code:`None`. + """ + return self.__W_0 + + @property + def Vx_0(self): + """:math:`V_x^0` - x matrix coefficient at initial shooting node (0). + Default: :code:`None`. + """ + return self.__Vx_0 + + @property + def Vu_0(self): + """:math:`V_u^0` - u matrix coefficient at initial shooting node (0). + Default: :code:`None`. + """ + return self.__Vu_0 + + @property + def Vz_0(self): + """:math:`V_z^0` - z matrix coefficient at initial shooting node (0). + Default: :code:`None`. + """ + return self.__Vz_0 + + @property + def yref_0(self): + """:math:`y_\\text{ref}^0` - reference at initial shooting node (0). + Default: :code:`None`. + """ + return self.__yref_0 + + @property + def cost_ext_fun_type_0(self): + """Type of external function for cost at initial shooting node (0) + -- string in {casadi, generic} or :code:`None` + Default: :code:'casadi'. + + .. note:: Cost at initial stage is the same as for intermediate shooting nodes if not set differently explicitly. + """ + return self.__cost_ext_fun_type_0 + + @yref_0.setter + def yref_0(self, yref_0): + if isinstance(yref_0, np.ndarray): + self.__yref_0 = yref_0 + else: + raise Exception('Invalid yref_0 value, expected numpy array. Exiting.') + + @W_0.setter + def W_0(self, W_0): + if isinstance(W_0, np.ndarray) and len(W_0.shape) == 2: + self.__W_0 = W_0 + else: + raise Exception('Invalid cost W_0 value. ' \ + + 'Should be 2 dimensional numpy array. Exiting.') + + @Vx_0.setter + def Vx_0(self, Vx_0): + if isinstance(Vx_0, np.ndarray) and len(Vx_0.shape) == 2: + self.__Vx_0 = Vx_0 + else: + raise Exception('Invalid cost Vx_0 value. ' \ + + 'Should be 2 dimensional numpy array. Exiting.') + + @Vu_0.setter + def Vu_0(self, Vu_0): + if isinstance(Vu_0, np.ndarray) and len(Vu_0.shape) == 2: + self.__Vu_0 = Vu_0 + else: + raise Exception('Invalid cost Vu_0 value. ' \ + + 'Should be 2 dimensional numpy array. Exiting.') + + @Vz_0.setter + def Vz_0(self, Vz_0): + if isinstance(Vz_0, np.ndarray) and len(Vz_0.shape) == 2: + self.__Vz_0 = Vz_0 + else: + raise Exception('Invalid cost Vz_0 value. ' \ + + 'Should be 2 dimensional numpy array. Exiting.') + + @cost_ext_fun_type_0.setter + def cost_ext_fun_type_0(self, cost_ext_fun_type_0): + if cost_ext_fun_type_0 in ['casadi', 'generic']: + self.__cost_ext_fun_type_0 = cost_ext_fun_type_0 + else: + raise Exception('Invalid cost_ext_fun_type_0 value, expected numpy array. Exiting.') + + # Lagrange term + @property + def cost_type(self): + """ + Cost type at intermediate shooting nodes (1 to N-1) + -- string in {EXTERNAL, LINEAR_LS, NONLINEAR_LS}. + Default: 'LINEAR_LS'. + """ + return self.__cost_type + + @property + def W(self): + """:math:`W` - weight matrix at intermediate shooting nodes (1 to N-1). + Default: :code:`np.zeros((0,0))`. + """ + return self.__W + + @property + def Vx(self): + """:math:`V_x` - x matrix coefficient at intermediate shooting nodes (1 to N-1). + Default: :code:`np.zeros((0,0))`. + """ + return self.__Vx + + @property + def Vu(self): + """:math:`V_u` - u matrix coefficient at intermediate shooting nodes (1 to N-1). + Default: :code:`np.zeros((0,0))`. + """ + return self.__Vu + + @property + def Vz(self): + """:math:`V_z` - z matrix coefficient at intermediate shooting nodes (1 to N-1). + Default: :code:`np.zeros((0,0))`. + """ + return self.__Vz + + @property + def yref(self): + """:math:`y_\\text{ref}` - reference at intermediate shooting nodes (1 to N-1). + Default: :code:`np.array([])`. + """ + return self.__yref + + @property + def Zl(self): + """:math:`Z_l` - diagonal of Hessian wrt lower slack at intermediate shooting nodes (1 to N-1). + Default: :code:`np.array([])`. + """ + return self.__Zl + + @property + def Zu(self): + """:math:`Z_u` - diagonal of Hessian wrt upper slack at intermediate shooting nodes (1 to N-1). + Default: :code:`np.array([])`. + """ + return self.__Zu + + @property + def zl(self): + """:math:`z_l` - gradient wrt lower slack at intermediate shooting nodes (1 to N-1). + Default: :code:`np.array([])`. + """ + return self.__zl + + @property + def zu(self): + """:math:`z_u` - gradient wrt upper slack at intermediate shooting nodes (1 to N-1). + Default: :code:`np.array([])`. + """ + return self.__zu + + @property + def cost_ext_fun_type(self): + """Type of external function for cost at intermediate shooting nodes (1 to N-1). + -- string in {casadi, generic} + Default: :code:'casadi'. + """ + return self.__cost_ext_fun_type + + @cost_type.setter + def cost_type(self, cost_type): + cost_types = ('LINEAR_LS', 'NONLINEAR_LS', 'EXTERNAL') + if cost_type in cost_types: + self.__cost_type = cost_type + else: + raise Exception('Invalid cost_type value. Exiting.') + + @cost_type_0.setter + def cost_type_0(self, cost_type_0): + cost_types = ('LINEAR_LS', 'NONLINEAR_LS', 'EXTERNAL') + if cost_type_0 in cost_types: + self.__cost_type_0 = cost_type_0 + else: + raise Exception('Invalid cost_type_0 value. Exiting.') + + @W.setter + def W(self, W): + if isinstance(W, np.ndarray) and len(W.shape) == 2: + self.__W = W + else: + raise Exception('Invalid cost W value. ' \ + + 'Should be 2 dimensional numpy array. Exiting.') + + + @Vx.setter + def Vx(self, Vx): + if isinstance(Vx, np.ndarray) and len(Vx.shape) == 2: + self.__Vx = Vx + else: + raise Exception('Invalid cost Vx value. ' \ + + 'Should be 2 dimensional numpy array. Exiting.') + + @Vu.setter + def Vu(self, Vu): + if isinstance(Vu, np.ndarray) and len(Vu.shape) == 2: + self.__Vu = Vu + else: + raise Exception('Invalid cost Vu value. ' \ + + 'Should be 2 dimensional numpy array. Exiting.') + + @Vz.setter + def Vz(self, Vz): + if isinstance(Vz, np.ndarray) and len(Vz.shape) == 2: + self.__Vz = Vz + else: + raise Exception('Invalid cost Vz value. ' \ + + 'Should be 2 dimensional numpy array. Exiting.') + + @yref.setter + def yref(self, yref): + if isinstance(yref, np.ndarray): + self.__yref = yref + else: + raise Exception('Invalid yref value, expected numpy array. Exiting.') + + @Zl.setter + def Zl(self, Zl): + if isinstance(Zl, np.ndarray): + self.__Zl = Zl + else: + raise Exception('Invalid Zl value, expected numpy array. Exiting.') + + @Zu.setter + def Zu(self, Zu): + if isinstance(Zu, np.ndarray): + self.__Zu = Zu + else: + raise Exception('Invalid Zu value, expected numpy array. Exiting.') + + @zl.setter + def zl(self, zl): + if isinstance(zl, np.ndarray): + self.__zl = zl + else: + raise Exception('Invalid zl value, expected numpy array. Exiting.') + + @zu.setter + def zu(self, zu): + if isinstance(zu, np.ndarray): + self.__zu = zu + else: + raise Exception('Invalid zu value, expected numpy array. Exiting.') + + @cost_ext_fun_type.setter + def cost_ext_fun_type(self, cost_ext_fun_type): + if cost_ext_fun_type in ['casadi', 'generic']: + self.__cost_ext_fun_type = cost_ext_fun_type + else: + raise Exception('Invalid cost_ext_fun_type value, expected numpy array. Exiting.') + + # Mayer term + @property + def cost_type_e(self): + """ + Cost type at terminal shooting node (N) + -- string in {EXTERNAL, LINEAR_LS, NONLINEAR_LS}. + Default: 'LINEAR_LS'. + """ + return self.__cost_type_e + + @property + def W_e(self): + """:math:`W_e` - weight matrix at terminal shooting node (N). + Default: :code:`np.zeros((0,0))`. + """ + return self.__W_e + + @property + def Vx_e(self): + """:math:`V_x^e` - x matrix coefficient for cost at terminal shooting node (N). + Default: :code:`np.zeros((0,0))`. + """ + return self.__Vx_e + + @property + def yref_e(self): + """:math:`y_\\text{ref}^e` - cost reference at terminal shooting node (N). + Default: :code:`np.array([])`. + """ + return self.__yref_e + + @property + def Zl_e(self): + """:math:`Z_l^e` - diagonal of Hessian wrt lower slack at terminal shooting node (N). + Default: :code:`np.array([])`. + """ + return self.__Zl_e + + @property + def Zu_e(self): + """:math:`Z_u^e` - diagonal of Hessian wrt upper slack at terminal shooting node (N). + Default: :code:`np.array([])`. + """ + return self.__Zu_e + + @property + def zl_e(self): + """:math:`z_l^e` - gradient wrt lower slack at terminal shooting node (N). + Default: :code:`np.array([])`. + """ + return self.__zl_e + + @property + def zu_e(self): + """:math:`z_u^e` - gradient wrt upper slack at terminal shooting node (N). + Default: :code:`np.array([])`. + """ + return self.__zu_e + + @property + def cost_ext_fun_type_e(self): + """Type of external function for cost at intermediate shooting nodes (1 to N-1). + -- string in {casadi, generic} + Default: :code:'casadi'. + """ + return self.__cost_ext_fun_type_e + + @cost_type_e.setter + def cost_type_e(self, cost_type_e): + cost_types = ('LINEAR_LS', 'NONLINEAR_LS', 'EXTERNAL') + + if cost_type_e in cost_types: + self.__cost_type_e = cost_type_e + else: + raise Exception('Invalid cost_type_e value. Exiting.') + + @W_e.setter + def W_e(self, W_e): + if isinstance(W_e, np.ndarray) and len(W_e.shape) == 2: + self.__W_e = W_e + else: + raise Exception('Invalid cost W_e value. ' \ + + 'Should be 2 dimensional numpy array. Exiting.') + + @Vx_e.setter + def Vx_e(self, Vx_e): + if isinstance(Vx_e, np.ndarray) and len(Vx_e.shape) == 2: + self.__Vx_e = Vx_e + else: + raise Exception('Invalid cost Vx_e value. ' \ + + 'Should be 2 dimensional numpy array. Exiting.') + + @yref_e.setter + def yref_e(self, yref_e): + if isinstance(yref_e, np.ndarray): + self.__yref_e = yref_e + else: + raise Exception('Invalid yref_e value, expected numpy array. Exiting.') + + @Zl_e.setter + def Zl_e(self, Zl_e): + if isinstance(Zl_e, np.ndarray): + self.__Zl_e = Zl_e + else: + raise Exception('Invalid Zl_e value, expected numpy array. Exiting.') + + @Zu_e.setter + def Zu_e(self, Zu_e): + if isinstance(Zu_e, np.ndarray): + self.__Zu_e = Zu_e + else: + raise Exception('Invalid Zu_e value, expected numpy array. Exiting.') + + @zl_e.setter + def zl_e(self, zl_e): + if isinstance(zl_e, np.ndarray): + self.__zl_e = zl_e + else: + raise Exception('Invalid zl_e value, expected numpy array. Exiting.') + + @zu_e.setter + def zu_e(self, zu_e): + if isinstance(zu_e, np.ndarray): + self.__zu_e = zu_e + else: + raise Exception('Invalid zu_e value, expected numpy array. Exiting.') + + @cost_ext_fun_type_e.setter + def cost_ext_fun_type_e(self, cost_ext_fun_type_e): + if cost_ext_fun_type_e in ['casadi', 'generic']: + self.__cost_ext_fun_type_e = cost_ext_fun_type_e + else: + raise Exception('Invalid cost_ext_fun_type_e value, expected numpy array. Exiting.') + + def set(self, attr, value): + setattr(self, attr, value) + + +def print_J_to_idx_note(): + print("NOTE: J* matrix is converted to zero based vector idx* vector, which is returned here.") + + +class AcadosOcpConstraints: + """ + class containing the description of the constraints + """ + def __init__(self): + self.__constr_type = 'BGH' + self.__constr_type_e = 'BGH' + # initial x + self.__lbx_0 = np.array([]) + self.__ubx_0 = np.array([]) + self.__idxbx_0 = np.array([]) + self.__idxbxe_0 = np.array([]) + # state bounds + self.__lbx = np.array([]) + self.__ubx = np.array([]) + self.__idxbx = np.array([]) + # bounds on x at shooting node N + self.__lbx_e = np.array([]) + self.__ubx_e = np.array([]) + self.__idxbx_e = np.array([]) + # bounds on u + self.__lbu = np.array([]) + self.__ubu = np.array([]) + self.__idxbu = np.array([]) + # polytopic constraints + self.__lg = np.array([]) + self.__ug = np.array([]) + self.__D = np.zeros((0,0)) + self.__C = np.zeros((0,0)) + # polytopic constraints at shooting node N + self.__C_e = np.zeros((0,0)) + self.__lg_e = np.array([]) + self.__ug_e = np.array([]) + # nonlinear constraints + self.__lh = np.array([]) + self.__uh = np.array([]) + # nonlinear constraints at shooting node N + self.__uh_e = np.array([]) + self.__lh_e = np.array([]) + # convex-over-nonlinear constraints + self.__lphi = np.array([]) + self.__uphi = np.array([]) + # nonlinear constraints at shooting node N + self.__uphi_e = np.array([]) + self.__lphi_e = np.array([]) + # SLACK BOUNDS + # soft bounds on x + self.__lsbx = np.array([]) + self.__usbx = np.array([]) + self.__idxsbx = np.array([]) + # soft bounds on u + self.__lsbu = np.array([]) + self.__usbu = np.array([]) + self.__idxsbu = np.array([]) + # soft bounds on x at shooting node N + self.__lsbx_e = np.array([]) + self.__usbx_e = np.array([]) + self.__idxsbx_e= np.array([]) + # soft bounds on general linear constraints + self.__lsg = np.array([]) + self.__usg = np.array([]) + self.__idxsg = np.array([]) + # soft bounds on nonlinear constraints + self.__lsh = np.array([]) + self.__ush = np.array([]) + self.__idxsh = np.array([]) + # soft bounds on nonlinear constraints + self.__lsphi = np.array([]) + self.__usphi = np.array([]) + self.__idxsphi = np.array([]) + # soft bounds on general linear constraints at shooting node N + self.__lsg_e = np.array([]) + self.__usg_e = np.array([]) + self.__idxsg_e = np.array([]) + # soft bounds on nonlinear constraints at shooting node N + self.__lsh_e = np.array([]) + self.__ush_e = np.array([]) + self.__idxsh_e = np.array([]) + # soft bounds on nonlinear constraints at shooting node N + self.__lsphi_e = np.array([]) + self.__usphi_e = np.array([]) + self.__idxsphi_e = np.array([]) + + + # types + @property + def constr_type(self): + """Constraints type for shooting nodes (0 to N-1). string in {BGH, BGP}. + Default: BGH; BGP is for convex over nonlinear.""" + return self.__constr_type + + @property + def constr_type_e(self): + """Constraints type for terminal shooting node N. string in {BGH, BGP}. + Default: BGH; BGP is for convex over nonlinear.""" + return self.__constr_type_e + + # initial bounds on x + @property + def lbx_0(self): + """:math:`\\underline{x_0}` - lower bounds on x at initial stage 0. + Type: :code:`np.ndarray`; default: :code:`np.array([])`.""" + return self.__lbx_0 + + @property + def ubx_0(self): + """:math:`\\bar{x_0}` - upper bounds on x at initial stage 0. + Type: :code:`np.ndarray`; default: :code:`np.array([])`""" + return self.__ubx_0 + + @property + def Jbx_0(self): + """:math:`J_{bx,0}` - matrix coefficient for bounds on x at initial stage 0. + Translated internally to :py:attr:`idxbx_0`""" + print_J_to_idx_note() + return self.__idxbx_0 + + @property + def idxbx_0(self): + """Indices of bounds on x at initial stage 0 + -- can be set automatically via x0. + Can be set by using :py:attr:`Jbx_0`. + Type: :code:`np.ndarray`; default: :code:`np.array([])`""" + return self.__idxbx_0 + + @property + def idxbxe_0(self): + """Indices of bounds on x0 that are equalities -- can be set automatically via :py:attr:`x0`. + Type: :code:`np.ndarray`; default: :code:`np.array([])`""" + return self.__idxbxe_0 + + # bounds on x + @property + def lbx(self): + """:math:`\\underline{x}` - lower bounds on x at intermediate shooting nodes (1 to N-1). + Type: :code:`np.ndarray`; default: :code:`np.array([])`""" + return self.__lbx + + @property + def ubx(self): + """:math:`\\bar{x}` - upper bounds on x at intermediate shooting nodes (1 to N-1). + Type: :code:`np.ndarray`; default: :code:`np.array([])`""" + return self.__ubx + + @property + def idxbx(self): + """indices of bounds on x (defines :math:`J_{bx}`) at intermediate shooting nodes (1 to N-1). + Can be set by using :py:attr:`Jbx`. + Type: :code:`np.ndarray`; default: :code:`np.array([])`""" + return self.__idxbx + + @property + def Jbx(self): + """:math:`J_{bx}` - matrix coefficient for bounds on x + at intermediate shooting nodes (1 to N-1). + Translated internally into :py:attr:`idxbx`.""" + print_J_to_idx_note() + return self.__idxbx + + # bounds on x at shooting node N + @property + def lbx_e(self): + """:math:`\\underline{x}^e` - lower bounds on x at terminal shooting node N. + Type: :code:`np.ndarray`; default: :code:`np.array([])`""" + return self.__lbx_e + + @property + def ubx_e(self): + """:math:`\\bar{x}^e` - upper bounds on x at terminal shooting node N. + Type: :code:`np.ndarray`; default: :code:`np.array([])`""" + return self.__ubx_e + + @property + def idxbx_e(self): + """Indices for bounds on x at terminal shooting node N (defines :math:`J_{bx}^e`). + Can be set by using :py:attr:`Jbx_e`. + Type: :code:`np.ndarray`; default: :code:`np.array([])`""" + return self.__idxbx_e + + @property + def Jbx_e(self): + """:math:`J_{bx}^e` matrix coefficient for bounds on x at terminal shooting node N. + Translated internally into :py:attr:`idxbx_e`.""" + print_J_to_idx_note() + return self.__idxbx_e + + # bounds on u + @property + def lbu(self): + """:math:`\\underline{u}` - lower bounds on u at shooting nodes (0 to N-1). + Type: :code:`np.ndarray`; default: :code:`np.array([])` + """ + return self.__lbu + + @property + def ubu(self): + """:math:`\\bar{u}` - upper bounds on u at shooting nodes (0 to N-1). + Type: :code:`np.ndarray`; default: :code:`np.array([])` + """ + return self.__ubu + + @property + def idxbu(self): + """Indices of bounds on u (defines :math:`J_{bu}`) at shooting nodes (0 to N-1). + Can be set by using :py:attr:`Jbu`. + Type: :code:`np.ndarray`; default: :code:`np.array([])` + """ + return self.__idxbu + + @property + def Jbu(self): + """:math:`J_{bu}` - matrix coefficient for bounds on u at shooting nodes (0 to N-1). + Translated internally to :py:attr:`idxbu`. + """ + print_J_to_idx_note() + return self.__idxbu + + # polytopic constraints + @property + def C(self): + """:math:`C` - C matrix in :math:`\\underline{g} \\leq D \, u + C \, x \\leq \\bar{g}` + at shooting nodes (0 to N-1). + Type: :code:`np.ndarray`; default: :code:`np.array((0,0))`. + """ + return self.__C + + @property + def D(self): + """:math:`D` - D matrix in :math:`\\underline{g} \\leq D \, u + C \, x \\leq \\bar{g}` + at shooting nodes (0 to N-1). + Type: :code:`np.ndarray`; default: :code:`np.array((0,0))` + """ + return self.__D + + @property + def lg(self): + """:math:`\\underline{g}` - lower bound for general polytopic inequalities + at shooting nodes (0 to N-1). + Type: :code:`np.ndarray`; default: :code:`np.array([])` + """ + return self.__lg + + @property + def ug(self): + """:math:`\\bar{g}` - upper bound for general polytopic inequalities + at shooting nodes (0 to N-1). + Type: :code:`np.ndarray`; default: :code:`np.array([])`. + """ + return self.__ug + + # polytopic constraints at shooting node N + @property + def C_e(self): + """:math:`C^e` - C matrix at terminal shooting node N. + Type: :code:`np.ndarray`; default: :code:`np.array((0,0))`. + """ + return self.__C_e + + @property + def lg_e(self): + """:math:`\\underline{g}^e` - lower bound on general polytopic inequalities + at terminal shooting node N. + Type: :code:`np.ndarray`; default: :code:`np.array([])`. + """ + return self.__lg_e + + @property + def ug_e(self): + """:math:`\\bar{g}^e` - upper bound on general polytopic inequalities + at terminal shooting node N. + Type: :code:`np.ndarray`; default: :code:`np.array([])`. + """ + return self.__ug_e + + + # nonlinear constraints + @property + def lh(self): + """:math:`\\underline{h}` - lower bound for nonlinear inequalities + at shooting nodes (0 to N-1). + Type: :code:`np.ndarray`; default: :code:`np.array([])`. + """ + return self.__lh + + @property + def uh(self): + """:math:`\\bar{h}` - upper bound for nonlinear inequalities + at shooting nodes (0 to N-1). + Type: :code:`np.ndarray`; default: :code:`np.array([])`. + """ + return self.__uh + + # nonlinear constraints at shooting node N + @property + def lh_e(self): + """:math:`\\underline{h}^e` - lower bound on nonlinear inequalities + at terminal shooting node N. + Type: :code:`np.ndarray`; default: :code:`np.array([])`. + """ + return self.__lh_e + + @property + def uh_e(self): + """:math:`\\bar{h}^e` - upper bound on nonlinear inequalities + at terminal shooting node N. + Type: :code:`np.ndarray`; default: :code:`np.array([])`. + """ + return self.__uh_e + + # convex-over-nonlinear constraints + @property + def lphi(self): + """:math:`\\underline{\phi}` - lower bound for convex-over-nonlinear inequalities + at shooting nodes (0 to N-1). + Type: :code:`np.ndarray`; default: :code:`np.array([])`. + """ + return self.__lphi + + @property + def uphi(self): + """:math:`\\bar{\phi}` - upper bound for convex-over-nonlinear inequalities + at shooting nodes (0 to N-1). + Type: :code:`np.ndarray`; default: :code:`np.array([])`. + """ + return self.__uphi + + # convex-over-nonlinear constraints at shooting node N + @property + def lphi_e(self): + """:math:`\\underline{\phi}^e` - lower bound on convex-over-nonlinear inequalities + at terminal shooting node N. + Type: :code:`np.ndarray`; default: :code:`np.array([])`. + """ + return self.__lphi_e + + @property + def uphi_e(self): + """:math:`\\bar{\phi}^e` - upper bound on convex-over-nonlinear inequalities + at terminal shooting node N. + Type: :code:`np.ndarray`; default: :code:`np.array([])`. + """ + return self.__uphi_e + + + # SLACK bounds + # soft bounds on x + @property + def lsbx(self): + """Lower bounds on slacks corresponding to soft lower bounds on x + at stages (1 to N-1); + not required - zeros by default""" + return self.__lsbx + + @property + def usbx(self): + """Lower bounds on slacks corresponding to soft upper bounds on x + at stages (1 to N-1); + not required - zeros by default""" + return self.__usbx + + @property + def idxsbx(self): + """Indices of soft bounds on x within the indices of bounds on x + at stages (1 to N-1). + Can be set by using :py:attr:`Jsbx`. + Type: :code:`np.ndarray`; default: :code:`np.array([])`""" + return self.__idxsbx + + @property + def Jsbx(self): + """:math:`J_{sbx}` - matrix coefficient for soft bounds on x + at stages (1 to N-1); + Translated internally into :py:attr:`idxsbx`.""" + print_J_to_idx_note() + return self.__idxsbx + + # soft bounds on u + @property + def lsbu(self): + """Lower bounds on slacks corresponding to soft lower bounds on u + at stages (0 to N-1). + Not required - zeros by default.""" + return self.__lsbu + + @property + def usbu(self): + """Lower bounds on slacks corresponding to soft upper bounds on u + at stages (0 to N-1); + not required - zeros by default""" + return self.__usbu + + @property + def idxsbu(self): + """Indices of soft bounds on u within the indices of bounds on u + at stages (0 to N-1). + Can be set by using :py:attr:`Jsbu`. + Type: :code:`np.ndarray`; default: :code:`np.array([])`""" + return self.__idxsbu + + @property + def Jsbu(self): + """:math:`J_{sbu}` - matrix coefficient for soft bounds on u + at stages (0 to N-1); + internally translated into :py:attr:`idxsbu`""" + print_J_to_idx_note() + return self.__idxsbu + + # soft bounds on x at shooting node N + @property + def lsbx_e(self): + """Lower bounds on slacks corresponding to soft lower bounds on x at shooting node N. + Not required - zeros by default""" + return self.__lsbx_e + + @property + def usbx_e(self): + """Lower bounds on slacks corresponding to soft upper bounds on x at shooting node N. + Not required - zeros by default""" + return self.__usbx_e + + @property + def idxsbx_e(self): + """Indices of soft bounds on x at shooting node N, within the indices of bounds on x at terminal shooting node N. + Can be set by using :py:attr:`Jsbx_e`. + Type: :code:`np.ndarray`; default: :code:`np.array([])`""" + return self.__idxsbx_e + + @property + def Jsbx_e(self): + """:math:`J_{sbx}^e` - matrix coefficient for soft bounds on x at terminal shooting node N. + Translated internally to :py:attr:`idxsbx_e`""" + print_J_to_idx_note() + return self.__idxsbx_e + + # soft general linear constraints + @property + def lsg(self): + """Lower bounds on slacks corresponding to soft lower bounds for general linear constraints + at stages (0 to N-1). + Type: :code:`np.ndarray`; default: :code:`np.array([])` + """ + return self.__lsg + + @property + def usg(self): + """Lower bounds on slacks corresponding to soft upper bounds for general linear constraints. + Not required - zeros by default""" + return self.__usg + + @property + def idxsg(self): + """Indices of soft general linear constraints within the indices of general linear constraints. + Can be set by using :py:attr:`Jsg`. + Type: :code:`np.ndarray`; default: :code:`np.array([])`""" + return self.__idxsg + + @property + def Jsg(self): + """:math:`J_{sg}` - matrix coefficient for soft bounds on general linear constraints. + Translated internally to :py:attr:`idxsg`""" + print_J_to_idx_note() + return self.__idxsg + + # soft nonlinear constraints + @property + def lsh(self): + """Lower bounds on slacks corresponding to soft lower bounds for nonlinear constraints. + Not required - zeros by default""" + return self.__lsh + + @property + def ush(self): + """Lower bounds on slacks corresponding to soft upper bounds for nonlinear constraints. + Not required - zeros by default""" + return self.__ush + + @property + def idxsh(self): + """Indices of soft nonlinear constraints within the indices of nonlinear constraints. + Can be set by using :py:attr:`Jbx`. + Type: :code:`np.ndarray`; default: :code:`np.array([])`""" + return self.__idxsh + + @property + def Jsh(self): + """:math:`J_{sh}` - matrix coefficient for soft bounds on nonlinear constraints. + Translated internally to :py:attr:`idxsh`""" + print_J_to_idx_note() + return self.__idxsh + + # soft bounds on convex-over-nonlinear constraints + @property + def lsphi(self): + """Lower bounds on slacks corresponding to soft lower bounds for convex-over-nonlinear constraints. + Not required - zeros by default""" + return self.__lsphi + + @property + def usphi(self): + """Lower bounds on slacks corresponding to soft upper bounds for convex-over-nonlinear constraints. + Not required - zeros by default""" + return self.__usphi + + @property + def idxsphi(self): + """Indices of soft convex-over-nonlinear constraints within the indices of nonlinear constraints. + Can be set by using :py:attr:`Jsphi`. + Type: :code:`np.ndarray`; default: :code:`np.array([])`""" + return self.__idxsphi + + @property + def Jsphi(self): + """:math:`J_{s, \phi}` - matrix coefficient for soft bounds on convex-over-nonlinear constraints. + Translated internally into :py:attr:`idxsphi`.""" + print_J_to_idx_note() + return self.__idxsphi + + + # soft bounds on general linear constraints at shooting node N + @property + def lsg_e(self): + """Lower bounds on slacks corresponding to soft lower bounds for general linear constraints at shooting node N. + Not required - zeros by default""" + return self.__lsg_e + + @property + def usg_e(self): + """Lower bounds on slacks corresponding to soft upper bounds for general linear constraints at shooting node N. + Not required - zeros by default""" + return self.__usg_e + + @property + def idxsg_e(self): + """Indices of soft general linear constraints at shooting node N within the indices of general linear constraints at shooting node N. + Can be set by using :py:attr:`Jsg_e`.""" + return self.__idxsg_e + + @property + def Jsg_e(self): + """:math:`J_{s,h}^e` - matrix coefficient for soft bounds on general linear constraints at terminal shooting node N. + Translated internally to :py:attr:`idxsg_e`""" + print_J_to_idx_note() + return self.__idxsg_e + + + # soft bounds on nonlinear constraints at shooting node N + @property + def lsh_e(self): + """Lower bounds on slacks corresponding to soft lower bounds for nonlinear constraints at terminal shooting node N. + Not required - zeros by default""" + return self.__lsh_e + + @property + def ush_e(self): + """Lower bounds on slacks corresponding to soft upper bounds for nonlinear constraints at terminal shooting node N. + Not required - zeros by default""" + return self.__ush_e + + @property + def idxsh_e(self): + """Indices of soft nonlinear constraints at shooting node N within the indices of nonlinear constraints at terminal shooting node N. + Can be set by using :py:attr:`Jsh_e`.""" + return self.__idxsh_e + + @property + def Jsh_e(self): + """:math:`J_{s,h}^e` - matrix coefficient for soft bounds on nonlinear constraints at terminal shooting node N; fills :py:attr:`idxsh_e`""" + print_J_to_idx_note() + return self.__idxsh_e + + # soft bounds on convex-over-nonlinear constraints at shooting node N + @property + def lsphi_e(self): + """Lower bounds on slacks corresponding to soft lower bounds for convex-over-nonlinear constraints at terminal shooting node N. + Not required - zeros by default""" + return self.__lsphi_e + + @property + def usphi_e(self): + """Lower bounds on slacks corresponding to soft upper bounds for convex-over-nonlinear constraints at terminal shooting node N. + Not required - zeros by default""" + return self.__usphi_e + + @property + def idxsphi_e(self): + """Indices of soft nonlinear constraints at shooting node N within the indices of nonlinear constraints at terminal shooting node N. + Can be set by using :py:attr:`Jsphi_e`. + Type: :code:`np.ndarray`; default: :code:`np.array([])`""" + return self.__idxsphi_e + + @property + def Jsphi_e(self): + """:math:`J_{sh}^e` - matrix coefficient for soft bounds on convex-over-nonlinear constraints at shooting node N. + Translated internally to :py:attr:`idxsphi_e`""" + print_J_to_idx_note() + return self.__idxsphi_e + + @property + def x0(self): + """:math:`x_0 \\in \mathbb{R}^{n_x}` - initial state -- + Translated internally to :py:attr:`idxbx_0`, :py:attr:`lbx_0`, :py:attr:`ubx_0`, :py:attr:`idxbxe_0` """ + print("x0 is converted to lbx_0, ubx_0, idxbx_0") + print("idxbx_0: ", self.__idxbx_0) + print("lbx_0: ", self.__lbx_0) + print("ubx_0: ", self.__ubx_0) + print("idxbxe_0: ", self.__idxbxe_0) + return None + + # SETTERS + @constr_type.setter + def constr_type(self, constr_type): + constr_types = ('BGH', 'BGP') + if constr_type in constr_types: + self.__constr_type = constr_type + else: + raise Exception('Invalid constr_type value. Possible values are:\n\n' \ + + ',\n'.join(constr_types) + '.\n\nYou have: ' + constr_type + '.\n\nExiting.') + + @constr_type_e.setter + def constr_type_e(self, constr_type_e): + constr_types = ('BGH', 'BGP') + if constr_type_e in constr_types: + self.__constr_type_e = constr_type_e + else: + raise Exception('Invalid constr_type_e value. Possible values are:\n\n' \ + + ',\n'.join(constr_types) + '.\n\nYou have: ' + constr_type_e + '.\n\nExiting.') + + # initial x + @lbx_0.setter + def lbx_0(self, lbx_0): + if type(lbx_0) == np.ndarray: + self.__lbx_0 = lbx_0 + else: + raise Exception('Invalid lbx_0 value. Exiting.') + + @ubx_0.setter + def ubx_0(self, ubx_0): + if type(ubx_0) == np.ndarray: + self.__ubx_0 = ubx_0 + else: + raise Exception('Invalid ubx_0 value. Exiting.') + + @idxbx_0.setter + def idxbx_0(self, idxbx_0): + if isinstance(idxbx_0, np.ndarray): + self.__idxbx_0 = idxbx_0 + else: + raise Exception('Invalid idxbx_0 value. Exiting.') + + @Jbx_0.setter + def Jbx_0(self, Jbx_0): + if type(Jbx_0) == np.ndarray: + self.__idxbx_0 = J_to_idx(Jbx_0) + else: + raise Exception('Invalid Jbx_0 value. Exiting.') + + @idxbxe_0.setter + def idxbxe_0(self, idxbxe_0): + if isinstance(idxbxe_0, np.ndarray): + self.__idxbxe_0 = idxbxe_0 + else: + raise Exception('Invalid idxbxe_0 value. Exiting.') + + + @x0.setter + def x0(self, x0): + if isinstance(x0, np.ndarray): + self.__lbx_0 = x0 + self.__ubx_0 = x0 + self.__idxbx_0 = np.arange(x0.size) + self.__idxbxe_0 = np.arange(x0.size) + else: + raise Exception('Invalid x0 value. Exiting.') + + # bounds on x + @lbx.setter + def lbx(self, lbx): + if type(lbx) == np.ndarray: + self.__lbx = lbx + else: + raise Exception('Invalid lbx value. Exiting.') + + @ubx.setter + def ubx(self, ubx): + if type(ubx) == np.ndarray: + self.__ubx = ubx + else: + raise Exception('Invalid ubx value. Exiting.') + + @idxbx.setter + def idxbx(self, idxbx): + if type(idxbx) == np.ndarray: + self.__idxbx = idxbx + else: + raise Exception('Invalid idxbx value. Exiting.') + + @Jbx.setter + def Jbx(self, Jbx): + if type(Jbx) == np.ndarray: + self.__idxbx = J_to_idx(Jbx) + else: + raise Exception('Invalid Jbx value. Exiting.') + + # bounds on u + @lbu.setter + def lbu(self, lbu): + if type(lbu) == np.ndarray: + self.__lbu = lbu + else: + raise Exception('Invalid lbu value. Exiting.') + + @ubu.setter + def ubu(self, ubu): + if type(ubu) == np.ndarray: + self.__ubu = ubu + else: + raise Exception('Invalid ubu value. Exiting.') + + @idxbu.setter + def idxbu(self, idxbu): + if type(idxbu) == np.ndarray: + self.__idxbu = idxbu + else: + raise Exception('Invalid idxbu value. Exiting.') + + @Jbu.setter + def Jbu(self, Jbu): + if type(Jbu) == np.ndarray: + self.__idxbu = J_to_idx(Jbu) + else: + raise Exception('Invalid Jbu value. Exiting.') + + # bounds on x at shooting node N + @lbx_e.setter + def lbx_e(self, lbx_e): + if type(lbx_e) == np.ndarray: + self.__lbx_e = lbx_e + else: + raise Exception('Invalid lbx_e value. Exiting.') + + @ubx_e.setter + def ubx_e(self, ubx_e): + if type(ubx_e) == np.ndarray: + self.__ubx_e = ubx_e + else: + raise Exception('Invalid ubx_e value. Exiting.') + + @idxbx_e.setter + def idxbx_e(self, idxbx_e): + if type(idxbx_e) == np.ndarray: + self.__idxbx_e = idxbx_e + else: + raise Exception('Invalid idxbx_e value. Exiting.') + + @Jbx_e.setter + def Jbx_e(self, Jbx_e): + if type(Jbx_e) == np.ndarray: + self.__idxbx_e = J_to_idx(Jbx_e) + else: + raise Exception('Invalid Jbx_e value. Exiting.') + + # polytopic constraints + @D.setter + def D(self, D): + if isinstance(D, np.ndarray) and len(D.shape) == 2: + self.__D = D + else: + raise Exception('Invalid constraint D value.' \ + + 'Should be 2 dimensional numpy array. Exiting.') + + @C.setter + def C(self, C): + if isinstance(C, np.ndarray) and len(C.shape) == 2: + self.__C = C + else: + raise Exception('Invalid constraint C value.' \ + + 'Should be 2 dimensional numpy array. Exiting.') + + @lg.setter + def lg(self, lg): + if type(lg) == np.ndarray: + self.__lg = lg + else: + raise Exception('Invalid lg value. Exiting.') + + @ug.setter + def ug(self, ug): + if type(ug) == np.ndarray: + self.__ug = ug + else: + raise Exception('Invalid ug value. Exiting.') + + # polytopic constraints at shooting node N + @C_e.setter + def C_e(self, C_e): + if isinstance(C_e, np.ndarray) and len(C_e.shape) == 2: + self.__C_e = C_e + else: + raise Exception('Invalid constraint C_e value.' \ + + 'Should be 2 dimensional numpy array. Exiting.') + + @lg_e.setter + def lg_e(self, lg_e): + if type(lg_e) == np.ndarray: + self.__lg_e = lg_e + else: + raise Exception('Invalid lg_e value. Exiting.') + + @ug_e.setter + def ug_e(self, ug_e): + if type(ug_e) == np.ndarray: + self.__ug_e = ug_e + else: + raise Exception('Invalid ug_e value. Exiting.') + + # nonlinear constraints + @lh.setter + def lh(self, lh): + if type(lh) == np.ndarray: + self.__lh = lh + else: + raise Exception('Invalid lh value. Exiting.') + + @uh.setter + def uh(self, uh): + if type(uh) == np.ndarray: + self.__uh = uh + else: + raise Exception('Invalid uh value. Exiting.') + + # convex-over-nonlinear constraints + @lphi.setter + def lphi(self, lphi): + if type(lphi) == np.ndarray: + self.__lphi = lphi + else: + raise Exception('Invalid lphi value. Exiting.') + + @uphi.setter + def uphi(self, uphi): + if type(uphi) == np.ndarray: + self.__uphi = uphi + else: + raise Exception('Invalid uphi value. Exiting.') + + # nonlinear constraints at shooting node N + @lh_e.setter + def lh_e(self, lh_e): + if type(lh_e) == np.ndarray: + self.__lh_e = lh_e + else: + raise Exception('Invalid lh_e value. Exiting.') + + @uh_e.setter + def uh_e(self, uh_e): + if type(uh_e) == np.ndarray: + self.__uh_e = uh_e + else: + raise Exception('Invalid uh_e value. Exiting.') + + # convex-over-nonlinear constraints at shooting node N + @lphi_e.setter + def lphi_e(self, lphi_e): + if type(lphi_e) == np.ndarray: + self.__lphi_e = lphi_e + else: + raise Exception('Invalid lphi_e value. Exiting.') + + @uphi_e.setter + def uphi_e(self, uphi_e): + if type(uphi_e) == np.ndarray: + self.__uphi_e = uphi_e + else: + raise Exception('Invalid uphi_e value. Exiting.') + + # SLACK bounds + # soft bounds on x + @lsbx.setter + def lsbx(self, lsbx): + if type(lsbx) == np.ndarray: + self.__lsbx = lsbx + else: + raise Exception('Invalid lsbx value. Exiting.') + + @usbx.setter + def usbx(self, usbx): + if type(usbx) == np.ndarray: + self.__usbx = usbx + else: + raise Exception('Invalid usbx value. Exiting.') + + @idxsbx.setter + def idxsbx(self, idxsbx): + if type(idxsbx) == np.ndarray: + self.__idxsbx = idxsbx + else: + raise Exception('Invalid idxsbx value. Exiting.') + + @Jsbx.setter + def Jsbx(self, Jsbx): + if isinstance(Jsbx, np.ndarray): + self.__idxsbx = J_to_idx_slack(Jsbx) + else: + raise Exception('Invalid Jsbx value, expected numpy array. Exiting.') + + # soft bounds on u + @lsbu.setter + def lsbu(self, lsbu): + if type(lsbu) == np.ndarray: + self.__lsbu = lsbu + else: + raise Exception('Invalid lsbu value. Exiting.') + + @usbu.setter + def usbu(self, usbu): + if type(usbu) == np.ndarray: + self.__usbu = usbu + else: + raise Exception('Invalid usbu value. Exiting.') + + @idxsbu.setter + def idxsbu(self, idxsbu): + if type(idxsbu) == np.ndarray: + self.__idxsbu = idxsbu + else: + raise Exception('Invalid idxsbu value. Exiting.') + + @Jsbu.setter + def Jsbu(self, Jsbu): + if type(Jsbu) == np.ndarray: + self.__idxsbu = J_to_idx_slack(Jsbu) + else: + raise Exception('Invalid Jsbu value. Exiting.') + + # soft bounds on x at shooting node N + @lsbx_e.setter + def lsbx_e(self, lsbx_e): + if type(lsbx_e) == np.ndarray: + self.__lsbx_e = lsbx_e + else: + raise Exception('Invalid lsbx_e value. Exiting.') + + @usbx_e.setter + def usbx_e(self, usbx_e): + if type(usbx_e) == np.ndarray: + self.__usbx_e = usbx_e + else: + raise Exception('Invalid usbx_e value. Exiting.') + + @idxsbx_e.setter + def idxsbx_e(self, idxsbx_e): + if type(idxsbx_e) == np.ndarray: + self.__idxsbx_e = idxsbx_e + else: + raise Exception('Invalid idxsbx_e value. Exiting.') + + @Jsbx_e.setter + def Jsbx_e(self, Jsbx_e): + if type(Jsbx_e) == np.ndarray: + self.__idxsbx_e = J_to_idx_slack(Jsbx_e) + else: + raise Exception('Invalid Jsbx_e value. Exiting.') + + + # soft bounds on general linear constraints + @lsg.setter + def lsg(self, lsg): + if isinstance(lsg, np.ndarray): + self.__lsg = lsg + else: + raise Exception('Invalid lsg value. Exiting.') + + @usg.setter + def usg(self, usg): + if isinstance(usg, np.ndarray): + self.__usg = usg + else: + raise Exception('Invalid usg value. Exiting.') + + @idxsg.setter + def idxsg(self, idxsg): + if isinstance(idxsg, np.ndarray): + self.__idxsg = idxsg + else: + raise Exception('Invalid idxsg value. Exiting.') + + @Jsg.setter + def Jsg(self, Jsg): + if isinstance(Jsg, np.ndarray): + self.__idxsg = J_to_idx_slack(Jsg) + else: + raise Exception('Invalid Jsg value, expected numpy array. Exiting.') + + + # soft bounds on nonlinear constraints + @lsh.setter + def lsh(self, lsh): + if type(lsh) == np.ndarray: + self.__lsh = lsh + else: + raise Exception('Invalid lsh value. Exiting.') + + @ush.setter + def ush(self, ush): + if type(ush) == np.ndarray: + self.__ush = ush + else: + raise Exception('Invalid ush value. Exiting.') + + @idxsh.setter + def idxsh(self, idxsh): + if type(idxsh) == np.ndarray: + self.__idxsh = idxsh + else: + raise Exception('Invalid idxsh value. Exiting.') + + + @Jsh.setter + def Jsh(self, Jsh): + if isinstance(Jsh, np.ndarray): + self.__idxsh = J_to_idx_slack(Jsh) + else: + raise Exception('Invalid Jsh value, expected numpy array. Exiting.') + + # soft bounds on convex-over-nonlinear constraints + @lsphi.setter + def lsphi(self, lsphi): + if type(lsphi) == np.ndarray: + self.__lsphi = lsphi + else: + raise Exception('Invalid lsphi value. Exiting.') + + @usphi.setter + def usphi(self, usphi): + if type(usphi) == np.ndarray: + self.__usphi = usphi + else: + raise Exception('Invalid usphi value. Exiting.') + + @idxsphi.setter + def idxsphi(self, idxsphi): + if type(idxsphi) == np.ndarray: + self.__idxsphi = idxsphi + else: + raise Exception('Invalid idxsphi value. Exiting.') + + @Jsphi.setter + def Jsphi(self, Jsphi): + if isinstance(Jsphi, np.ndarray): + self.__idxsphi = J_to_idx_slack(Jsphi) + else: + raise Exception('Invalid Jsphi value, expected numpy array. Exiting.') + + # soft bounds on general linear constraints at shooting node N + @lsg_e.setter + def lsg_e(self, lsg_e): + if isinstance(lsg_e, np.ndarray): + self.__lsg_e = lsg_e + else: + raise Exception('Invalid lsg_e value. Exiting.') + + @usg_e.setter + def usg_e(self, usg_e): + if isinstance(usg_e, np.ndarray): + self.__usg_e = usg_e + else: + raise Exception('Invalid usg_e value. Exiting.') + + @idxsg_e.setter + def idxsg_e(self, idxsg_e): + if isinstance(idxsg_e, np.ndarray): + self.__idxsg_e = idxsg_e + else: + raise Exception('Invalid idxsg_e value. Exiting.') + + @Jsg_e.setter + def Jsg_e(self, Jsg_e): + if isinstance(Jsg_e, np.ndarray): + self.__idxsg_e = J_to_idx_slack(Jsg_e) + else: + raise Exception('Invalid Jsg_e value, expected numpy array. Exiting.') + + # soft bounds on nonlinear constraints at shooting node N + @lsh_e.setter + def lsh_e(self, lsh_e): + if isinstance(lsh_e, np.ndarray): + self.__lsh_e = lsh_e + else: + raise Exception('Invalid lsh_e value. Exiting.') + + @ush_e.setter + def ush_e(self, ush_e): + if isinstance(ush_e, np.ndarray): + self.__ush_e = ush_e + else: + raise Exception('Invalid ush_e value. Exiting.') + + @idxsh_e.setter + def idxsh_e(self, idxsh_e): + if isinstance(idxsh_e, np.ndarray): + self.__idxsh_e = idxsh_e + else: + raise Exception('Invalid idxsh_e value. Exiting.') + + @Jsh_e.setter + def Jsh_e(self, Jsh_e): + if isinstance(Jsh_e, np.ndarray): + self.__idxsh_e = J_to_idx_slack(Jsh_e) + else: + raise Exception('Invalid Jsh_e value, expected numpy array. Exiting.') + + + # soft bounds on convex-over-nonlinear constraints at shooting node N + @lsphi_e.setter + def lsphi_e(self, lsphi_e): + if isinstance(lsphi_e, np.ndarray): + self.__lsphi_e = lsphi_e + else: + raise Exception('Invalid lsphi_e value. Exiting.') + + @usphi_e.setter + def usphi_e(self, usphi_e): + if isinstance(usphi_e, np.ndarray): + self.__usphi_e = usphi_e + else: + raise Exception('Invalid usphi_e value. Exiting.') + + @idxsphi_e.setter + def idxsphi_e(self, idxsphi_e): + if isinstance(idxsphi_e, np.ndarray): + self.__idxsphi_e = idxsphi_e + else: + raise Exception('Invalid idxsphi_e value. Exiting.') + + @Jsphi_e.setter + def Jsphi_e(self, Jsphi_e): + if isinstance(Jsphi_e, np.ndarray): + self.__idxsphi_e = J_to_idx_slack(Jsphi_e) + else: + raise Exception('Invalid Jsphi_e value. Exiting.') + + def set(self, attr, value): + setattr(self, attr, value) + + +class AcadosOcpOptions: + """ + class containing the description of the solver options + """ + def __init__(self): + self.__qp_solver = 'PARTIAL_CONDENSING_HPIPM' # qp solver to be used in the NLP solver + self.__hessian_approx = 'GAUSS_NEWTON' # hessian approximation + self.__integrator_type = 'ERK' # integrator type + self.__tf = None # prediction horizon + self.__nlp_solver_type = 'SQP_RTI' # NLP solver + self.__globalization = 'FIXED_STEP' + self.__nlp_solver_step_length = 1.0 # fixed Newton step length + self.__levenberg_marquardt = 0.0 + self.__sim_method_num_stages = 4 # number of stages in the integrator + self.__sim_method_num_steps = 1 # number of steps in the integrator + self.__sim_method_newton_iter = 3 # number of Newton iterations in simulation method + self.__sim_method_jac_reuse = False + self.__qp_solver_tol_stat = None # QP solver stationarity tolerance + self.__qp_solver_tol_eq = None # QP solver equality tolerance + self.__qp_solver_tol_ineq = None # QP solver inequality + self.__qp_solver_tol_comp = None # QP solver complementarity + self.__qp_solver_iter_max = 50 # QP solver max iter + self.__qp_solver_cond_N = None # QP solver: new horizon after partial condensing + self.__qp_solver_warm_start = 0 + self.__nlp_solver_tol_stat = 1e-6 # NLP solver stationarity tolerance + self.__nlp_solver_tol_eq = 1e-6 # NLP solver equality tolerance + self.__nlp_solver_tol_ineq = 1e-6 # NLP solver inequality + self.__nlp_solver_tol_comp = 1e-6 # NLP solver complementarity + self.__nlp_solver_max_iter = 100 # NLP solver maximum number of iterations + self.__Tsim = None # automatically calculated as tf/N + self.__print_level = 0 # print level + self.__initialize_t_slacks = 0 # possible values: 0, 1 + self.__model_external_shared_lib_dir = None # path to the the .so lib + self.__model_external_shared_lib_name = None # name of the the .so lib + self.__regularize_method = None + self.__time_steps = None + self.__shooting_nodes = None + self.__exact_hess_cost = 1 + self.__exact_hess_dyn = 1 + self.__exact_hess_constr = 1 + self.__ext_cost_num_hess = 0 + self.__alpha_min = 0.05 + self.__alpha_reduction = 0.7 + + + @property + def qp_solver(self): + """QP solver to be used in the NLP solver. + String in ('PARTIAL_CONDENSING_HPIPM', 'FULL_CONDENSING_QPOASES', 'FULL_CONDENSING_HPIPM', 'PARTIAL_CONDENSING_QPDUNES', 'PARTIAL_CONDENSING_OSQP'). + Default: 'PARTIAL_CONDENSING_HPIPM'. + """ + return self.__qp_solver + + @property + def hessian_approx(self): + """Hessian approximation. + String in ('GAUSS_NEWTON', 'EXACT'). + Default: 'GAUSS_NEWTON'. + """ + return self.__hessian_approx + + @property + def integrator_type(self): + """ + Integrator type. + String in ('ERK', 'IRK', 'GNSF', 'DISCRETE'). + Default: 'ERK'. + """ + return self.__integrator_type + + @property + def nlp_solver_type(self): + """NLP solver. + String in ('SQP', 'SQP_RTI'). + Default: 'SQP_RTI'. + """ + return self.__nlp_solver_type + + @property + def globalization(self): + """Globalization type. + String in ('FIXED_STEP', 'MERIT_BACKTRACKING'). + Default: 'FIXED_STEP'. + + .. note:: preliminary implementation. + """ + return self.__globalization + + @property + def regularize_method(self): + """Regularization method for the Hessian. + String in ('NO_REGULARIZE', 'MIRROR', 'PROJECT', 'PROJECT_REDUC_HESS', 'CONVEXIFY') or :code:`None`. + + Default: :code:`None`. + """ + return self.__regularize_method + + @property + def nlp_solver_step_length(self): + """ + Fixed Newton step length. + Type: float > 0. + Default: 1.0. + """ + return self.__nlp_solver_step_length + + @property + def levenberg_marquardt(self): + """ + Factor for LM regularization. + Type: float >= 0 + Default: 0.0. + """ + return self.__levenberg_marquardt + + @property + def sim_method_num_stages(self): + """ + Number of stages in the integrator. + Type: int > 0 or ndarray of ints > 0 of shape (N,). + Default: 4 + """ + return self.__sim_method_num_stages + + @property + def sim_method_num_steps(self): + """ + Number of steps in the integrator. + Type: int > 0 or ndarray of ints > 0 of shape (N,). + Default: 1 + """ + return self.__sim_method_num_steps + + @property + def sim_method_newton_iter(self): + """ + Number of Newton iterations in simulation method. + Type: int > 0 + Default: 3 + """ + return self.__sim_method_newton_iter + + @property + def sim_method_jac_reuse(self): + """ + Boolean determining if jacobians are reused within integrator. + Default: False + """ + return self.__sim_method_jac_reuse + + @property + def qp_solver_tol_stat(self): + """ + QP solver stationarity tolerance. + Default: :code:`None` + """ + return self.__qp_solver_tol_stat + + @property + def qp_solver_tol_eq(self): + """ + QP solver equality tolerance. + Default: :code:`None` + """ + return self.__qp_solver_tol_eq + + @property + def qp_solver_tol_ineq(self): + """ + QP solver inequality. + Default: :code:`None` + """ + return self.__qp_solver_tol_ineq + + @property + def qp_solver_tol_comp(self): + """ + QP solver complementarity. + Default: :code:`None` + """ + return self.__qp_solver_tol_comp + + @property + def qp_solver_cond_N(self): + """QP solver: New horizon after partial condensing. + Set to N by default -> no condensing.""" + return self.__qp_solver_cond_N + + @property + def qp_solver_warm_start(self): + """QP solver: Warm starting. + 0: no warm start; 1: warm start; 2: hot start.""" + return self.__qp_solver_warm_start + + @property + def qp_solver_iter_max(self): + """ + QP solver: maximum number of iterations. + Type: int > 0 + Default: 50 + """ + return self.__qp_solver_iter_max + + @property + def tol(self): + """ + NLP solver tolerance. Sets or gets the max of :py:attr:`nlp_solver_tol_eq`, + :py:attr:`nlp_solver_tol_ineq`, :py:attr:`nlp_solver_tol_comp` + and :py:attr:`nlp_solver_tol_stat`. + """ + return max([self.__nlp_solver_tol_eq, self.__nlp_solver_tol_ineq,\ + self.__nlp_solver_tol_comp, self.__nlp_solver_tol_stat]) + + @property + def qp_tol(self): + """ + QP solver tolerance. + Sets all of the following at once or gets the max of + :py:attr:`qp_solver_tol_eq`, :py:attr:`qp_solver_tol_ineq`, + :py:attr:`qp_solver_tol_comp` and + :py:attr:`qp_solver_tol_stat`. + """ + return max([self.__qp_solver_tol_eq, self.__qp_solver_tol_ineq,\ + self.__qp_solver_tol_comp, self.__qp_solver_tol_stat]) + + @property + def nlp_solver_tol_stat(self): + """ + NLP solver stationarity tolerance. + Type: float > 0 + Default: 1e-6 + """ + return self.__nlp_solver_tol_stat + + @property + def nlp_solver_tol_eq(self): + """NLP solver equality tolerance""" + return self.__nlp_solver_tol_eq + + @property + def alpha_min(self): + """Minimal step size for globalization MERIT_BACKTRACKING, default: 0.05.""" + return self.__alpha_min + + @property + def alpha_reduction(self): + """Step size reduction factor for globalization MERIT_BACKTRACKING, default: 0.7.""" + return self.__alpha_reduction + + @property + def nlp_solver_tol_ineq(self): + """NLP solver inequality tolerance""" + return self.__nlp_solver_tol_ineq + + @property + def nlp_solver_tol_comp(self): + """NLP solver complementarity tolerance""" + return self.__nlp_solver_tol_comp + + @property + def nlp_solver_max_iter(self): + """ + NLP solver maximum number of iterations. + Type: int > 0 + Default: 100 + """ + return self.__nlp_solver_max_iter + + @property + def time_steps(self): + """ + Vector with time steps between the shooting nodes. Set automatically to uniform discretization if :py:attr:`N` and :py:attr:`tf` are provided. + Default: :code:`None` + """ + return self.__time_steps + + @property + def shooting_nodes(self): + """ + Vector with the shooting nodes, time_steps will be computed from it automatically. + Default: :code:`None` + """ + return self.__shooting_nodes + + @property + def tf(self): + """ + Prediction horizon + Type: float > 0 + Default: :code:`None` + """ + return self.__tf + + @property + def Tsim(self): + """ + Time horizon for one integrator step. Automatically calculated as :py:attr:`tf`/:py:attr:`N`. + Default: :code:`None` + """ + return self.__Tsim + + @property + def print_level(self): + """ + Verbosity of printing. + Type: int >= 0 + Default: 0 + """ + return self.__print_level + + @property + def model_external_shared_lib_dir(self): + """Path to the .so lib""" + return self.__model_external_shared_lib_dir + + @property + def model_external_shared_lib_name(self): + """Name of the .so lib""" + return self.__model_external_shared_lib_name + + @property + def exact_hess_constr(self): + """ + Used in case of hessian_approx == 'EXACT'.\n + Can be used to turn off exact hessian contributions from the constraints module. + """ + return self.__exact_hess_constr + + @property + def exact_hess_cost(self): + """ + Used in case of hessian_approx == 'EXACT'.\n + Can be used to turn off exact hessian contributions from the cost module. + """ + return self.__exact_hess_cost + + @property + def exact_hess_dyn(self): + """ + Used in case of hessian_approx == 'EXACT'.\n + Can be used to turn off exact hessian contributions from the dynamics module. + """ + return self.__exact_hess_dyn + + @property + def ext_cost_num_hess(self): + """ + Determines if custom hessian approximation for cost contribution is used (> 0).\n + Or if hessian contribution is evaluated exactly using CasADi external function (=0 - default). + """ + return self.__ext_cost_num_hess + + @qp_solver.setter + def qp_solver(self, qp_solver): + qp_solvers = ('PARTIAL_CONDENSING_HPIPM', \ + 'FULL_CONDENSING_QPOASES', 'FULL_CONDENSING_HPIPM', \ + 'PARTIAL_CONDENSING_QPDUNES', 'PARTIAL_CONDENSING_OSQP') + if qp_solver in qp_solvers: + self.__qp_solver = qp_solver + else: + raise Exception('Invalid qp_solver value. Possible values are:\n\n' \ + + ',\n'.join(qp_solvers) + '.\n\nYou have: ' + qp_solver + '.\n\nExiting.') + + @regularize_method.setter + def regularize_method(self, regularize_method): + regularize_methods = ('NO_REGULARIZE', 'MIRROR', 'PROJECT', \ + 'PROJECT_REDUC_HESS', 'CONVEXIFY') + if regularize_method in regularize_methods: + self.__regularize_method = regularize_method + else: + raise Exception('Invalid regularize_method value. Possible values are:\n\n' \ + + ',\n'.join(regularize_methods) + '.\n\nYou have: ' + regularize_method + '.\n\nExiting.') + + @hessian_approx.setter + def hessian_approx(self, hessian_approx): + hessian_approxs = ('GAUSS_NEWTON', 'EXACT') + if hessian_approx in hessian_approxs: + self.__hessian_approx = hessian_approx + else: + raise Exception('Invalid hessian_approx value. Possible values are:\n\n' \ + + ',\n'.join(hessian_approxs) + '.\n\nYou have: ' + hessian_approx + '.\n\nExiting.') + + @integrator_type.setter + def integrator_type(self, integrator_type): + integrator_types = ('ERK', 'IRK', 'GNSF', 'DISCRETE') + if integrator_type in integrator_types: + self.__integrator_type = integrator_type + else: + raise Exception('Invalid integrator_type value. Possible values are:\n\n' \ + + ',\n'.join(integrator_types) + '.\n\nYou have: ' + integrator_type + '.\n\nExiting.') + + @tf.setter + def tf(self, tf): + self.__tf = tf + + @time_steps.setter + def time_steps(self, time_steps): + self.__time_steps = time_steps + + @shooting_nodes.setter + def shooting_nodes(self, shooting_nodes): + self.__shooting_nodes = shooting_nodes + + + @Tsim.setter + def Tsim(self, Tsim): + self.__Tsim = Tsim + + @globalization.setter + def globalization(self, globalization): + self.__globalization = globalization + + @alpha_min.setter + def alpha_min(self, alpha_min): + self.__alpha_min = alpha_min + + @alpha_reduction.setter + def alpha_reduction(self, alpha_reduction): + self.__alpha_reduction = alpha_reduction + + @sim_method_num_stages.setter + def sim_method_num_stages(self, sim_method_num_stages): + + # if type(sim_method_num_stages) == int: + # self.__sim_method_num_stages = sim_method_num_stages + # else: + # raise Exception('Invalid sim_method_num_stages value. sim_method_num_stages must be an integer. Exiting.') + + self.__sim_method_num_stages = sim_method_num_stages + + @sim_method_num_steps.setter + def sim_method_num_steps(self, sim_method_num_steps): + + # if type(sim_method_num_steps) == int: + # self.__sim_method_num_steps = sim_method_num_steps + # else: + # raise Exception('Invalid sim_method_num_steps value. sim_method_num_steps must be an integer. Exiting.') + self.__sim_method_num_steps = sim_method_num_steps + + + @sim_method_newton_iter.setter + def sim_method_newton_iter(self, sim_method_newton_iter): + + if type(sim_method_newton_iter) == int: + self.__sim_method_newton_iter = sim_method_newton_iter + else: + raise Exception('Invalid sim_method_newton_iter value. sim_method_newton_iter must be an integer. Exiting.') + + @sim_method_jac_reuse.setter + def sim_method_jac_reuse(self, sim_method_jac_reuse): + if sim_method_jac_reuse in (True, False): + self.__sim_method_jac_reuse = sim_method_jac_reuse + else: + raise Exception('Invalid sim_method_jac_reuse value. sim_method_jac_reuse must be a Boolean.') + + @nlp_solver_type.setter + def nlp_solver_type(self, nlp_solver_type): + nlp_solver_types = ('SQP', 'SQP_RTI') + if nlp_solver_type in nlp_solver_types: + self.__nlp_solver_type = nlp_solver_type + else: + raise Exception('Invalid nlp_solver_type value. Possible values are:\n\n' \ + + ',\n'.join(nlp_solver_types) + '.\n\nYou have: ' + nlp_solver_type + '.\n\nExiting.') + + @nlp_solver_step_length.setter + def nlp_solver_step_length(self, nlp_solver_step_length): + if type(nlp_solver_step_length) == float and nlp_solver_step_length > 0: + self.__nlp_solver_step_length = nlp_solver_step_length + else: + raise Exception('Invalid nlp_solver_step_length value. nlp_solver_step_length must be a positive float. Exiting') + + @levenberg_marquardt.setter + def levenberg_marquardt(self, levenberg_marquardt): + if isinstance(levenberg_marquardt, float) and levenberg_marquardt >= 0: + self.__levenberg_marquardt = levenberg_marquardt + else: + raise Exception('Invalid levenberg_marquardt value. levenberg_marquardt must be a positive float. Exiting') + + @qp_solver_iter_max.setter + def qp_solver_iter_max(self, qp_solver_iter_max): + if isinstance(qp_solver_iter_max, int) and qp_solver_iter_max > 0: + self.__qp_solver_iter_max = qp_solver_iter_max + else: + raise Exception('Invalid qp_solver_iter_max value. qp_solver_iter_max must be a positive int. Exiting') + + @qp_solver_cond_N.setter + def qp_solver_cond_N(self, qp_solver_cond_N): + if isinstance(qp_solver_cond_N, int) and qp_solver_cond_N > 0: + self.__qp_solver_cond_N = qp_solver_cond_N + else: + raise Exception('Invalid qp_solver_cond_N value. qp_solver_cond_N must be a positive int. Exiting') + + @qp_solver_warm_start.setter + def qp_solver_warm_start(self, qp_solver_warm_start): + if qp_solver_warm_start in [0, 1, 2]: + self.__qp_solver_warm_start = qp_solver_warm_start + else: + raise Exception('Invalid qp_solver_warm_start value. qp_solver_warm_start must be 0 or 1 or 2. Exiting') + + @qp_tol.setter + def qp_tol(self, qp_tol): + if isinstance(qp_tol, float) and qp_tol > 0: + self.__qp_solver_tol_eq = qp_tol + self.__qp_solver_tol_ineq = qp_tol + self.__qp_solver_tol_stat = qp_tol + self.__qp_solver_tol_comp = qp_tol + else: + raise Exception('Invalid qp_tol value. qp_tol must be a positive float. Exiting') + + @qp_solver_tol_stat.setter + def qp_solver_tol_stat(self, qp_solver_tol_stat): + if isinstance(qp_solver_tol_stat, float) and qp_solver_tol_stat > 0: + self.__qp_solver_tol_stat = qp_solver_tol_stat + else: + raise Exception('Invalid qp_solver_tol_stat value. qp_solver_tol_stat must be a positive float. Exiting') + + @qp_solver_tol_eq.setter + def qp_solver_tol_eq(self, qp_solver_tol_eq): + if isinstance(qp_solver_tol_eq, float) and qp_solver_tol_eq > 0: + self.__qp_solver_tol_eq = qp_solver_tol_eq + else: + raise Exception('Invalid qp_solver_tol_eq value. qp_solver_tol_eq must be a positive float. Exiting') + + @qp_solver_tol_ineq.setter + def qp_solver_tol_ineq(self, qp_solver_tol_ineq): + if isinstance(qp_solver_tol_ineq, float) and qp_solver_tol_ineq > 0: + self.__qp_solver_tol_ineq = qp_solver_tol_ineq + else: + raise Exception('Invalid qp_solver_tol_ineq value. qp_solver_tol_ineq must be a positive float. Exiting') + + @qp_solver_tol_comp.setter + def qp_solver_tol_comp(self, qp_solver_tol_comp): + if isinstance(qp_solver_tol_comp, float) and qp_solver_tol_comp > 0: + self.__qp_solver_tol_comp = qp_solver_tol_comp + else: + raise Exception('Invalid qp_solver_tol_comp value. qp_solver_tol_comp must be a positive float. Exiting') + + @tol.setter + def tol(self, tol): + if isinstance(tol, float) and tol > 0: + self.__nlp_solver_tol_eq = tol + self.__nlp_solver_tol_ineq = tol + self.__nlp_solver_tol_stat = tol + self.__nlp_solver_tol_comp = tol + else: + raise Exception('Invalid tol value. tol must be a positive float. Exiting') + + @nlp_solver_tol_stat.setter + def nlp_solver_tol_stat(self, nlp_solver_tol_stat): + if isinstance(nlp_solver_tol_stat, float) and nlp_solver_tol_stat > 0: + self.__nlp_solver_tol_stat = nlp_solver_tol_stat + else: + raise Exception('Invalid nlp_solver_tol_stat value. nlp_solver_tol_stat must be a positive float. Exiting') + + @nlp_solver_tol_eq.setter + def nlp_solver_tol_eq(self, nlp_solver_tol_eq): + if isinstance(nlp_solver_tol_eq, float) and nlp_solver_tol_eq > 0: + self.__nlp_solver_tol_eq = nlp_solver_tol_eq + else: + raise Exception('Invalid nlp_solver_tol_eq value. nlp_solver_tol_eq must be a positive float. Exiting') + + @nlp_solver_tol_ineq.setter + def nlp_solver_tol_ineq(self, nlp_solver_tol_ineq): + if isinstance(nlp_solver_tol_ineq, float) and nlp_solver_tol_ineq > 0: + self.__nlp_solver_tol_ineq = nlp_solver_tol_ineq + else: + raise Exception('Invalid nlp_solver_tol_ineq value. nlp_solver_tol_ineq must be a positive float. Exiting') + + @nlp_solver_tol_comp.setter + def nlp_solver_tol_comp(self, nlp_solver_tol_comp): + if isinstance(nlp_solver_tol_comp, float) and nlp_solver_tol_comp > 0: + self.__nlp_solver_tol_comp = nlp_solver_tol_comp + else: + raise Exception('Invalid nlp_solver_tol_comp value. nlp_solver_tol_comp must be a positive float. Exiting') + + @nlp_solver_max_iter.setter + def nlp_solver_max_iter(self, nlp_solver_max_iter): + + if type(nlp_solver_max_iter) == int and nlp_solver_max_iter > 0: + self.__nlp_solver_max_iter = nlp_solver_max_iter + else: + raise Exception('Invalid nlp_solver_max_iter value. nlp_solver_max_iter must be a positive int. Exiting') + + @print_level.setter + def print_level(self, print_level): + if type(print_level) == int and print_level >= 0: + self.__print_level = print_level + else: + raise Exception('Invalid print_level value. print_level takes one of the values >=0. Exiting') + + @model_external_shared_lib_dir.setter + def model_external_shared_lib_dir(self, model_external_shared_lib_dir): + if type(model_external_shared_lib_dir) == str : + self.__model_external_shared_lib_dir = model_external_shared_lib_dir + else: + raise Exception('Invalid model_external_shared_lib_dir value. Str expected.' \ + + '.\n\nYou have: ' + type(model_external_shared_lib_dir) + '.\n\nExiting.') + + @model_external_shared_lib_name.setter + def model_external_shared_lib_name(self, model_external_shared_lib_name): + if type(model_external_shared_lib_name) == str : + if model_external_shared_lib_name[-3:] == '.so' : + raise Exception('Invalid model_external_shared_lib_name value. Remove the .so extension.' \ + + '.\n\nYou have: ' + type(model_external_shared_lib_name) + '.\n\nExiting.') + else : + self.__model_external_shared_lib_name = model_external_shared_lib_name + else: + raise Exception('Invalid model_external_shared_lib_name value. Str expected.' \ + + '.\n\nYou have: ' + type(model_external_shared_lib_name) + '.\n\nExiting.') + + @exact_hess_constr.setter + def exact_hess_constr(self, exact_hess_constr): + if exact_hess_constr in [0, 1]: + self.__exact_hess_constr = exact_hess_constr + else: + raise Exception('Invalid exact_hess_constr value. exact_hess_constr takes one of the values 0, 1. Exiting') + + @exact_hess_cost.setter + def exact_hess_cost(self, exact_hess_cost): + if exact_hess_cost in [0, 1]: + self.__exact_hess_cost = exact_hess_cost + else: + raise Exception('Invalid exact_hess_cost value. exact_hess_cost takes one of the values 0, 1. Exiting') + + @exact_hess_dyn.setter + def exact_hess_dyn(self, exact_hess_dyn): + if exact_hess_dyn in [0, 1]: + self.__exact_hess_dyn = exact_hess_dyn + else: + raise Exception('Invalid exact_hess_dyn value. exact_hess_dyn takes one of the values 0, 1. Exiting') + + @ext_cost_num_hess.setter + def ext_cost_num_hess(self, ext_cost_num_hess): + if ext_cost_num_hess in [0, 1]: + self.__ext_cost_num_hess = ext_cost_num_hess + else: + raise Exception('Invalid ext_cost_num_hess value. ext_cost_num_hess takes one of the values 0, 1. Exiting') + + def set(self, attr, value): + setattr(self, attr, value) + + +class AcadosOcp: + """ + Class containing the full description of the optimal control problem. + This object can be used to create an :py:class:`acados_template.acados_ocp_solver.AcadosOcpSolver`. + + The class has the following properties that can be modified to formulate a specific OCP, see below: + + - :py:attr:`dims` of type :py:class:`acados_template.acados_ocp.AcadosOcpDims` + - :py:attr:`model` of type :py:class:`acados_template.acados_model.AcadosModel` + - :py:attr:`cost` of type :py:class:`acados_template.acados_ocp.AcadosOcpCost` + - :py:attr:`constraints` of type :py:class:`acados_template.acados_ocp.AcadosOcpConstraints` + - :py:attr:`solver_options` of type :py:class:`acados_template.acados_ocp.AcadosOcpOptions` + + - :py:attr:`acados_include_path` (set automatically) + - :py:attr:`acados_lib_path` (set automatically) + - :py:attr:`parameter_values` - used to initialize the parameters (can be changed) + """ + def __init__(self, acados_path=''): + """ + Keyword arguments: + acados_path -- path of your acados installation + """ + if acados_path == '': + acados_path = get_acados_path() + + self.dims = AcadosOcpDims() + """Dimension definitions, type :py:class:`acados_template.acados_ocp.AcadosOcpDims`""" + self.model = AcadosModel() + """Model definitions, type :py:class:`acados_template.acados_model.AcadosModel`""" + self.cost = AcadosOcpCost() + """Cost definitions, type :py:class:`acados_template.acados_ocp.AcadosOcpCost`""" + self.constraints = AcadosOcpConstraints() + """Constraints definitions, type :py:class:`acados_template.acados_ocp.AcadosOcpConstraints`""" + self.solver_options = AcadosOcpOptions() + """Solver Options, type :py:class:`acados_template.acados_ocp.AcadosOcpOptions`""" + + self.acados_include_path = f'{acados_path}/include' + """Path to acados include directory, type: string""" + self.acados_lib_path = f'{acados_path}/lib' + """Path to where acados library is located, type: string""" + + self.__parameter_values = np.array([]) + self.__problem_class = 'OCP' + + self.code_export_directory = 'c_generated_code' + """Path to where code will be exported. Default: `c_generated_code`.""" + + @property + def parameter_values(self): + """:math:`p` - initial values for parameter - can be updated stagewise""" + return self.__parameter_values + + @parameter_values.setter + def parameter_values(self, parameter_values): + if isinstance(parameter_values, np.ndarray): + self.__parameter_values = parameter_values + else: + raise Exception('Invalid parameter_values value. ' + + f'Expected numpy array, got {type(parameter_values)}.') + + def set(self, attr, value): + # tokenize string + tokens = attr.split('_', 1) + if len(tokens) > 1: + setter_to_call = getattr(getattr(self, tokens[0]), 'set') + else: + setter_to_call = getattr(self, 'set') + + setter_to_call(tokens[1], value) + + return diff --git a/pyextra/acados_template/acados_ocp_solver.py b/pyextra/acados_template/acados_ocp_solver.py new file mode 100644 index 0000000000..aa780e1a26 --- /dev/null +++ b/pyextra/acados_template/acados_ocp_solver.py @@ -0,0 +1,1470 @@ +# -*- 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 datetime import datetime + +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 .generate_c_code_discrete_dynamics import generate_c_code_discrete_dynamics +from .generate_c_code_constraint import generate_c_code_constraint +from .generate_c_code_nls_cost import generate_c_code_nls_cost +from .generate_c_code_external_cost import generate_c_code_external_cost +from .acados_ocp import AcadosOcp +from .acados_model import acados_model_strip_casadi_symbolics +from .utils import is_column, is_empty, casadi_length, render_template, acados_class2dict,\ + format_class_dict, ocp_check_against_layout, np_array_to_list, make_model_consistent,\ + set_up_imported_gnsf_model, get_acados_path + + +def make_ocp_dims_consistent(acados_ocp): + dims = acados_ocp.dims + cost = acados_ocp.cost + constraints = acados_ocp.constraints + model = acados_ocp.model + opts = acados_ocp.solver_options + + # nx + if is_column(model.x): + dims.nx = casadi_length(model.x) + else: + raise Exception('model.x should be column vector!') + + # nu + if is_empty(model.u): + dims.nu = 0 + else: + dims.nu = casadi_length(model.u) + + # nz + if is_empty(model.z): + dims.nz = 0 + else: + dims.nz = casadi_length(model.z) + + # np + if is_empty(model.p): + dims.np = 0 + else: + dims.np = casadi_length(model.p) + if acados_ocp.parameter_values.shape[0] != dims.np: + raise Exception('inconsistent dimension np, regarding model.p and parameter_values.' + \ + f'\nGot np = {dims.np}, acados_ocp.parameter_values.shape = {acados_ocp.parameter_values.shape[0]}\n') + + ## cost + # initial stage - if not set, copy fields from path constraints + if cost.cost_type_0 == None: + cost.cost_type_0 = cost.cost_type + cost.W_0 = cost.W + cost.Vx_0 = cost.Vx + cost.Vu_0 = cost.Vu + cost.Vz_0 = cost.Vz + cost.yref_0 = cost.yref + cost.cost_ext_fun_type_0 = cost.cost_ext_fun_type + model.cost_y_expr_0 = model.cost_y_expr + model.cost_expr_ext_cost_0 = model.cost_expr_ext_cost + + if cost.cost_type_0 == 'LINEAR_LS': + ny_0 = cost.W_0.shape[0] + if cost.Vx_0.shape[0] != ny_0 or cost.Vu_0.shape[0] != ny_0: + raise Exception('inconsistent dimension ny_0, regarding W_0, Vx_0, Vu_0.' + \ + f'\nGot W_0[{cost.W_0.shape}], Vx_0[{cost.Vx_0.shape}], Vu_0[{cost.Vu_0.shape}]\n') + if dims.nz != 0 and cost.Vz_0.shape[0] != ny_0: + raise Exception('inconsistent dimension ny_0, regarding W_0, Vx_0, Vu_0, Vz_0.' + \ + f'\nGot W_0[{cost.W_0.shape}], Vx_0[{cost.Vx_0.shape}], Vu_0[{cost.Vu_0.shape}], Vz_0[{cost.Vz_0.shape}]\n') + if cost.Vx_0.shape[1] != dims.nx and ny_0 != 0: + raise Exception('inconsistent dimension: Vx_0 should have nx columns.') + if cost.Vu_0.shape[1] != dims.nu and ny_0 != 0: + raise Exception('inconsistent dimension: Vu_0 should have nu columns.') + if cost.yref_0.shape[0] != ny_0: + raise Exception('inconsistent dimension: regarding W_0, yref_0.' + \ + f'\nGot W_0[{cost.W_0.shape}], yref_0[{cost.yref_0.shape}]\n') + dims.ny_0 = ny_0 + + elif cost.cost_type_0 == 'NONLINEAR_LS': + ny_0 = cost.W_0.shape[0] + if is_empty(model.cost_y_expr_0) and ny_0 != 0: + raise Exception('inconsistent dimension ny_0: regarding W_0, cost_y_expr.') + elif casadi_length(model.cost_y_expr_0) != ny_0: + raise Exception('inconsistent dimension ny_0: regarding W_0, cost_y_expr.') + if cost.yref_0.shape[0] != ny_0: + raise Exception('inconsistent dimension: regarding W_0, yref_0.' + \ + f'\nGot W_0[{cost.W.shape}], yref_0[{cost.yref_0.shape}]\n') + dims.ny_0 = ny_0 + + # path + if cost.cost_type == 'LINEAR_LS': + ny = cost.W.shape[0] + if cost.Vx.shape[0] != ny or cost.Vu.shape[0] != ny: + raise Exception('inconsistent dimension ny, regarding W, Vx, Vu.' + \ + f'\nGot W[{cost.W.shape}], Vx[{cost.Vx.shape}], Vu[{cost.Vu.shape}]\n') + if dims.nz != 0 and cost.Vz.shape[0] != ny: + raise Exception('inconsistent dimension ny, regarding W, Vx, Vu, Vz.' + \ + f'\nGot W[{cost.W.shape}], Vx[{cost.Vx.shape}], Vu[{cost.Vu.shape}], Vz[{cost.Vz.shape}]\n') + if cost.Vx.shape[1] != dims.nx and ny != 0: + raise Exception('inconsistent dimension: Vx should have nx columns.') + if cost.Vu.shape[1] != dims.nu and ny != 0: + raise Exception('inconsistent dimension: Vu should have nu columns.') + if cost.yref.shape[0] != ny: + raise Exception('inconsistent dimension: regarding W, yref.' + \ + f'\nGot W[{cost.W.shape}], yref[{cost.yref.shape}]\n') + dims.ny = ny + + elif cost.cost_type == 'NONLINEAR_LS': + ny = cost.W.shape[0] + if is_empty(model.cost_y_expr) and ny != 0: + raise Exception('inconsistent dimension ny: regarding W, cost_y_expr.') + elif casadi_length(model.cost_y_expr) != ny: + raise Exception('inconsistent dimension ny: regarding W, cost_y_expr.') + if cost.yref.shape[0] != ny: + raise Exception('inconsistent dimension: regarding W, yref.' + \ + f'\nGot W[{cost.W.shape}], yref[{cost.yref.shape}]\n') + dims.ny = ny + + # terminal + if cost.cost_type_e == 'LINEAR_LS': + ny_e = cost.W_e.shape[0] + if cost.Vx_e.shape[0] != ny_e: + raise Exception('inconsistent dimension ny_e: regarding W_e, cost_y_expr_e.' + \ + f'\nGot W_e[{cost.W_e.shape}], Vx_e[{cost.Vx_e.shape}]') + if cost.Vx_e.shape[1] != dims.nx and ny_e != 0: + raise Exception('inconsistent dimension: Vx_e should have nx columns.') + if cost.yref_e.shape[0] != ny_e: + raise Exception('inconsistent dimension: regarding W_e, yref_e.') + dims.ny_e = ny_e + + elif cost.cost_type_e == 'NONLINEAR_LS': + ny_e = cost.W_e.shape[0] + if is_empty(model.cost_y_expr_e) and ny_e != 0: + raise Exception('inconsistent dimension ny_e: regarding W_e, cost_y_expr_e.') + elif casadi_length(model.cost_y_expr_e) != ny_e: + raise Exception('inconsistent dimension ny_e: regarding W_e, cost_y_expr_e.') + if cost.yref_e.shape[0] != ny_e: + raise Exception('inconsistent dimension: regarding W_e, yref_e.') + dims.ny_e = ny_e + + + ## constraints + # initial + if (constraints.lbx_0 == [] and constraints.ubx_0 == []): + dims.nbx_0 = 0 + else: + this_shape = constraints.lbx_0.shape + other_shape = constraints.ubx_0.shape + if not this_shape == other_shape: + raise Exception('lbx_0, ubx_0 have different shapes!') + if not is_column(constraints.lbx_0): + raise Exception('lbx_0, ubx_0 must be column vectors!') + dims.nbx_0 = constraints.lbx_0.size + + if all(constraints.lbx_0 == constraints.ubx_0) and dims.nbx_0 == dims.nx \ + and dims.nbxe_0 == None \ + and (constraints.idxbxe_0.shape == constraints.idxbx_0.shape)\ + and all(constraints.idxbxe_0 == constraints.idxbx_0): + # case: x0 was set: nbx0 are all equlities. + dims.nbxe_0 = dims.nbx_0 + elif dims.nbxe_0 == None: + # case: x0 was not set -> dont assume nbx0 to be equality constraints. + dims.nbxe_0 = 0 + + # path + nbx = constraints.idxbx.shape[0] + if constraints.ubx.shape[0] != nbx or constraints.lbx.shape[0] != nbx: + raise Exception('inconsistent dimension nbx, regarding idxbx, ubx, lbx.') + else: + dims.nbx = nbx + + nbu = constraints.idxbu.shape[0] + if constraints.ubu.shape[0] != nbu or constraints.lbu.shape[0] != nbu: + raise Exception('inconsistent dimension nbu, regarding idxbu, ubu, lbu.') + else: + dims.nbu = nbu + + ng = constraints.lg.shape[0] + if constraints.ug.shape[0] != ng or constraints.C.shape[0] != ng \ + or constraints.D.shape[0] != ng: + raise Exception('inconsistent dimension ng, regarding lg, ug, C, D.') + else: + dims.ng = ng + + if not is_empty(model.con_h_expr): + nh = casadi_length(model.con_h_expr) + else: + nh = 0 + + if constraints.uh.shape[0] != nh or constraints.lh.shape[0] != nh: + raise Exception('inconsistent dimension nh, regarding lh, uh, con_h_expr.') + else: + dims.nh = nh + + if is_empty(model.con_phi_expr): + dims.nphi = 0 + dims.nr = 0 + else: + dims.nphi = casadi_length(model.con_phi_expr) + if is_empty(model.con_r_expr): + raise Exception('convex over nonlinear constraints: con_r_expr but con_phi_expr is nonempty') + else: + dims.nr = casadi_length(model.con_r_expr) + + # terminal + nbx_e = constraints.idxbx_e.shape[0] + if constraints.ubx_e.shape[0] != nbx_e or constraints.lbx_e.shape[0] != nbx_e: + raise Exception('inconsistent dimension nbx_e, regarding idxbx_e, ubx_e, lbx_e.') + else: + dims.nbx_e = nbx_e + + ng_e = constraints.lg_e.shape[0] + if constraints.ug_e.shape[0] != ng_e or constraints.C_e.shape[0] != ng_e: + raise Exception('inconsistent dimension ng_e, regarding_e lg_e, ug_e, C_e.') + else: + dims.ng_e = ng_e + + if not is_empty(model.con_h_expr_e): + nh_e = casadi_length(model.con_h_expr_e) + else: + nh_e = 0 + + if constraints.uh_e.shape[0] != nh_e or constraints.lh_e.shape[0] != nh_e: + raise Exception('inconsistent dimension nh_e, regarding lh_e, uh_e, con_h_expr_e.') + else: + dims.nh_e = nh_e + + if is_empty(model.con_phi_expr_e): + dims.nphi_e = 0 + dims.nr_e = 0 + else: + dims.nphi_e = casadi_length(model.con_phi_expr_e) + if is_empty(model.con_r_expr_e): + raise Exception('convex over nonlinear constraints: con_r_expr_e but con_phi_expr_e is nonempty') + else: + dims.nr_e = casadi_length(model.con_r_expr_e) + + # Slack dimensions + nsbx = constraints.idxsbx.shape[0] + if is_empty(constraints.lsbx): + constraints.lsbx = np.zeros((nsbx,)) + elif constraints.lsbx.shape[0] != nsbx: + raise Exception('inconsistent dimension nsbx, regarding idxsbx, lsbx.') + if is_empty(constraints.usbx): + constraints.usbx = np.zeros((nsbx,)) + elif constraints.usbx.shape[0] != nsbx: + raise Exception('inconsistent dimension nsbx, regarding idxsbx, usbx.') + dims.nsbx = nsbx + + nsbu = constraints.idxsbu.shape[0] + if is_empty(constraints.lsbu): + constraints.lsbu = np.zeros((nsbu,)) + elif constraints.lsbu.shape[0] != nsbu: + raise Exception('inconsistent dimension nsbu, regarding idxsbu, lsbu.') + if is_empty(constraints.usbu): + constraints.usbu = np.zeros((nsbu,)) + elif constraints.usbu.shape[0] != nsbu: + raise Exception('inconsistent dimension nsbu, regarding idxsbu, usbu.') + dims.nsbu = nsbu + + nsh = constraints.idxsh.shape[0] + if is_empty(constraints.lsh): + constraints.lsh = np.zeros((nsh,)) + elif constraints.lsh.shape[0] != nsh: + raise Exception('inconsistent dimension nsh, regarding idxsh, lsh.') + if is_empty(constraints.ush): + constraints.ush = np.zeros((nsh,)) + elif constraints.ush.shape[0] != nsh: + raise Exception('inconsistent dimension nsh, regarding idxsh, ush.') + dims.nsh = nsh + + nsphi = constraints.idxsphi.shape[0] + if is_empty(constraints.lsphi): + constraints.lsphi = np.zeros((nsphi,)) + elif constraints.lsphi.shape[0] != nsphi: + raise Exception('inconsistent dimension nsphi, regarding idxsphi, lsphi.') + if is_empty(constraints.usphi): + constraints.usphi = np.zeros((nsphi,)) + elif constraints.usphi.shape[0] != nsphi: + raise Exception('inconsistent dimension nsphi, regarding idxsphi, usphi.') + dims.nsphi = nsphi + + nsg = constraints.idxsg.shape[0] + if is_empty(constraints.lsg): + constraints.lsg = np.zeros((nsg,)) + elif constraints.lsg.shape[0] != nsg: + raise Exception('inconsistent dimension nsg, regarding idxsg, lsg.') + if is_empty(constraints.usg): + constraints.usg = np.zeros((nsg,)) + elif constraints.usg.shape[0] != nsg: + raise Exception('inconsistent dimension nsg, regarding idxsg, usg.') + dims.nsg = nsg + + ns = nsbx + nsbu + nsh + nsg + nsphi + wrong_field = "" + if cost.Zl.shape[0] != ns: + wrong_field = "Zl" + dim = cost.Zl.shape[0] + elif cost.Zu.shape[0] != ns: + wrong_field = "Zu" + dim = cost.Zu.shape[0] + elif cost.zl.shape[0] != ns: + wrong_field = "zl" + dim = cost.zl.shape[0] + elif cost.zu.shape[0] != ns: + wrong_field = "zu" + dim = cost.zu.shape[0] + + if wrong_field != "": + raise Exception(f'Inconsistent size for field {wrong_field}, with dimension {dim}, \n\t'\ + + f'Detected ns = {ns} = nsbx + nsbu + nsg + nsh + nsphi.\n\t'\ + + f'With nsbx = {nsbx}, nsbu = {nsbu}, nsg = {nsg}, nsh = {nsh}, nsphi = {nsphi}') + + dims.ns = ns + + nsbx_e = constraints.idxsbx_e.shape[0] + if is_empty(constraints.lsbx_e): + constraints.lsbx_e = np.zeros((nsbx_e,)) + elif constraints.lsbx_e.shape[0] != nsbx_e: + raise Exception('inconsistent dimension nsbx_e, regarding idxsbx_e, lsbx_e.') + if is_empty(constraints.usbx_e): + constraints.usbx_e = np.zeros((nsbx_e,)) + elif constraints.usbx_e.shape[0] != nsbx_e: + raise Exception('inconsistent dimension nsbx_e, regarding idxsbx_e, usbx_e.') + dims.nsbx_e = nsbx_e + + nsh_e = constraints.idxsh_e.shape[0] + if is_empty(constraints.lsh_e): + constraints.lsh_e = np.zeros((nsh_e,)) + elif constraints.lsh_e.shape[0] != nsh_e: + raise Exception('inconsistent dimension nsh_e, regarding idxsh_e, lsh_e.') + if is_empty(constraints.ush_e): + constraints.ush_e = np.zeros((nsh_e,)) + elif constraints.ush_e.shape[0] != nsh_e: + raise Exception('inconsistent dimension nsh_e, regarding idxsh_e, ush_e.') + dims.nsh_e = nsh_e + + nsg_e = constraints.idxsg_e.shape[0] + if is_empty(constraints.lsg_e): + constraints.lsg_e = np.zeros((nsg_e,)) + elif constraints.lsg_e.shape[0] != nsg_e: + raise Exception('inconsistent dimension nsg_e, regarding idxsg_e, lsg_e.') + if is_empty(constraints.usg_e): + constraints.usg_e = np.zeros((nsg_e,)) + elif constraints.usg_e.shape[0] != nsg_e: + raise Exception('inconsistent dimension nsg_e, regarding idxsg_e, usg_e.') + dims.nsg_e = nsg_e + + nsphi_e = constraints.idxsphi_e.shape[0] + if is_empty(constraints.lsphi_e): + constraints.lsphi_e = np.zeros((nsphi_e,)) + elif constraints.lsphi_e.shape[0] != nsphi_e: + raise Exception('inconsistent dimension nsphi_e, regarding idxsphi_e, lsphi_e.') + if is_empty(constraints.usphi_e): + constraints.usphi_e = np.zeros((nsphi_e,)) + elif constraints.usphi_e.shape[0] != nsphi_e: + raise Exception('inconsistent dimension nsphi_e, regarding idxsphi_e, usphi_e.') + dims.nsphi_e = nsphi_e + + # terminal + ns_e = nsbx_e + nsh_e + nsg_e + nsphi_e + wrong_field = "" + if cost.Zl_e.shape[0] != ns_e: + wrong_field = "Zl_e" + dim = cost.Zl_e.shape[0] + elif cost.Zu_e.shape[0] != ns_e: + wrong_field = "Zu_e" + dim = cost.Zu_e.shape[0] + elif cost.zl_e.shape[0] != ns_e: + wrong_field = "zl_e" + dim = cost.zl_e.shape[0] + elif cost.zu_e.shape[0] != ns_e: + wrong_field = "zu_e" + dim = cost.zu_e.shape[0] + + if wrong_field != "": + raise Exception(f'Inconsistent size for field {wrong_field}, with dimension {dim}, \n\t'\ + + f'Detected ns_e = {ns_e} = nsbx_e + nsg_e + nsh_e + nsphi_e.\n\t'\ + + f'With nsbx_e = {nsbx_e}, nsg_e = {nsg_e}, nsh_e = {nsh_e}, nsphi_e = {nsphi_e}') + + dims.ns_e = ns_e + + # discretization + if is_empty(opts.time_steps) and is_empty(opts.shooting_nodes): + # uniform discretization + opts.time_steps = opts.tf / dims.N * np.ones((dims.N,)) + + elif not is_empty(opts.shooting_nodes): + if np.shape(opts.shooting_nodes)[0] != dims.N+1: + raise Exception('inconsistent dimension N, regarding shooting_nodes.') + + time_steps = np.zeros((dims.N,)) + for i in range(dims.N): + time_steps[i] = opts.shooting_nodes[i+1] - opts.shooting_nodes[i] + opts.time_steps = time_steps + + elif (not is_empty(opts.time_steps)) and (not is_empty(opts.shooting_nodes)): + Exception('Please provide either time_steps or shooting_nodes for nonuniform discretization') + + tf = np.sum(opts.time_steps) + if (tf - opts.tf) / tf > 1e-15: + raise Exception(f'Inconsistent discretization: {opts.tf}'\ + f' = tf != sum(opts.time_steps) = {tf}.') + + # num_steps + if isinstance(opts.sim_method_num_steps, np.ndarray) and opts.sim_method_num_steps.size == 1: + opts.sim_method_num_steps = opts.sim_method_num_steps.item() + + if isinstance(opts.sim_method_num_steps, (int, float)) and opts.sim_method_num_steps % 1 == 0: + opts.sim_method_num_steps = opts.sim_method_num_steps * np.ones((dims.N,), dtype=np.int64) + elif isinstance(opts.sim_method_num_steps, np.ndarray) and opts.sim_method_num_steps.size == dims.N \ + and np.all(np.equal(np.mod(opts.sim_method_num_steps, 1), 0)): + opts.sim_method_num_steps = np.reshape(opts.sim_method_num_steps, (dims.N,)).astype(np.int64) + else: + raise Exception("Wrong value for sim_method_num_steps. Should be either int or array of ints of shape (N,).") + + # num_stages + if isinstance(opts.sim_method_num_stages, np.ndarray) and opts.sim_method_num_stages.size == 1: + opts.sim_method_num_stages = opts.sim_method_num_stages.item() + + if isinstance(opts.sim_method_num_stages, (int, float)) and opts.sim_method_num_stages % 1 == 0: + opts.sim_method_num_stages = opts.sim_method_num_stages * np.ones((dims.N,), dtype=np.int64) + elif isinstance(opts.sim_method_num_stages, np.ndarray) and opts.sim_method_num_stages.size == dims.N \ + and np.all(np.equal(np.mod(opts.sim_method_num_stages, 1), 0)): + opts.sim_method_num_stages = np.reshape(opts.sim_method_num_stages, (dims.N,)).astype(np.int64) + else: + raise Exception("Wrong value for sim_method_num_stages. Should be either int or array of ints of shape (N,).") + + + + +def get_ocp_nlp_layout(): + current_module = sys.modules[__name__] + acados_path = os.path.dirname(current_module.__file__) + with open(acados_path + '/acados_layout.json', 'r') as f: + ocp_nlp_layout = json.load(f) + return ocp_nlp_layout + + +def ocp_formulation_json_dump(acados_ocp, simulink_opts, json_file='acados_ocp_nlp.json'): + # Load acados_ocp_nlp structure description + ocp_layout = get_ocp_nlp_layout() + + # Copy input ocp object dictionary + ocp_nlp_dict = dict(deepcopy(acados_ocp).__dict__) + # TODO: maybe make one function with formatting + + for acados_struct, v in ocp_layout.items(): + # skip non dict attributes + if not isinstance(v, dict): continue + # setattr(ocp_nlp, acados_struct, dict(getattr(acados_ocp, acados_struct).__dict__)) + # Copy ocp object attributes dictionaries + ocp_nlp_dict[acados_struct]=dict(getattr(acados_ocp, acados_struct).__dict__) + + ocp_nlp_dict = format_class_dict(ocp_nlp_dict) + + # strip symbolics + ocp_nlp_dict['model'] = acados_model_strip_casadi_symbolics(ocp_nlp_dict['model']) + + # strip shooting_nodes + ocp_nlp_dict['solver_options'].pop('shooting_nodes', None) + + dims_dict = acados_class2dict(acados_ocp.dims) + + ocp_check_against_layout(ocp_nlp_dict, dims_dict) + + # add simulink options + ocp_nlp_dict['simulink_opts'] = simulink_opts + + with open(json_file, 'w') as f: + json.dump(ocp_nlp_dict, f, default=np_array_to_list, indent=4, sort_keys=True) + + + +def ocp_formulation_json_load(json_file='acados_ocp_nlp.json'): + # Load acados_ocp_nlp structure description + ocp_layout = get_ocp_nlp_layout() + + with open(json_file, 'r') as f: + ocp_nlp_json = json.load(f) + + ocp_nlp_dict = json2dict(ocp_nlp_json, ocp_nlp_json['dims']) + + # Instantiate AcadosOcp object + acados_ocp = AcadosOcp() + + # load class dict + acados_ocp.__dict__ = ocp_nlp_dict + + # laod class attributes dict, dims, constraints, etc + for acados_struct, v in ocp_layout.items(): + # skip non dict attributes + if not isinstance(v, dict): continue + acados_attribute = getattr(acados_ocp, acados_struct) + acados_attribute.__dict__ = ocp_nlp_dict[acados_struct] + setattr(acados_ocp, acados_struct, acados_attribute) + + return acados_ocp + + +def ocp_generate_external_functions(acados_ocp, model): + + model = make_model_consistent(model) + + if acados_ocp.solver_options.hessian_approx == 'EXACT': + opts = dict(generate_hess=1) + else: + opts = dict(generate_hess=0) + code_export_dir = acados_ocp.code_export_directory + opts['code_export_directory'] = code_export_dir + + if acados_ocp.model.dyn_ext_fun_type != 'casadi': + raise Exception("ocp_generate_external_functions: dyn_ext_fun_type only supports 'casadi' for now.\ + Extending the Python interface with generic function support is welcome.") + + if acados_ocp.solver_options.integrator_type == 'ERK': + # explicit model -- generate C code + generate_c_code_explicit_ode(model, opts) + elif acados_ocp.solver_options.integrator_type == 'IRK': + # implicit model -- generate C code + generate_c_code_implicit_ode(model, opts) + elif acados_ocp.solver_options.integrator_type == 'GNSF': + generate_c_code_gnsf(model, opts) + elif acados_ocp.solver_options.integrator_type == 'DISCRETE': + generate_c_code_discrete_dynamics(model, opts) + else: + raise Exception("ocp_generate_external_functions: unknown integrator type.") + + if acados_ocp.dims.nphi > 0 or acados_ocp.dims.nh > 0: + generate_c_code_constraint(model, model.name, False, opts) + + if acados_ocp.dims.nphi_e > 0 or acados_ocp.dims.nh_e > 0: + generate_c_code_constraint(model, model.name, True, opts) + + # dummy matrices + if not acados_ocp.cost.cost_type_0 == 'LINEAR_LS': + acados_ocp.cost.Vx_0 = np.zeros((acados_ocp.dims.ny_0, acados_ocp.dims.nx)) + acados_ocp.cost.Vu_0 = np.zeros((acados_ocp.dims.ny_0, acados_ocp.dims.nu)) + if not acados_ocp.cost.cost_type == 'LINEAR_LS': + acados_ocp.cost.Vx = np.zeros((acados_ocp.dims.ny, acados_ocp.dims.nx)) + acados_ocp.cost.Vu = np.zeros((acados_ocp.dims.ny, acados_ocp.dims.nu)) + if not acados_ocp.cost.cost_type_e == 'LINEAR_LS': + acados_ocp.cost.Vx_e = np.zeros((acados_ocp.dims.ny_e, acados_ocp.dims.nx)) + + if acados_ocp.cost.cost_type_0 == 'NONLINEAR_LS': + generate_c_code_nls_cost(model, model.name, 'initial', opts) + elif acados_ocp.cost.cost_type_0 == 'EXTERNAL': + generate_c_code_external_cost(model, 'initial', opts) + + if acados_ocp.cost.cost_type == 'NONLINEAR_LS': + generate_c_code_nls_cost(model, model.name, 'path', opts) + elif acados_ocp.cost.cost_type == 'EXTERNAL': + generate_c_code_external_cost(model, 'path', opts) + + if acados_ocp.cost.cost_type_e == 'NONLINEAR_LS': + generate_c_code_nls_cost(model, model.name, 'terminal', opts) + elif acados_ocp.cost.cost_type_e == 'EXTERNAL': + generate_c_code_external_cost(model, 'terminal', opts) + + +def ocp_render_templates(acados_ocp, json_file): + + name = acados_ocp.model.name + + # setting up loader and environment + json_path = '{cwd}/{json_file}'.format( + cwd=os.getcwd(), + json_file=json_file) + + if not os.path.exists(json_path): + raise Exception('{} not found!'.format(json_path)) + + code_export_dir = acados_ocp.code_export_directory + template_dir = code_export_dir + + ## Render templates + in_file = 'main.in.c' + out_file = f'main_{name}.c' + render_template(in_file, out_file, template_dir, json_path) + + in_file = 'acados_solver.in.c' + out_file = f'acados_solver_{name}.c' + render_template(in_file, out_file, template_dir, json_path) + + in_file = 'acados_solver.in.h' + out_file = f'acados_solver_{name}.h' + render_template(in_file, out_file, template_dir, json_path) + + in_file = 'Makefile.in' + out_file = 'Makefile' + render_template(in_file, out_file, template_dir, json_path) + + in_file = 'acados_solver_sfun.in.c' + out_file = f'acados_solver_sfunction_{name}.c' + render_template(in_file, out_file, template_dir, json_path) + + in_file = 'make_sfun.in.m' + out_file = f'make_sfun_{name}.m' + render_template(in_file, out_file, template_dir, json_path) + + # sim + in_file = 'acados_sim_solver.in.c' + out_file = f'acados_sim_solver_{name}.c' + render_template(in_file, out_file, template_dir, json_path) + + in_file = 'acados_sim_solver.in.h' + out_file = f'acados_sim_solver_{name}.h' + render_template(in_file, out_file, template_dir, json_path) + + in_file = 'main_sim.in.c' + out_file = f'main_sim_{name}.c' + render_template(in_file, out_file, template_dir, json_path) + + ## folder model + template_dir = f'{code_export_dir}/{name}_model/' + + in_file = 'model.in.h' + out_file = f'{name}_model.h' + render_template(in_file, out_file, template_dir, json_path) + + # constraints on convex over nonlinear function + if acados_ocp.constraints.constr_type == 'BGP' and acados_ocp.dims.nphi > 0: + # constraints on outer function + template_dir = f'{code_export_dir}/{name}_constraints/' + in_file = 'phi_constraint.in.h' + out_file = f'{name}_phi_constraint.h' + render_template(in_file, out_file, template_dir, json_path) + + # terminal constraints on convex over nonlinear function + if acados_ocp.constraints.constr_type_e == 'BGP' and acados_ocp.dims.nphi_e > 0: + # terminal constraints on outer function + template_dir = f'{code_export_dir}/{name}_constraints/' + in_file = 'phi_e_constraint.in.h' + out_file = f'{name}_phi_e_constraint.h' + render_template(in_file, out_file, template_dir, json_path) + + # nonlinear constraints + if acados_ocp.constraints.constr_type == 'BGH' and acados_ocp.dims.nh > 0: + template_dir = f'{code_export_dir}/{name}_constraints/' + in_file = 'h_constraint.in.h' + out_file = f'{name}_h_constraint.h' + render_template(in_file, out_file, template_dir, json_path) + + # terminal nonlinear constraints + if acados_ocp.constraints.constr_type_e == 'BGH' and acados_ocp.dims.nh_e > 0: + template_dir = f'{code_export_dir}/{name}_constraints/' + in_file = 'h_e_constraint.in.h' + out_file = f'{name}_h_e_constraint.h' + render_template(in_file, out_file, template_dir, json_path) + + # initial stage Nonlinear LS cost function + if acados_ocp.cost.cost_type_0 == 'NONLINEAR_LS': + template_dir = f'{code_export_dir}/{name}_cost/' + in_file = 'cost_y_0_fun.in.h' + out_file = f'{name}_cost_y_0_fun.h' + render_template(in_file, out_file, template_dir, json_path) + # external cost - terminal + elif acados_ocp.cost.cost_type_0 == 'EXTERNAL': + template_dir = f'{code_export_dir}/{name}_cost/' + in_file = 'external_cost_0.in.h' + out_file = f'{name}_external_cost_0.h' + render_template(in_file, out_file, template_dir, json_path) + + # path Nonlinear LS cost function + if acados_ocp.cost.cost_type == 'NONLINEAR_LS': + template_dir = f'{code_export_dir}/{name}_cost/' + in_file = 'cost_y_fun.in.h' + out_file = f'{name}_cost_y_fun.h' + render_template(in_file, out_file, template_dir, json_path) + + # terminal Nonlinear LS cost function + if acados_ocp.cost.cost_type_e == 'NONLINEAR_LS': + template_dir = f'{code_export_dir}/{name}_cost/' + in_file = 'cost_y_e_fun.in.h' + out_file = f'{name}_cost_y_e_fun.h' + render_template(in_file, out_file, template_dir, json_path) + + # external cost + if acados_ocp.cost.cost_type == 'EXTERNAL': + template_dir = f'{code_export_dir}/{name}_cost/' + in_file = 'external_cost.in.h' + out_file = f'{name}_external_cost.h' + render_template(in_file, out_file, template_dir, json_path) + + # external cost - terminal + if acados_ocp.cost.cost_type_e == 'EXTERNAL': + template_dir = f'{code_export_dir}/{name}_cost/' + in_file = 'external_cost_e.in.h' + out_file = f'{name}_external_cost_e.h' + render_template(in_file, out_file, template_dir, json_path) + + +def remove_x0_elimination(acados_ocp): + acados_ocp.constraints.idxbxe_0 = np.zeros((0,)) + acados_ocp.dims.nbxe_0 = 0 + + +class AcadosOcpSolver: + """ + Class to interact with the acados ocp solver C object. + + :param acados_ocp: type AcadosOcp - description of the OCP for acados + :param json_file: name for the json file used to render the templated code - default: acados_ocp_nlp.json + :param simulink_opts: Options to configure Simulink S-function blocks, mainly to activate possible Inputs and Outputs + :param build: Option to disable rendering templates and compiling if previously built - default: True + """ + if sys.platform=="win32": + from ctypes import wintypes + dlclose = WinDLL('kernel32', use_last_error=True).FreeLibrary + dlclose.argtypes = [wintypes.HMODULE] + else: + dlclose = CDLL(None).dlclose + dlclose.argtypes = [c_void_p] + + def __init__(self, acados_ocp, json_file='acados_ocp_nlp.json', simulink_opts=None, build=True): + + self.solver_created = False + self.N = acados_ocp.dims.N + model = acados_ocp.model + + if simulink_opts == None: + acados_path = get_acados_path() + json_path = os.path.join(acados_path, 'interfaces/acados_template/acados_template') + with open(json_path + '/simulink_default_opts.json', 'r') as f: + simulink_opts = json.load(f) + + # make dims consistent + make_ocp_dims_consistent(acados_ocp) + + # module dependent post processing + if acados_ocp.solver_options.integrator_type == 'GNSF': + set_up_imported_gnsf_model(acados_ocp) + + if acados_ocp.solver_options.qp_solver == 'PARTIAL_CONDENSING_QPDUNES': + remove_x0_elimination(acados_ocp) + + # set integrator time automatically + acados_ocp.solver_options.Tsim = acados_ocp.solver_options.time_steps[0] + + # generate external functions + ocp_generate_external_functions(acados_ocp, model) + + # dump to json + ocp_formulation_json_dump(acados_ocp, simulink_opts, json_file) + + code_export_dir = acados_ocp.code_export_directory + if build: + # render templates + ocp_render_templates(acados_ocp, json_file) + + ## Compile solver + cwd = os.getcwd() + os.chdir(code_export_dir) + os.system('make clean_ocp_shared_lib') + os.system('make ocp_shared_lib') + os.chdir(cwd) + + self.shared_lib_name = f'{code_export_dir}/libacados_ocp_solver_{model.name}.so' + + # get shared_lib + self.shared_lib = CDLL(self.shared_lib_name) + + # create capsule + getattr(self.shared_lib, f"{model.name}_acados_create_capsule").restype = c_void_p + self.capsule = getattr(self.shared_lib, f"{model.name}_acados_create_capsule")() + + # create solver + getattr(self.shared_lib, f"{model.name}_acados_create").argtypes = [c_void_p] + getattr(self.shared_lib, f"{model.name}_acados_create").restype = c_int + assert getattr(self.shared_lib, f"{model.name}_acados_create")(self.capsule)==0 + self.solver_created = True + + # get pointers solver + getattr(self.shared_lib, f"{model.name}_acados_get_nlp_opts").argtypes = [c_void_p] + getattr(self.shared_lib, f"{model.name}_acados_get_nlp_opts").restype = c_void_p + self.nlp_opts = getattr(self.shared_lib, f"{model.name}_acados_get_nlp_opts")(self.capsule) + + getattr(self.shared_lib, f"{model.name}_acados_get_nlp_dims").argtypes = [c_void_p] + getattr(self.shared_lib, f"{model.name}_acados_get_nlp_dims").restype = c_void_p + self.nlp_dims = getattr(self.shared_lib, f"{model.name}_acados_get_nlp_dims")(self.capsule) + + getattr(self.shared_lib, f"{model.name}_acados_get_nlp_config").argtypes = [c_void_p] + getattr(self.shared_lib, f"{model.name}_acados_get_nlp_config").restype = c_void_p + self.nlp_config = getattr(self.shared_lib, f"{model.name}_acados_get_nlp_config")(self.capsule) + + getattr(self.shared_lib, f"{model.name}_acados_get_nlp_out").argtypes = [c_void_p] + getattr(self.shared_lib, f"{model.name}_acados_get_nlp_out").restype = c_void_p + self.nlp_out = getattr(self.shared_lib, f"{model.name}_acados_get_nlp_out")(self.capsule) + + getattr(self.shared_lib, f"{model.name}_acados_get_nlp_in").argtypes = [c_void_p] + getattr(self.shared_lib, f"{model.name}_acados_get_nlp_in").restype = c_void_p + self.nlp_in = getattr(self.shared_lib, f"{model.name}_acados_get_nlp_in")(self.capsule) + + getattr(self.shared_lib, f"{model.name}_acados_get_nlp_solver").argtypes = [c_void_p] + getattr(self.shared_lib, f"{model.name}_acados_get_nlp_solver").restype = c_void_p + self.nlp_solver = getattr(self.shared_lib, f"{model.name}_acados_get_nlp_solver")(self.capsule) + + self.acados_ocp = acados_ocp + + + def solve(self): + """ + Solve the ocp with current input. + """ + model = self.acados_ocp.model + + getattr(self.shared_lib, f"{model.name}_acados_solve").argtypes = [c_void_p] + getattr(self.shared_lib, f"{model.name}_acados_solve").restype = c_int + status = getattr(self.shared_lib, f"{model.name}_acados_solve")(self.capsule) + return status + + + def get(self, stage_, field_): + """ + Get the last solution of the solver: + + :param stage: integer corresponding to shooting node + :param field: string in ['x', 'u', 'z', 'pi', 'lam', 't', 'sl', 'su',] + + .. note:: regarding lam, t: \n + the inequalities are internally organized in the following order: \n + [ lbu lbx lg lh lphi ubu ubx ug uh uphi; \n + lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi] + + .. note:: pi: multipliers for dynamics equality constraints \n + lam: multipliers for inequalities \n + t: slack variables corresponding to evaluation of all inequalities (at the solution) \n + sl: slack variables of soft lower inequality constraints \n + su: slack variables of soft upper inequality constraints \n + """ + + out_fields = ['x', 'u', 'z', 'pi', 'lam', 't'] + mem_fields = ['sl', 'su'] + field = field_ + field = field.encode('utf-8') + + if (field_ not in out_fields + mem_fields): + raise Exception('AcadosOcpSolver.get(): {} is an invalid argument.\ + \n Possible values are {}. Exiting.'.format(field_, out_fields + mem_fields)) + + if not isinstance(stage_, int): + raise Exception('AcadosOcpSolver.get(): stage index must be Integer.') + + if stage_ < 0 or stage_ > self.N: + raise Exception('AcadosOcpSolver.get(): stage index must be in [0, N], got: {}.'.format(self.N)) + + if stage_ == self.N and field_ == 'pi': + raise Exception('AcadosOcpSolver.get(): field {} does not exist at final stage {}.'\ + .format(field_, stage_)) + + self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = \ + [c_void_p, c_void_p, c_void_p, c_int, c_char_p] + self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int + + dims = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, \ + self.nlp_dims, self.nlp_out, stage_, field) + + out = np.ascontiguousarray(np.zeros((dims,)), dtype=np.float64) + out_data = cast(out.ctypes.data, POINTER(c_double)) + + if (field_ in out_fields): + self.shared_lib.ocp_nlp_out_get.argtypes = \ + [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] + self.shared_lib.ocp_nlp_out_get(self.nlp_config, \ + self.nlp_dims, self.nlp_out, stage_, field, out_data) + elif field_ in mem_fields: + self.shared_lib.ocp_nlp_get_at_stage.argtypes = \ + [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] + self.shared_lib.ocp_nlp_get_at_stage(self.nlp_config, \ + self.nlp_dims, self.nlp_solver, stage_, field, out_data) + + 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 + """ + stat = self.get_stats("statistics") + + if self.acados_ocp.solver_options.nlp_solver_type == 'SQP': + print('\niter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter') + if stat.shape[0]>7: + print('\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp') + for jj in range(stat.shape[1]): + print('{:d}\t{:e}\t{:e}\t{:e}\t{:e}\t{:d}\t{:d}'.format( \ + int(stat[0][jj]), stat[1][jj], stat[2][jj], \ + stat[3][jj], stat[4][jj], int(stat[5][jj]), int(stat[6][jj]))) + if stat.shape[0]>7: + print('\t{:e}\t{:e}\t{:e}\t{:e}'.format( \ + stat[7][jj], stat[8][jj], stat[9][jj], stat[10][jj])) + print('\n') + elif self.acados_ocp.solver_options.nlp_solver_type == 'SQP_RTI': + print('\niter\tqp_stat\tqp_iter') + if stat.shape[0]>3: + print('\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp') + for jj in range(stat.shape[1]): + print('{:d}\t{:d}\t{:d}'.format( int(stat[0][jj]), int(stat[1][jj]), int(stat[2][jj]))) + if stat.shape[0]>3: + print('\t{:e}\t{:e}\t{:e}\t{:e}'.format( \ + stat[3][jj], stat[4][jj], stat[5][jj], stat[6][jj])) + print('\n') + + return + + + def store_iterate(self, filename='', overwrite=False): + """ + Stores the current iterate of the ocp solver in a json file. + + :param filename: if not set, use model_name + timestamp + '.json' + :param overwrite: if false and filename exists add timestamp to filename + """ + if filename == '': + filename += self.acados_ocp.model.name + '_' + 'iterate' + '.json' + + if not overwrite: + # append timestamp + if os.path.isfile(filename): + filename = filename[:-5] + filename += datetime.utcnow().strftime('%Y-%m-%d-%H:%M:%S.%f') + '.json' + + # get iterate: + solution = dict() + + for i in range(self.N+1): + solution['x_'+str(i)] = self.get(i,'x') + solution['u_'+str(i)] = self.get(i,'u') + solution['z_'+str(i)] = self.get(i,'z') + solution['lam_'+str(i)] = self.get(i,'lam') + solution['t_'+str(i)] = self.get(i, 't') + solution['sl_'+str(i)] = self.get(i, 'sl') + solution['su_'+str(i)] = self.get(i, 'su') + for i in range(self.N): + solution['pi_'+str(i)] = self.get(i,'pi') + + # save + with open(filename, 'w') as f: + json.dump(solution, f, default=np_array_to_list, indent=4, sort_keys=True) + print("stored current iterate in ", os.path.join(os.getcwd(), filename)) + + + def load_iterate(self, filename): + """ + Loads the iterate stored in json file with filename into the ocp solver. + """ + if not os.path.isfile(filename): + raise Exception('load_iterate: failed, file does not exist: ' + os.path.join(os.getcwd(), filename)) + + with open(filename, 'r') as f: + solution = json.load(f) + + 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'] + """ + + fields = ['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_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', + 'stat_n', + ] + + field = field_ + field = field.encode('utf-8') + if (field_ not in fields): + raise Exception('AcadosOcpSolver.get_stats(): {} is not a valid argument.\ + \n Possible values are {}. Exiting.'.format(fields, fields)) + + if field_ in ['sqp_iter', 'stat_m', 'stat_n']: + out = np.ascontiguousarray(np.zeros((1,)), dtype=np.int64) + out_data = cast(out.ctypes.data, POINTER(c_int64)) + + elif field_ == 'statistics': + sqp_iter = self.get_stats("sqp_iter") + stat_m = self.get_stats("stat_m") + stat_n = self.get_stats("stat_n") + + min_size = min([stat_m, sqp_iter+1]) + + out = np.ascontiguousarray( + np.zeros( (stat_n[0]+1, min_size[0]) ), dtype=np.float64) + out_data = cast(out.ctypes.data, POINTER(c_double)) + + elif field_ == 'qp_iter': + full_stats = self.get_stats('statistics') + if self.acados_ocp.solver_options.nlp_solver_type == 'SQP': + out = full_stats[6, :] + elif self.acados_ocp.solver_options.nlp_solver_type == 'SQP_RTI': + out = full_stats[2, :] + + else: + out = np.ascontiguousarray(np.zeros((1,)), dtype=np.float64) + out_data = cast(out.ctypes.data, POINTER(c_double)) + + if not field_ == 'qp_iter': + self.shared_lib.ocp_nlp_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p] + self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data) + + return out + + + def get_cost(self): + """ + Returns the cost value of the current solution. + """ + # compute cost internally + self.shared_lib.ocp_nlp_eval_cost.argtypes = [c_void_p, c_void_p, c_void_p] + self.shared_lib.ocp_nlp_eval_cost(self.nlp_solver, self.nlp_in, self.nlp_out) + + # create output array + out = np.ascontiguousarray(np.zeros((1,)), dtype=np.float64) + out_data = cast(out.ctypes.data, POINTER(c_double)) + + # call getter + self.shared_lib.ocp_nlp_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p] + + field = "cost_value".encode('utf-8') + self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data) + + return out[0] + + + def get_residuals(self): + """ + Returns an array of the form [res_stat, res_eq, res_ineq, res_comp]. + """ + # compute residuals if RTI + if self.acados_ocp.solver_options.nlp_solver_type == 'SQP_RTI': + self.shared_lib.ocp_nlp_eval_residuals.argtypes = [c_void_p, c_void_p, c_void_p] + self.shared_lib.ocp_nlp_eval_residuals(self.nlp_solver, self.nlp_in, self.nlp_out) + + # create output array + out = np.ascontiguousarray(np.zeros((4, 1)), dtype=np.float64) + out_data = cast(out.ctypes.data, POINTER(c_double)) + + # call getters + self.shared_lib.ocp_nlp_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p] + + field = "res_stat".encode('utf-8') + self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data) + + out_data = cast(out[1].ctypes.data, POINTER(c_double)) + field = "res_eq".encode('utf-8') + self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data) + + out_data = cast(out[2].ctypes.data, POINTER(c_double)) + field = "res_ineq".encode('utf-8') + self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data) + + out_data = cast(out[3].ctypes.data, POINTER(c_double)) + field = "res_comp".encode('utf-8') + self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data) + + return out.flatten() + + + # Note: this function should not be used anymore, better use cost_set, constraints_set + def set(self, stage_, field_, value_): + """ + Set numerical data inside the solver. + + :param stage: integer corresponding to shooting node + :param field: string in ['x', 'u', 'pi', 'lam', 't', 'p'] + + .. 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'] + mem_fields = ['sl', 'su'] + + # cast value_ to avoid conversion issues + if isinstance(value_, (float, int)): + value_ = np.array([value_]) + value_ = value_.astype(float) + + model = self.acados_ocp.model + + field = field_ + field = field.encode('utf-8') + + stage = c_int(stage_) + + # treat parameters separately + if field_ == 'p': + getattr(self.shared_lib, f"{model.name}_acados_update_params").argtypes = [c_void_p, c_int, POINTER(c_double)] + getattr(self.shared_lib, f"{model.name}_acados_update_params").restype = c_int + + value_data = cast(value_.ctypes.data, POINTER(c_double)) + + assert getattr(self.shared_lib, f"{model.name}_acados_update_params")(self.capsule, stage, value_data, value_.shape[0])==0 + else: + if field_ not in constraints_fields + cost_fields + out_fields + mem_fields: + raise Exception("AcadosOcpSolver.set(): {} is not a valid argument.\ + \nPossible values are {}. Exiting.".format(field, \ + constraints_fields + cost_fields + out_fields + ['p'])) + + self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = \ + [c_void_p, c_void_p, c_void_p, c_int, c_char_p] + self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int + + dims = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, \ + self.nlp_dims, self.nlp_out, stage_, field) + + if value_.shape[0] != dims: + msg = 'AcadosOcpSolver.set(): mismatching dimension for field "{}" '.format(field_) + msg += 'with dimension {} (you have {})'.format(dims, value_.shape[0]) + raise Exception(msg) + + value_data = cast(value_.ctypes.data, POINTER(c_double)) + value_data_p = cast((value_data), c_void_p) + + if field_ in constraints_fields: + self.shared_lib.ocp_nlp_constraints_model_set.argtypes = \ + [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] + self.shared_lib.ocp_nlp_constraints_model_set(self.nlp_config, \ + self.nlp_dims, self.nlp_in, stage, field, value_data_p) + elif field_ in cost_fields: + self.shared_lib.ocp_nlp_cost_model_set.argtypes = \ + [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] + self.shared_lib.ocp_nlp_cost_model_set(self.nlp_config, \ + self.nlp_dims, self.nlp_in, stage, field, value_data_p) + elif field_ in out_fields: + self.shared_lib.ocp_nlp_out_set.argtypes = \ + [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] + self.shared_lib.ocp_nlp_out_set(self.nlp_config, \ + self.nlp_dims, self.nlp_out, stage, field, value_data_p) + elif field_ in mem_fields: + self.shared_lib.ocp_nlp_set.argtypes = \ + [c_void_p, c_void_p, c_int, c_char_p, c_void_p] + self.shared_lib.ocp_nlp_set(self.nlp_config, \ + self.nlp_solver, stage, field, value_data_p) + return + + + def cost_set(self, stage_, field_, value_, api='warn'): + """ + Set numerical data in the cost module of the solver. + + :param stage: integer corresponding to shooting node + :param field: string, e.g. 'yref', 'W', 'ext_cost_num_hess' + :param value: of appropriate size + """ + # cast value_ to avoid conversion issues + if isinstance(value_, (float, int)): + value_ = np.array([value_]) + value_ = value_.astype(float) + + field = field_ + field = field.encode('utf-8') + + stage = c_int(stage_) + self.shared_lib.ocp_nlp_cost_dims_get_from_attr.argtypes = \ + [c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)] + self.shared_lib.ocp_nlp_cost_dims_get_from_attr.restype = c_int + + dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc) + dims_data = cast(dims.ctypes.data, POINTER(c_int)) + + self.shared_lib.ocp_nlp_cost_dims_get_from_attr(self.nlp_config, \ + self.nlp_dims, self.nlp_out, stage_, field, dims_data) + + value_shape = value_.shape + if len(value_shape) == 1: + value_shape = (value_shape[0], 0) + + elif len(value_shape) == 2: + if api=='old': + pass + elif api=='warn': + if not np.all(np.ravel(value_, order='F')==np.ravel(value_, order='K')): + raise Exception("Ambiguity in API detected.\n" + "Are you making an acados model from scrach? Add api='new' to cost_set and carry on.\n" + "Are you seeing this error suddenly in previously running code? Read on.\n" + " You are relying on a now-fixed bug in cost_set for field '{}'.\n".format(field_) + + " acados_template now correctly passes on any matrices to acados in column major format.\n" + + " Two options to fix this error: \n" + + " * Add api='old' to cost_set to restore old incorrect behaviour\n" + + " * Add api='new' to cost_set and remove any unnatural manipulation of the value argument " + + "such as non-mathematical transposes, reshaping, casting to fortran order, etc... " + + "If there is no such manipulation, then you have probably been getting an incorrect solution before.") + # Get elements in column major order + value_ = np.ravel(value_, order='F') + elif api=='new': + # Get elements in column major order + value_ = np.ravel(value_, order='F') + else: + raise Exception("Unknown api: '{}'".format(api)) + + if value_shape != tuple(dims): + raise Exception('AcadosOcpSolver.cost_set(): mismatching dimension', \ + ' for field "{}" with dimension {} (you have {})'.format( \ + field_, tuple(dims), value_shape)) + + value_data = cast(value_.ctypes.data, POINTER(c_double)) + value_data_p = cast((value_data), c_void_p) + + self.shared_lib.ocp_nlp_cost_model_set.argtypes = \ + [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] + self.shared_lib.ocp_nlp_cost_model_set(self.nlp_config, \ + self.nlp_dims, self.nlp_in, stage, field, value_data_p) + + return + + + def constraints_set(self, stage_, field_, value_, api='warn'): + """ + Set numerical data in the constraint module of the solver. + + :param stage: integer corresponding to shooting node + :param field: string in ['lbx', 'ubx', 'lbu', 'ubu', 'lg', 'ug', 'lh', 'uh', 'uphi'] + :param value: of appropriate size + """ + # cast value_ to avoid conversion issues + if isinstance(value_, (float, int)): + value_ = np.array([value_]) + value_ = value_.astype(float) + + field = field_ + field = field.encode('utf-8') + + stage = c_int(stage_) + self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.argtypes = \ + [c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)] + self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.restype = c_int + + dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc) + dims_data = cast(dims.ctypes.data, POINTER(c_int)) + + self.shared_lib.ocp_nlp_constraint_dims_get_from_attr(self.nlp_config, \ + self.nlp_dims, self.nlp_out, stage_, field, dims_data) + + value_shape = value_.shape + if len(value_shape) == 1: + value_shape = (value_shape[0], 0) + elif len(value_shape) == 2: + if api=='old': + pass + elif api=='warn': + if not np.all(np.ravel(value_, order='F')==np.ravel(value_, order='K')): + raise Exception("Ambiguity in API detected.\n" + "Are you making an acados model from scrach? Add api='new' to constraints_set and carry on.\n" + "Are you seeing this error suddenly in previously running code? Read on.\n" + " You are relying on a now-fixed bug in constraints_set for field '{}'.\n".format(field_) + + " acados_template now correctly passes on any matrices to acados in column major format.\n" + + " Two options to fix this error: \n" + + " * Add api='old' to constraints_set to restore old incorrect behaviour\n" + + " * Add api='new' to constraints_set and remove any unnatural manipulation of the value argument " + + "such as non-mathematical transposes, reshaping, casting to fortran order, etc... " + + "If there is no such manipulation, then you have probably been getting an incorrect solution before.") + # Get elements in column major order + value_ = np.ravel(value_, order='F') + elif api=='new': + # Get elements in column major order + value_ = np.ravel(value_, order='F') + else: + raise Exception("Unknown api: '{}'".format(api)) + + if value_shape != tuple(dims): + raise Exception('AcadosOcpSolver.constraints_set(): mismatching dimension' \ + ' for field "{}" with dimension {} (you have {})'.format(field_, tuple(dims), value_shape)) + + value_data = cast(value_.ctypes.data, POINTER(c_double)) + value_data_p = cast((value_data), c_void_p) + + self.shared_lib.ocp_nlp_constraints_model_set.argtypes = \ + [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] + self.shared_lib.ocp_nlp_constraints_model_set(self.nlp_config, \ + self.nlp_dims, self.nlp_in, stage, field, value_data_p) + + return + + + def dynamics_get(self, stage_, field_): + """ + Get numerical data from the dynamics module of the solver: + + :param stage: integer corresponding to shooting node + :param field: string, e.g. 'A' + """ + + field = field_ + field = field.encode('utf-8') + stage = c_int(stage_) + + # get dims + self.shared_lib.ocp_nlp_dynamics_dims_get_from_attr.argtypes = \ + [c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)] + self.shared_lib.ocp_nlp_dynamics_dims_get_from_attr.restype = c_int + + dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc) + dims_data = cast(dims.ctypes.data, POINTER(c_int)) + + self.shared_lib.ocp_nlp_dynamics_dims_get_from_attr(self.nlp_config, \ + self.nlp_dims, self.nlp_out, stage_, field, dims_data) + + # create output data + out = np.ascontiguousarray(np.zeros((np.prod(dims),)), dtype=np.float64) + out = out.reshape(dims[0], dims[1], order='F') + + out_data = cast(out.ctypes.data, POINTER(c_double)) + out_data_p = cast((out_data), c_void_p) + + # call getter + self.shared_lib.ocp_nlp_get_at_stage.argtypes = \ + [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] + self.shared_lib.ocp_nlp_get_at_stage(self.nlp_config, \ + self.nlp_dims, self.nlp_solver, stage, field, out_data_p) + + return out + + + def options_set(self, field_, value_): + """ + Set options of the solver. + + :param field: string, e.g. 'print_level', 'rti_phase', 'initialize_t_slacks', 'step_length', 'alpha_min', 'alpha_reduction' + :param value: of type int, float + """ + int_fields = ['print_level', 'rti_phase', 'initialize_t_slacks'] + double_fields = ['step_length', 'tol_eq', 'tol_stat', 'tol_ineq', 'tol_comp', 'alpha_min', 'alpha_reduction'] + string_fields = ['globalization'] + + # check field availability and type + if field_ in int_fields: + if not isinstance(value_, int): + raise Exception('solver option {} must be of type int. You have {}.'.format(field_, type(value_))) + else: + value_ctypes = c_int(value_) + + elif field_ in double_fields: + if not isinstance(value_, float): + raise Exception('solver option {} must be of type float. You have {}.'.format(field_, type(value_))) + else: + value_ctypes = c_double(value_) + + elif field_ in string_fields: + if not isinstance(value_, str): + raise Exception('solver option {} must be of type str. You have {}.'.format(field_, type(value_))) + else: + value_ctypes = value_.encode('utf-8') + else: + raise Exception('AcadosOcpSolver.options_set() does not support field {}.'\ + '\n Possible values are {}.'.format(field_, ', '.join(int_fields + double_fields + string_fields))) + + + if field_ == 'rti_phase': + if value_ < 0 or value_ > 2: + raise Exception('AcadosOcpSolver.solve(): argument \'rti_phase\' can ' + 'take only values 0, 1, 2 for SQP-RTI-type solvers') + if self.acados_ocp.solver_options.nlp_solver_type != 'SQP_RTI' and value_ > 0: + raise Exception('AcadosOcpSolver.solve(): argument \'rti_phase\' can ' + 'take only value 0 for SQP-type solvers') + + # encode + field = field_ + field = field.encode('utf-8') + + # call C interface + if field_ in string_fields: + self.shared_lib.ocp_nlp_solver_opts_set.argtypes = \ + [c_void_p, c_void_p, c_char_p, c_char_p] + self.shared_lib.ocp_nlp_solver_opts_set(self.nlp_config, \ + self.nlp_opts, field, value_ctypes) + else: + self.shared_lib.ocp_nlp_solver_opts_set.argtypes = \ + [c_void_p, c_void_p, c_char_p, c_void_p] + self.shared_lib.ocp_nlp_solver_opts_set(self.nlp_config, \ + self.nlp_opts, field, byref(value_ctypes)) + + return + + + def __del__(self): + model = self.acados_ocp.model + + if self.solver_created: + getattr(self.shared_lib, f"{model.name}_acados_free").argtypes = [c_void_p] + getattr(self.shared_lib, f"{model.name}_acados_free").restype = c_int + getattr(self.shared_lib, f"{model.name}_acados_free")(self.capsule) + + getattr(self.shared_lib, f"{model.name}_acados_free_capsule").argtypes = [c_void_p] + getattr(self.shared_lib, f"{model.name}_acados_free_capsule").restype = c_int + getattr(self.shared_lib, f"{model.name}_acados_free_capsule")(self.capsule) + + try: + self.dlclose(self.shared_lib._handle) + except: + pass diff --git a/pyextra/acados_template/acados_sim.py b/pyextra/acados_template/acados_sim.py new file mode 100644 index 0000000000..9f2e19c5df --- /dev/null +++ b/pyextra/acados_template/acados_sim.py @@ -0,0 +1,294 @@ +# -*- 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 type(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 type(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 type(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 type(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.__tf = 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 = False + + @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): + """Boolean determining if jacobians are reused. Default: False""" + return self.__sim_method_jac_reuse + + @property + def T(self): + """Time horizon""" + return self.__Tsim + + @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.') + + @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 (True, False): + self.__sim_method_jac_reuse = sim_method_jac_reuse + else: + raise Exception('Invalid sim_method_jac_reuse value. sim_method_jac_reuse must be a Boolean.') + +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) + """ + 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 = f'{acados_path}/include' + """Path to acados include directors (set automatically), type: `string`""" + self.acados_lib_path = f'{acados_path}/lib' + """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`.""" + + def set(self, attr, value): + # tokenize string + tokens = attr.split('_', 1) + if len(tokens) > 1: + setter_to_call = getattr(getattr(self, tokens[0]), 'set') + else: + setter_to_call = getattr(self, 'set') + + setter_to_call(tokens[1], value) + + return diff --git a/pyextra/acados_template/acados_sim_layout.json b/pyextra/acados_template/acados_sim_layout.json new file mode 100644 index 0000000000..c8f4280b84 --- /dev/null +++ b/pyextra/acados_template/acados_sim_layout.json @@ -0,0 +1,44 @@ +{ + "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" + ], + "Tsim": [ + "float" + ], + "sim_method_num_stages": [ + "int" + ], + "sim_method_num_steps": [ + "int" + ], + "sim_method_newton_iter": [ + "int" + ] + } +} diff --git a/pyextra/acados_template/acados_sim_solver.py b/pyextra/acados_template/acados_sim_solver.py new file mode 100644 index 0000000000..a5447907b9 --- /dev/null +++ b/pyextra/acados_template/acados_sim_solver.py @@ -0,0 +1,407 @@ +# -*- 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 + + +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(): + current_module = sys.modules[__name__] + acados_path = os.path.dirname(current_module.__file__) + with open(acados_path + '/acados_sim_layout.json', '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_render_templates(json_file, model_name, code_export_dir): + # setting up loader and environment + json_path = '{cwd}/{json_file}'.format( + cwd=os.getcwd(), + json_file=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) + + in_file = 'Makefile.in' + out_file = f'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 = f'{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 + """ + def __init__(self, acados_sim_, json_file='acados_sim.json', build=True): + + 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) + + ## Compile solver + cwd = os.getcwd() + os.chdir(code_export_dir) + 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 + + # Ctypes + shared_lib = f'{code_export_dir}/libacados_sim_solver_{model_name}.so' + self.shared_lib = CDLL(shared_lib) + + + # create capsule + getattr(self.shared_lib, f"{model_name}_acados_sim_solver_create_capsule").restype = c_void_p + self.capsule = getattr(self.shared_lib, f"{model_name}_acados_sim_solver_create_capsule")() + + # create solver + getattr(self.shared_lib, f"{model_name}_acados_sim_create").argtypes = [c_void_p] + getattr(self.shared_lib, f"{model_name}_acados_sim_create").restype = c_int + assert getattr(self.shared_lib, f"{model_name}_acados_sim_create")(self.capsule)==0 + self.solver_created = True + + getattr(self.shared_lib, f"{model_name}_acados_get_sim_opts").argtypes = [c_void_p] + getattr(self.shared_lib, f"{model_name}_acados_get_sim_opts").restype = c_void_p + self.sim_opts = getattr(self.shared_lib, f"{model_name}_acados_get_sim_opts")(self.capsule) + + getattr(self.shared_lib, f"{model_name}_acados_get_sim_dims").argtypes = [c_void_p] + getattr(self.shared_lib, f"{model_name}_acados_get_sim_dims").restype = c_void_p + self.sim_dims = getattr(self.shared_lib, f"{model_name}_acados_get_sim_dims")(self.capsule) + + getattr(self.shared_lib, f"{model_name}_acados_get_sim_config").argtypes = [c_void_p] + getattr(self.shared_lib, f"{model_name}_acados_get_sim_config").restype = c_void_p + self.sim_config = getattr(self.shared_lib, f"{model_name}_acados_get_sim_config")(self.capsule) + + getattr(self.shared_lib, f"{model_name}_acados_get_sim_out").argtypes = [c_void_p] + getattr(self.shared_lib, f"{model_name}_acados_get_sim_out").restype = c_void_p + self.sim_out = getattr(self.shared_lib, f"{model_name}_acados_get_sim_out")(self.capsule) + + getattr(self.shared_lib, f"{model_name}_acados_get_sim_in").argtypes = [c_void_p] + getattr(self.shared_lib, f"{model_name}_acados_get_sim_in").restype = c_void_p + self.sim_in = getattr(self.shared_lib, f"{model_name}_acados_get_sim_in")(self.capsule) + + getattr(self.shared_lib, f"{model_name}_acados_get_sim_solver").argtypes = [c_void_p] + getattr(self.shared_lib, f"{model_name}_acados_get_sim_solver").restype = c_void_p + self.sim_solver = getattr(self.shared_lib, f"{model_name}_acados_get_sim_solver")(self.capsule) + + nu = self.sim_struct.dims.nu + nx = self.sim_struct.dims.nx + nz = self.sim_struct.dims.nz + self.gettable = { + 'x': nx, + 'xn': nx, + 'u': nu, + 'z': nz, + 'S_forw': nx*(nx+nu), + 'Sx': nx*nx, + 'Su': nx*nu, + 'S_adj': nx+nu, + 'S_hess': (nx+nu)*(nx+nu), + 'S_algebraic': (nz)*(nx+nu), + } + + self.settable = ['S_adj', 'T', 'x', 'u', 'xdot', 'z', 'p'] # S_forw + + + def solve(self): + """ + Solve the simulation problem with current input. + """ + getattr(self.shared_lib, f"{self.model_name}_acados_sim_solve").argtypes = [c_void_p] + getattr(self.shared_lib, f"{self.model_name}_acados_sim_solve").restype = c_int + + status = getattr(self.shared_lib, f"{self.model_name}_acados_sim_solve")(self.capsule) + return status + + + def get(self, field_): + """ + Get the last solution of the solver. + + :param str field: string in ['x', 'u', 'z', 'S_forw', 'Sx', 'Su', 'S_adj', 'S_hess', 'S_algebraic'] + """ + field = field_ + field = field.encode('utf-8') + + if field_ in self.gettable.keys(): + + # allocate array + dims = self.gettable[field_] + out = np.ascontiguousarray(np.zeros((dims,)), dtype=np.float64) + out_data = cast(out.ctypes.data, POINTER(c_double)) + + self.shared_lib.sim_out_get.argtypes = [c_void_p, c_void_p, c_void_p, c_char_p, c_void_p] + self.shared_lib.sim_out_get(self.sim_config, self.sim_dims, self.sim_out, field, out_data) + + if field_ == 'S_forw': + nu = self.sim_struct.dims.nu + nx = self.sim_struct.dims.nx + out = out.reshape(nx, nx+nu, order='F') + elif field_ == 'Sx': + nx = self.sim_struct.dims.nx + out = out.reshape(nx, nx, order='F') + elif field_ == 'Su': + nx = self.sim_struct.dims.nx + nu = self.sim_struct.dims.nu + out = out.reshape(nx, nu, order='F') + elif field_ == 'S_hess': + nx = self.sim_struct.dims.nx + nu = self.sim_struct.dims.nu + out = out.reshape(nx+nu, nx+nu, order='F') + elif field_ == 'S_algebraic': + nx = self.sim_struct.dims.nx + nu = self.sim_struct.dims.nu + nz = self.sim_struct.dims.nz + out = out.reshape(nz, nx+nu, order='F') + else: + raise Exception(f'AcadosSimSolver.get(): Unknown field {field_},' \ + f' available fields are {", ".join(self.gettable.keys())}') + + return out + + + def set(self, field_, value_): + """ + Set numerical data inside the solver. + + :param field: string in ['p', 'S_adj', 'T', 'x', 'u', 'xdot', 'z'] + :param value: the value with appropriate size. + """ + # cast value_ to avoid conversion issues + if isinstance(value_, (float, int)): + value_ = np.array([value_]) + + value_ = value_.astype(float) + value_data = cast(value_.ctypes.data, POINTER(c_double)) + value_data_p = cast((value_data), c_void_p) + + field = field_ + field = field.encode('utf-8') + + # treat parameters separately + if field_ == 'p': + model_name = self.sim_struct.model.name + getattr(self.shared_lib, f"{model_name}_acados_sim_update_params").argtypes = [c_void_p, POINTER(c_double), c_int] + value_data = cast(value_.ctypes.data, POINTER(c_double)) + getattr(self.shared_lib, f"{model_name}_acados_sim_update_params")(self.capsule, value_data, value_.shape[0]) + return + else: + # dimension check + dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc) + dims_data = cast(dims.ctypes.data, POINTER(c_int)) + + self.shared_lib.sim_dims_get_from_attr.argtypes = [c_void_p, c_void_p, c_char_p, POINTER(c_int)] + self.shared_lib.sim_dims_get_from_attr(self.sim_config, self.sim_dims, field, dims_data) + + value_ = np.ravel(value_, order='F') + + value_shape = value_.shape + if len(value_shape) == 1: + value_shape = (value_shape[0], 0) + + if value_shape != tuple(dims): + raise Exception('AcadosSimSolver.set(): mismatching dimension' \ + ' for field "{}" with dimension {} (you have {})'.format(field_, tuple(dims), value_shape)) + + # set + if field_ in ['xdot', 'z']: + self.shared_lib.sim_solver_set.argtypes = [c_void_p, c_char_p, c_void_p] + self.shared_lib.sim_solver_set(self.sim_solver, field, value_data_p) + elif field_ in self.settable: + self.shared_lib.sim_in_set.argtypes = [c_void_p, c_void_p, c_void_p, c_char_p, c_void_p] + self.shared_lib.sim_in_set(self.sim_config, self.sim_dims, self.sim_in, field, value_data_p) + else: + raise Exception(f'AcadosSimSolver.set(): Unknown field {field_},' \ + f' available fields are {", ".join(self.settable)}') + + return + + + def __del__(self): + + if self.solver_created: + getattr(self.shared_lib, f"{self.model_name}_acados_sim_free").argtypes = [c_void_p] + getattr(self.shared_lib, f"{self.model_name}_acados_sim_free").restype = c_int + getattr(self.shared_lib, f"{self.model_name}_acados_sim_free")(self.capsule) + + getattr(self.shared_lib, f"{self.model_name}_acados_sim_solver_free_capsule").argtypes = [c_void_p] + getattr(self.shared_lib, f"{self.model_name}_acados_sim_solver_free_capsule").restype = c_int + getattr(self.shared_lib, f"{self.model_name}_acados_sim_solver_free_capsule")(self.capsule) + + try: + self.dlclose(self.shared_lib._handle) + except: + pass diff --git a/pyextra/acados_template/c_templates_tera/CPPLINT.cfg b/pyextra/acados_template/c_templates_tera/CPPLINT.cfg new file mode 100644 index 0000000000..bbd1caf057 --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/CPPLINT.cfg @@ -0,0 +1 @@ +exclude_files=[main, acados_solver, acados_solver_sfun, Makefile, model].*\.? diff --git a/pyextra/acados_template/c_templates_tera/Makefile.in b/pyextra/acados_template/c_templates_tera/Makefile.in new file mode 100644 index 0000000000..4f1899de4e --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/Makefile.in @@ -0,0 +1,485 @@ +# +# 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 %} + +{# acados flags #} +ACADOS_FLAGS = -fPIC -std=c99 {{ openmp_flag }} #-fno-diagnostics-show-line-numbers -g +{%- if qp_solver == "FULL_CONDENSING_QPOASES" %} +ACADOS_FLAGS += -DACADOS_WITH_QPOASES +{%- endif %} +{%- if qp_solver == "PARTIAL_CONDENSING_OSQP" %} +ACADOS_FLAGS += -DACADOS_WITH_OSQP +{%- endif %} +{%- if qp_solver == "PARTIAL_CONDENSING_QPDUNES" %} +ACADOS_FLAGS += -DACADOS_WITH_QPDUNES +{%- endif %} +# # Debugging +# ACADOS_FLAGS += -g3 + +MODEL_OBJ= +{%- if solver_options.integrator_type == "ERK" %} +MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_expl_ode_fun.o +MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_expl_vde_forw.o +{%- if hessian_approx == "EXACT" %} +MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_expl_ode_hess.o +{%- endif %} +{%- elif solver_options.integrator_type == "IRK" %} +MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun.o +MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_z.o +MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_jac_x_xdot_u_z.o +{%- if hessian_approx == "EXACT" %} +MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_hess.o +{%- endif %} +{%- elif solver_options.integrator_type == "GNSF" %} +MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.o +MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_fun_jac_y.o +MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.o +MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.o +MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.o +{%- elif solver_options.integrator_type == "DISCRETE" %} +{%- if model.dyn_ext_fun_type == "casadi" %} +MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun.o +MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac.o +{%- if hessian_approx == "EXACT" %} +MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac_hess.o +{%- endif %} +{%- else %} +MODEL_OBJ+= {{ model.name }}_model/{{ model.dyn_source_discrete }} +{%- endif %} +{%- endif %} + + +OCP_OBJ= +{%- if constr_type == "BGP" and dims_nphi > 0 %} +OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_phi_constraint.o +{%- endif %} +{%- if constr_type_e == "BGP" and dims_nphi_e > 0 %} +OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_phi_e_constraint.o +{%- endif %} + +{%- if constr_type == "BGH" and dims_nh > 0 %} +OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt.o +OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun.o +{%- if hessian_approx == "EXACT" %} +OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt_hess.o +{%- endif %} +{%- endif %} + +{%- if constr_type_e == "BGH" and dims_nh_e > 0 %} +OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt.o +OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun.o +{%- if hessian_approx == "EXACT" %} +OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess.o +{%- endif %} +{%- endif %} + +{%- if cost_type_0 == "NONLINEAR_LS" %} +OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_fun.c +OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_fun_jac_ut_xt.c +OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_hess.c +{%- elif cost_type_0 == "EXTERNAL" %} +{% if cost.cost_ext_fun_type_0 == "casadi" %} +OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun.c +OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac.c +OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac_hess.c +{% else %} +OCP_OBJ+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost_0 }} +{% endif %} +{%- endif %} +{%- if cost_type == "NONLINEAR_LS" %} +OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_fun.c +OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_fun_jac_ut_xt.c +OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_hess.c +{%- elif cost_type == "EXTERNAL" %} +{% if cost.cost_ext_fun_type == "casadi" %} +OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun.c +OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac.c +OCP_OBJ+= {{ 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_OBJ+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost }} +{% endif %} +{%- endif %} +{%- if cost_type_e == "NONLINEAR_LS" %} +OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_fun.c +OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_fun_jac_ut_xt.c +OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_hess.c +{%- elif cost_type_e == "EXTERNAL" %} +{% if cost.cost_ext_fun_type_e == "casadi" %} +OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun.c +OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac.c +OCP_OBJ+= {{ 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_OBJ+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost_e }} +{% endif %} +{%- endif %} +OCP_OBJ+= acados_solver_{{ model.name }}.o + + +SIM_OBJ= +SIM_OBJ+= acados_sim_solver_{{ model.name }}.o + +EX_OBJ= +EX_OBJ+= main_{{ model.name }}.o + +EX_SIM_OBJ= +EX_SIM_OBJ+= main_sim_{{ model.name }}.o + +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 }} + +{%- if solver_options.integrator_type == "DISCRETE" %} +all: clean casadi_fun example +shared_lib: ocp_shared_lib +{%- else %} +all: clean casadi_fun example_sim example +shared_lib: bundled_shared_lib ocp_shared_lib sim_shared_lib +{%- endif %} + +CASADI_MODEL_SOURCE= +{%- if solver_options.integrator_type == "ERK" %} +CASADI_MODEL_SOURCE+= {{ model.name }}_expl_ode_fun.c +CASADI_MODEL_SOURCE+= {{ model.name }}_expl_vde_forw.c +{%- if hessian_approx == "EXACT" %} +CASADI_MODEL_SOURCE+= {{ model.name }}_expl_ode_hess.c +{%- endif %} +{%- elif solver_options.integrator_type == "IRK" %} +CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_fun.c +CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_fun_jac_x_xdot_z.c +CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_jac_x_xdot_u_z.c +{%- if hessian_approx == "EXACT" %} +CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_hess.c +{%- endif %} +{%- elif solver_options.integrator_type == "GNSF" %} +CASADI_MODEL_SOURCE+= {{ model.name }}_gnsf_phi_fun.c +CASADI_MODEL_SOURCE+= {{ model.name }}_gnsf_phi_fun_jac_y.c +CASADI_MODEL_SOURCE+= {{ model.name }}_gnsf_phi_jac_y_uhat.c +CASADI_MODEL_SOURCE+= {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c +CASADI_MODEL_SOURCE+= {{ model.name }}_gnsf_get_matrices_fun.c +{%- elif solver_options.integrator_type == "DISCRETE" and model.dyn_ext_fun_type == "casadi" %} +CASADI_MODEL_SOURCE+= {{ model.name }}_dyn_disc_phi_fun.c +CASADI_MODEL_SOURCE+= {{ model.name }}_dyn_disc_phi_fun_jac.c +{%- if hessian_approx == "EXACT" %} +CASADI_MODEL_SOURCE+= {{ model.name }}_dyn_disc_phi_fun_jac_hess.c +{%- endif %} +{%- endif %} +{%- if constr_type == "BGP" and dims_nphi > 0 %} +CASADI_CON_PHI_SOURCE= +CASADI_CON_PHI_SOURCE+= {{ model.name }}_phi_constraint.c +{%- endif %} +{%- if constr_type_e == "BGP" and dims_nphi_e > 0 %} +CASADI_CON_PHI_E_SOURCE= +CASADI_CON_PHI_E_SOURCE+= {{ model.name }}_phi_e_constraint.c +{%- endif %} +{%- if constr_type == "BGH" and dims_nh > 0 %} +CASADI_CON_H_SOURCE= +CASADI_CON_H_SOURCE+= {{ model.name }}_constr_h_fun_jac_uxt_zt.c +CASADI_CON_H_SOURCE+= {{ model.name }}_constr_h_fun.c +{%- if hessian_approx == "EXACT" %} +CASADI_CON_H_SOURCE+= {{ model.name }}_constr_h_fun_jac_uxt_zt_hess.c +{%- endif %} +{%- endif %} + +{%- if dims_nh_e > 0 %} +CASADI_CON_H_E_SOURCE= +CASADI_CON_H_E_SOURCE+= {{ model.name }}_constr_h_e_fun_jac_uxt_zt.c +CASADI_CON_H_E_SOURCE+= {{ model.name }}_constr_h_e_fun.c +{%- if hessian_approx == "EXACT" %} +CASADI_CON_H_E_SOURCE+= {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess.c +{%- endif %} +{%- endif %} + +{%- if cost_type == "NONLINEAR_LS" %} +CASADI_COST_Y_SOURCE= +CASADI_COST_Y_SOURCE+= {{ model.name }}_cost_y_fun.c +CASADI_COST_Y_SOURCE+= {{ model.name }}_cost_y_fun_jac_ut_xt.c +CASADI_COST_Y_SOURCE+= {{ model.name }}_cost_y_hess.c +{%- endif %} +{%- if cost_type_e == "NONLINEAR_LS" %} +CASADI_COST_Y_E_SOURCE= +CASADI_COST_Y_E_SOURCE+= {{ model.name }}_cost_y_e_fun.c +CASADI_COST_Y_E_SOURCE+= {{ model.name }}_cost_y_e_fun_jac_ut_xt.c +CASADI_COST_Y_E_SOURCE+= {{ model.name }}_cost_y_e_hess.c +{%- endif %} +{%- if cost_type_0 == "NONLINEAR_LS" %} +CASADI_COST_Y_0_SOURCE= +CASADI_COST_Y_0_SOURCE+= {{ model.name }}_cost_y_0_fun.c +CASADI_COST_Y_0_SOURCE+= {{ model.name }}_cost_y_0_fun_jac_ut_xt.c +CASADI_COST_Y_0_SOURCE+= {{ model.name }}_cost_y_0_hess.c +{%- endif %} + +casadi_fun: + {%- if model.dyn_ext_fun_type == "casadi" %} + ( cd {{ model.name }}_model {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_MODEL_SOURCE)) + {%- endif %} + {%- if constr_type == "BGP" and dims_nphi > 0 %} + ( cd {{ model.name }}_constraints {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_CON_PHI_SOURCE)) + {%- endif %} + {%- if constr_type_e == "BGP" and dims_nphi_e > 0 %} + ( cd {{ model.name }}_constraints {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_CON_PHI_E_SOURCE)) + {%- endif %} + {%- if constr_type == "BGH" and dims_nh > 0 %} + ( cd {{ model.name }}_constraints {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_CON_H_SOURCE)) + {%- endif %} + {%- if constr_type_e == "BGH" and dims_nh_e > 0 %} + ( cd {{ model.name }}_constraints {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_CON_H_E_SOURCE)) + {%- endif %} + {%- if cost_type == "NONLINEAR_LS" %} + ( cd {{ model.name }}_cost {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_COST_Y_SOURCE)) + {%- endif %} + {%- if cost_type_e == "NONLINEAR_LS" %} + ( cd {{ model.name }}_cost {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_COST_Y_E_SOURCE)) + {%- endif %} + {%- if cost_type_0 == "NONLINEAR_LS" %} + ( cd {{ model.name }}_cost {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_COST_Y_0_SOURCE)) + {%- endif %} + +main: + gcc $(ACADOS_FLAGS) -c main_{{ model.name }}.c -I $(INCLUDE_PATH)/blasfeo/include/ -I $(INCLUDE_PATH)/hpipm/include/ \ + -I $(INCLUDE_PATH) -I $(INCLUDE_PATH)/acados/ \ + {%- if qp_solver == "FULL_CONDENSING_QPOASES" %} + -I $(INCLUDE_PATH)/qpOASES_e/ + {%- endif %} + +main_sim: + gcc $(ACADOS_FLAGS) -c main_sim_{{ model.name }}.c -I $(INCLUDE_PATH)/blasfeo/include/ -I $(INCLUDE_PATH)/hpipm/include/ \ + -I $(INCLUDE_PATH) -I $(INCLUDE_PATH)/acados/ + +ocp_solver: + gcc $(ACADOS_FLAGS) -c acados_solver_{{ model.name }}.c -I $(INCLUDE_PATH)/blasfeo/include/ -I $(INCLUDE_PATH)/hpipm/include/ \ + -I $(INCLUDE_PATH) -I $(INCLUDE_PATH)/acados/ \ + {%- if qp_solver == "FULL_CONDENSING_QPOASES" %} + -I $(INCLUDE_PATH)/qpOASES_e/ + {%- endif %} + +sim_solver: + gcc $(ACADOS_FLAGS) -c acados_sim_solver_{{ model.name }}.c -I $(INCLUDE_PATH)/blasfeo/include/ -I $(INCLUDE_PATH)/hpipm/include/ \ + -I $(INCLUDE_PATH) -I $(INCLUDE_PATH)/acados/ \ + {%- if qp_solver == "FULL_CONDENSING_QPOASES" %} + -I $(INCLUDE_PATH)/qpOASES_e/ + {%- endif %} + +example: ocp_solver main + gcc $(ACADOS_FLAGS) -o main_{{ model.name }} $(EX_OBJ) $(OBJ) -L $(LIB_PATH) \ + -lacados -lhpipm -lblasfeo \ + {{ link_libs }} \ + -lm \ + -I $(INCLUDE_PATH)/blasfeo/include/ \ + -I $(INCLUDE_PATH)/hpipm/include/ \ + -I $(INCLUDE_PATH) \ + -I $(INCLUDE_PATH)/acados/ \ + {%- if qp_solver == "FULL_CONDENSING_QPOASES" %} + -I $(INCLUDE_PATH)/qpOASES_e/ + {%- endif %} + + +example_sim: sim_solver main_sim + gcc $(ACADOS_FLAGS) -o main_sim_{{ model.name }} $(EX_SIM_OBJ) $(MODEL_OBJ) $(SIM_OBJ) -L $(LIB_PATH) \ + -lacados -lhpipm -lblasfeo \ + {{ link_libs }} \ + -lm \ + -I $(INCLUDE_PATH)/blasfeo/include/ \ + -I $(INCLUDE_PATH)/acados/ \ + +{%- if solver_options.integrator_type != "DISCRETE" %} + +bundled_shared_lib: casadi_fun ocp_solver sim_solver + gcc $(ACADOS_FLAGS) -shared -o libacados_solver_{{ model.name }}.so $(OBJ) \ + -I $(INCLUDE_PATH)/blasfeo/include/ \ + -I $(INCLUDE_PATH)/hpipm/include/ \ + -I $(INCLUDE_PATH) \ + -L $(LIB_PATH) \ + -lacados -lhpipm -lblasfeo \ + {{ link_libs }} \ + -lm \ + +ocp_shared_lib: casadi_fun ocp_solver + gcc $(ACADOS_FLAGS) -shared -o libacados_ocp_solver_{{ model.name }}.so $(OCP_OBJ) $(MODEL_OBJ) \ + -I $(INCLUDE_PATH)/blasfeo/include/ \ + -I $(INCLUDE_PATH)/hpipm/include/ \ + -I $(INCLUDE_PATH) \ + -L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB) \ + -L $(LIB_PATH) -lacados -lhpipm -lblasfeo \ + {{ link_libs }} \ + -lm \ + +{%- else %} + +ocp_shared_lib: casadi_fun ocp_solver + gcc $(ACADOS_FLAGS) -shared -o libacados_ocp_solver_{{ model.name }}.so $(OCP_OBJ) $(MODEL_OBJ) \ + -I $(INCLUDE_PATH)/blasfeo/include/ \ + -I $(INCLUDE_PATH)/hpipm/include/ \ + -I $(INCLUDE_PATH) \ + -L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB) \ + -L $(LIB_PATH) -lacados -lhpipm -lblasfeo \ + {{ link_libs }} \ + -lm \ + +{%- endif %} + +sim_shared_lib: casadi_fun sim_solver + gcc $(ACADOS_FLAGS) -shared -o libacados_sim_solver_{{ model.name }}.so $(SIM_OBJ) $(MODEL_OBJ) -L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB) \ + -L $(LIB_PATH) -lacados -lhpipm -lblasfeo \ + {{ link_libs }} \ + -lm \ + +{%- 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 + +{%- else %} + +clean: + rm -f *.o + rm -f *.so + rm -f main_{{ model.name }} + +clean_ocp_shared_lib: + rm -f libacados_ocp_solver_{{ model.name }}.so + rm -f acados_solver_{{ model.name }}.o + +{%- endif %} diff --git a/pyextra/acados_template/c_templates_tera/acados_mex_create.in.c b/pyextra/acados_template/c_templates_tera/acados_mex_create.in.c new file mode 100644 index 0000000000..7fa08d8f73 --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/acados_mex_create.in.c @@ -0,0 +1,383 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +// standard +#include +#include +#include + +// acados +#include "acados/utils/print.h" +#include "acados_c/ocp_nlp_interface.h" +#include "acados_solver_{{ model.name }}.h" + +// mex +#include "mex.h" + + +void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) +{ + long long *l_ptr; + int status = 0; + + // create solver + nlp_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 *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" %} + 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; + 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; + l_ptr = mxGetData(gnsf_get_matrices_fun_mat); + l_ptr[0] = (long long) acados_ocp_capsule->gnsf_get_matrices_fun; +{% elif solver_options.integrator_type == "DISCRETE" %} + l_ptr = mxGetData(disc_phi_fun_mat); + l_ptr[0] = (long long) acados_ocp_capsule->discr_dyn_phi_fun; + l_ptr = mxGetData(disc_phi_fun_jac_mat); + l_ptr[0] = (long long) acados_ocp_capsule->discr_dyn_phi_fun_jac_ut_xt; +{% if solver_options.hessian_approx == "EXACT" %} + l_ptr = mxGetData(disc_phi_fun_jac_hess_mat); + l_ptr[0] = (long long) acados_ocp_capsule->discr_dyn_phi_fun_jac_ut_xt_hess; +{%- endif %} +{%- endif %} + mxSetField(plhs[1], 0, "expl_ode_fun", expl_ode_fun_mat); + mxSetField(plhs[1], 0, "forw_vde", forw_vde_mat); + mxSetField(plhs[1], 0, "hess_vde", hess_vde_mat); + + mxSetField(plhs[1], 0, "gnsf_phi_fun", gnsf_phi_fun_mat); + mxSetField(plhs[1], 0, "gnsf_phi_fun_jac_y", gnsf_phi_fun_jac_y_mat); + mxSetField(plhs[1], 0, "gnsf_phi_jac_y_uhat", gnsf_phi_jac_y_uhat_mat); + mxSetField(plhs[1], 0, "gnsf_f_lo_jac_x1_x1dot_u_z", gnsf_f_lo_jac_x1_x1dot_u_z_mat); + mxSetField(plhs[1], 0, "gnsf_get_matrices_fun", gnsf_get_matrices_fun_mat); + + mxSetField(plhs[1], 0, "impl_dae_fun", impl_dae_fun_mat); + mxSetField(plhs[1], 0, "impl_dae_fun_jac_x_xdot_z", impl_dae_fun_jac_x_xdot_z_mat); + mxSetField(plhs[1], 0, "impl_dae_jac_x_xdot_u_z", impl_dae_jac_x_xdot_u_z_mat); + mxSetField(plhs[1], 0, "impl_dae_hess", impl_dae_hess_mat); + + mxSetField(plhs[1], 0, "disc_phi_fun", disc_phi_fun_mat); + mxSetField(plhs[1], 0, "disc_phi_fun_jac", disc_phi_fun_jac_mat); + mxSetField(plhs[1], 0, "disc_phi_fun_jac_hess", disc_phi_fun_jac_hess_mat); +/* constaints */ + mxArray *phi_constraint_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL); + l_ptr = mxGetData(phi_constraint_mat); +{%- if constraints.constr_type == "BGP" %} + l_ptr[0] = (long long) acados_ocp_capsule->phi_constraint; +{% endif %} +{% if constraints.constr_type_e == "BGP" %} + l_ptr[1] = (long long) &acados_ocp_capsule->phi_e_constraint; +{% endif %} + mxSetField(plhs[1], 0, "phi_constraint", phi_constraint_mat); + + mxArray *nl_constr_h_fun_jac_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL); + l_ptr = mxGetData(nl_constr_h_fun_jac_mat); +{% if constraints.constr_type == "BGH" and dims.nh > 0 %} + l_ptr[0] = (long long) acados_ocp_capsule->nl_constr_h_fun_jac; +{% endif %} +{% if constraints.constr_type_e == "BGH" and dims.nh_e > 0 %} + l_ptr[1] = (long long) &acados_ocp_capsule->nl_constr_h_e_fun_jac; +{%- endif %} + mxSetField(plhs[1], 0, "nl_constr_h_fun_jac", nl_constr_h_fun_jac_mat); + + mxArray *nl_constr_h_fun_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL); + l_ptr = mxGetData(nl_constr_h_fun_mat); +{% if constraints.constr_type == "BGH" and dims.nh > 0 %} + l_ptr[0] = (long long) acados_ocp_capsule->nl_constr_h_fun; +{% endif %} +{% if constraints.constr_type_e == "BGH" and dims.nh_e > 0 %} + l_ptr[1] = (long long) &acados_ocp_capsule->nl_constr_h_e_fun; +{%- endif %} + mxSetField(plhs[1], 0, "nl_constr_h_fun", nl_constr_h_fun_mat); + + mxArray *nl_constr_h_fun_jac_hess_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL); + l_ptr = mxGetData(nl_constr_h_fun_jac_hess_mat); +{% if constraints.constr_type == "BGH" and dims.nh > 0 and solver_options.hessian_approx == "EXACT" %} + l_ptr[0] = (long long) acados_ocp_capsule->nl_constr_h_fun_jac_hess; +{% endif %} +{% if constraints.constr_type_e == "BGH" and dims.nh_e > 0 and solver_options.hessian_approx == "EXACT" %} + l_ptr[1] = (long long) &acados_ocp_capsule->nl_constr_h_e_fun_jac_hess; +{%- endif %} + mxSetField(plhs[1], 0, "nl_constr_h_fun_jac_hess", nl_constr_h_fun_jac_hess_mat); + +/* cost */ + mxArray *cost_y_fun_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL); + l_ptr = mxGetData(cost_y_fun_mat); +{% if cost.cost_type == "NONLINEAR_LS" %} + l_ptr[0] = (long long) acados_ocp_capsule->cost_y_fun; +{% endif %} +{% if cost.cost_type_e == "NONLINEAR_LS" %} + l_ptr[1] = (long long) &acados_ocp_capsule->cost_y_e_fun; +{%- endif %} + mxSetField(plhs[1], 0, "cost_y_fun", cost_y_fun_mat); + + mxArray *cost_y_fun_jac_ut_xt_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL); + l_ptr = mxGetData(cost_y_fun_jac_ut_xt_mat); +{% if cost.cost_type == "NONLINEAR_LS" %} + l_ptr[0] = (long long) acados_ocp_capsule->cost_y_fun_jac_ut_xt; +{% endif %} +{% if cost.cost_type_e == "NONLINEAR_LS" %} + l_ptr[1] = (long long) &acados_ocp_capsule->cost_y_e_fun_jac_ut_xt; +{%- endif %} + mxSetField(plhs[1], 0, "cost_y_fun_jac_ut_xt", cost_y_fun_jac_ut_xt_mat); + + mxArray *cost_y_hess_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL); + l_ptr = mxGetData(cost_y_hess_mat); +{% if cost.cost_type == "NONLINEAR_LS" %} + l_ptr[0] = (long long) acados_ocp_capsule->cost_y_hess; +{% endif %} +{% if cost.cost_type_e == "NONLINEAR_LS" %} + l_ptr[1] = (long long) &acados_ocp_capsule->cost_y_e_hess; +{%- endif %} + mxSetField(plhs[1], 0, "cost_y_hess", cost_y_hess_mat); + + mxArray *ext_cost_fun_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL); + l_ptr = mxGetData(ext_cost_fun_mat); +{% if cost.cost_type == "EXTERNAL" %} + l_ptr[0] = (long long) acados_ocp_capsule->ext_cost_fun; +{% endif -%} +{% if cost.cost_type_e == "EXTERNAL" %} + l_ptr[1] = (long long) &acados_ocp_capsule->ext_cost_e_fun; +{%- endif %} + mxSetField(plhs[1], 0, "ext_cost_fun", ext_cost_fun_mat); + + mxArray *ext_cost_fun_jac_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL); + l_ptr = mxGetData(ext_cost_fun_jac_mat); +{% if cost.cost_type == "EXTERNAL" %} + l_ptr[0] = (long long) acados_ocp_capsule->ext_cost_fun_jac; +{% endif -%} +{% if cost.cost_type_e == "EXTERNAL" %} + l_ptr[1] = (long long) &acados_ocp_capsule->ext_cost_e_fun_jac; +{%- endif %} + mxSetField(plhs[1], 0, "ext_cost_fun_jac", ext_cost_fun_jac_mat); + + mxArray *ext_cost_fun_jac_hess_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL); + l_ptr = mxGetData(ext_cost_fun_jac_hess_mat); +{% if cost.cost_type == "EXTERNAL" %} + l_ptr[0] = (long long) acados_ocp_capsule->ext_cost_fun_jac_hess; +{% endif -%} +{% if cost.cost_type_e == "EXTERNAL" %} + l_ptr[1] = (long long) &acados_ocp_capsule->ext_cost_e_fun_jac_hess; +{%- endif %} + mxSetField(plhs[1], 0, "ext_cost_fun_jac_hess", ext_cost_fun_jac_hess_mat); + + + return; +} diff --git a/pyextra/acados_template/c_templates_tera/acados_mex_free.in.c b/pyextra/acados_template/c_templates_tera/acados_mex_free.in.c new file mode 100644 index 0000000000..843440297c --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/acados_mex_free.in.c @@ -0,0 +1,70 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + +// system +#include +#include +#include +// acados +#include "acados_solver_{{ model.name }}.h" + +// mex +#include "mex.h" + + +void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) +{ + int status = 0; + long long *ptr; + + // mexPrintf("\nin mex_acados_free\n"); + const mxArray *C_ocp = prhs[0]; + // capsule + ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "capsule" ) ); + nlp_solver_capsule *capsule = (nlp_solver_capsule *) ptr[0]; + + status = {{ model.name }}_acados_free(capsule); + if (status) + { + mexPrintf("{{ model.name }}_acados_free() returned status %d.\n", status); + } + + status = {{ model.name }}_acados_free_capsule(capsule); + if (status) + { + mexPrintf("{{ model.name }}_acados_free_capsule() returned status %d.\n", status); + } + + return; +} + diff --git a/pyextra/acados_template/c_templates_tera/acados_mex_set.in.c b/pyextra/acados_template/c_templates_tera/acados_mex_set.in.c new file mode 100644 index 0000000000..ee86a31164 --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/acados_mex_set.in.c @@ -0,0 +1,570 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +// standard +#include +#include +#include + +// acados +#include "acados/utils/print.h" +#include "acados_c/ocp_nlp_interface.h" +#include "acados_solver_{{ model.name }}.h" + +// mex +#include "mex.h" +#include "mex_macros.h" + + +void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) +{ + + long long *ptr; + int acados_size; + mxArray *mex_field; + char fun_name[20] = "ocp_set"; + char buffer [500]; // for error messages + + /* RHS */ + int min_nrhs = 6; + + char *ext_fun_type = mxArrayToString( prhs[0] ); + char *ext_fun_type_e = mxArrayToString( prhs[1] ); + + // C ocp + const mxArray *C_ocp = prhs[2]; + // capsule + ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "capsule" ) ); + nlp_solver_capsule *capsule = (nlp_solver_capsule *) ptr[0]; + // plan + ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "plan" ) ); + ocp_nlp_plan *plan = (ocp_nlp_plan *) ptr[0]; + // config + ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "config" ) ); + ocp_nlp_config *config = (ocp_nlp_config *) ptr[0]; + // dims + ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "dims" ) ); + ocp_nlp_dims *dims = (ocp_nlp_dims *) ptr[0]; + // opts + ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "opts" ) ); + void *opts = (void *) ptr[0]; + // in + ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "in" ) ); + ocp_nlp_in *in = (ocp_nlp_in *) ptr[0]; + // out + ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "out" ) ); + ocp_nlp_out *out = (ocp_nlp_out *) ptr[0]; + // solver + ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "solver" ) ); + ocp_nlp_solver *solver = (ocp_nlp_solver *) ptr[0]; + + const mxArray *C_ext_fun_pointers = prhs[3]; + // field + char *field = mxArrayToString( prhs[4] ); + // value + double *value = mxGetPr( prhs[5] ); + + // for checks + int matlab_size = (int) mxGetNumberOfElements( prhs[5] ); + int nrow = (int) mxGetM( prhs[5] ); + int ncol = (int) mxGetN( prhs[5] ); + + int N = dims->N; + int nu = dims->nu[0]; + int nx = dims->nx[0]; + + // stage + int s0, se; + if (nrhs==min_nrhs) + { + s0 = 0; + se = N; + } + else if (nrhs==min_nrhs+1) + { + s0 = mxGetScalar( prhs[6] ); + if (s0 > N) + { + sprintf(buffer, "ocp_set: N < specified stage = %d\n", s0); + mexErrMsgTxt(buffer); + } + se = s0 + 1; + } + else + { + sprintf(buffer, "ocp_set: wrong nrhs: %d\n", nrhs); + mexErrMsgTxt(buffer); + } + + /* Set value */ + // constraints + if (!strcmp(field, "constr_x0")) + { + acados_size = nx; + MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); + ocp_nlp_constraints_model_set(config, dims, in, 0, "lbx", value); + ocp_nlp_constraints_model_set(config, dims, in, 0, "ubx", value); + } + else if (!strcmp(field, "constr_C")) + { + for (int ii=s0; iinlp_cost[ii] == LINEAR_LS) || (plan->nlp_cost[ii] == NONLINEAR_LS)) + { + acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "y_ref"); + MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); + ocp_nlp_cost_model_set(config, dims, in, ii, "y_ref", value); + } + else + { + MEX_FIELD_NOT_SUPPORTED_FOR_COST_STAGE(fun_name, field, plan->nlp_cost[ii], ii); + } + } + } + else if (!strcmp(field, "cost_y_ref_e")) + { + acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, N, "y_ref"); + MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); + ocp_nlp_cost_model_set(config, dims, in, N, "y_ref", value); + } + else if (!strcmp(field, "cost_Vu")) + { + for (int ii=s0; iinlp_cost[ii] == LINEAR_LS) || (plan->nlp_cost[ii] == NONLINEAR_LS)) + { + int ny = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "y_ref"); + int nu = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "u"); + acados_size = ny * nu; + MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); + ocp_nlp_cost_model_set(config, dims, in, ii, "Vu", value); + } + else + { + MEX_FIELD_NOT_SUPPORTED_FOR_COST_STAGE(fun_name, field, plan->nlp_cost[ii], ii); + } + } + } + else if (!strcmp(field, "cost_Vx")) + { + for (int ii=s0; iinlp_cost[ii] == LINEAR_LS) || (plan->nlp_cost[ii] == NONLINEAR_LS)) + { + int ny = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "y_ref"); + int nx = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "x"); + acados_size = ny * nx; + MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); + ocp_nlp_cost_model_set(config, dims, in, ii, "Vx", value); + } + else + { + MEX_FIELD_NOT_SUPPORTED_FOR_COST_STAGE(fun_name, field, plan->nlp_cost[ii], ii); + } + } + } + else if (!strcmp(field, "cost_W")) + { + for (int ii=s0; iinlp_cost[ii] == LINEAR_LS) || (plan->nlp_cost[ii] == NONLINEAR_LS)) + { + int ny = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "y_ref"); + acados_size = ny * ny; + MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); + ocp_nlp_cost_model_set(config, dims, in, ii, "W", value); + } + else + { + MEX_FIELD_NOT_SUPPORTED_FOR_COST_STAGE(fun_name, field, plan->nlp_cost[ii], ii); + } + } + } + else if (!strcmp(field, "cost_Z")) + { + acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "cost_Z"); + MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); + for (int ii=s0; iisim_solver_plan[0]; + sim_solver_t type = sim_plan.sim_solver; + if (type == IRK) + { + int nz = ocp_nlp_dims_get_from_attr(config, dims, out, 0, "z"); + if (nrhs!=min_nrhs) + MEX_SETTER_NO_STAGE_SUPPORT(fun_name, field) + + acados_size = N*nz; + MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); + for (int ii=0; iisim_solver_plan[0]; + sim_solver_t type = sim_plan.sim_solver; + if (type == IRK) + { + int nx = ocp_nlp_dims_get_from_attr(config, dims, out, 0, "x"); + if (nrhs!=min_nrhs) + MEX_SETTER_NO_STAGE_SUPPORT(fun_name, field) + + acados_size = N*nx; + MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); + for (int ii=0; iisim_solver_plan[0]; + sim_solver_t type = sim_plan.sim_solver; + if (type == GNSF) + { + int nout = ocp_nlp_dims_get_from_attr(config, dims, out, 0, "init_gnsf_phi"); + + if (nrhs!=min_nrhs) + MEX_SETTER_NO_STAGE_SUPPORT(fun_name, field) + + acados_size = N*nout; + MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); + for (int ii=0; iinlp_solver == SQP && rti_phase != 0) + { + MEX_FIELD_ONLY_SUPPORTED_FOR_SOLVER(fun_name, field, "sqp_rti") + } + ocp_nlp_solver_opts_set(config, opts, "rti_phase", &rti_phase); + } + else if (!strcmp(field, "qp_warm_start")) + { + acados_size = 1; + MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); + int qp_warm_start = (int) value[0]; + ocp_nlp_solver_opts_set(config, opts, "qp_warm_start", &qp_warm_start); + } + else if (!strcmp(field, "warm_start_first_qp")) + { + acados_size = 1; + MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); + int warm_start_first_qp = (int) value[0]; + ocp_nlp_solver_opts_set(config, opts, "warm_start_first_qp", &warm_start_first_qp); + } + else if (!strcmp(field, "print_level")) + { + acados_size = 1; + MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); + int print_level = (int) value[0]; + ocp_nlp_solver_opts_set(config, opts, "print_level", &print_level); + } + else + { + MEX_FIELD_NOT_SUPPORTED_SUGGEST(fun_name, field, "p, constr_x0,\ + constr_lbx, constr_ubx, constr_C, constr_D, constr_lg, constr_ug, constr_lh, constr_uh\ + constr_lbu, constr_ubu, cost_y_ref[_e],\ + cost_Vu, cost_Vx, cost_Vz, cost_W, cost_Z, cost_Zl, cost_Zu, cost_z,\ + cost_zl, cost_zu, init_x, init_u, init_z, init_xdot, init_gnsf_phi,\ + init_pi, nlp_solver_max_iter, qp_warm_start, warm_start_first_qp, print_level"); + } + + return; +} + diff --git a/pyextra/acados_template/c_templates_tera/acados_mex_solve.in.c b/pyextra/acados_template/c_templates_tera/acados_mex_solve.in.c new file mode 100644 index 0000000000..9fd4feab29 --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/acados_mex_solve.in.c @@ -0,0 +1,59 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + +// system +#include +#include +#include +// acados +#include "acados_solver_{{ model.name }}.h" + +// mex +#include "mex.h" + + + +void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) +{ + // C_ocp + long long *ptr; + const mxArray *C_ocp = prhs[0]; + + // capsule + ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "capsule" ) ); + nlp_solver_capsule *capsule = (nlp_solver_capsule *) ptr[0]; + + // solve + {{ model.name }}_acados_solve(capsule); + +} diff --git a/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.c b/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.c new file mode 100644 index 0000000000..615978d02f --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.c @@ -0,0 +1,501 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + +{%- if solver_options.hessian_approx %} + {%- set hessian_approx = solver_options.hessian_approx %} +{%- elif solver_options.sens_hess %} + {%- set hessian_approx = "EXACT" %} +{%- else %} + {%- set hessian_approx = "GAUSS_NEWTON" %} +{%- endif %} +// standard +#include +#include + +// acados +#include "acados_c/external_function_interface.h" +#include "acados_c/sim_interface.h" +#include "acados_c/external_function_interface.h" + +#include "acados/sim/sim_common.h" +#include "acados/utils/external_function_generic.h" +#include "acados/utils/print.h" + + +// example specific +#include "{{ model.name }}_model/{{ model.name }}_model.h" +#include "acados_sim_solver_{{ model.name }}.h" + + +// ** solver data ** + +sim_solver_capsule * {{ model.name }}_acados_sim_solver_create_capsule() +{ + void* capsule_mem = malloc(sizeof(sim_solver_capsule)); + sim_solver_capsule *capsule = (sim_solver_capsule *) capsule_mem; + + return capsule; +} + + +int {{ model.name }}_acados_sim_solver_free_capsule(sim_solver_capsule * capsule) +{ + free(capsule); + return 0; +} + + +int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule) +{ + // initialize + int nx = {{ dims.nx }}; + int nu = {{ dims.nu }}; + int nz = {{ dims.nz }}; + + {#// 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, {{ dims.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, {{ dims.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, {{ dims.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, {{ dims.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, {{ dims.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, {{ dims.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, {{ dims.np }}); +{%- endif %} + + {% elif solver_options.integrator_type == "GNSF" -%} + 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)); + capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); + capsule->sim_gnsf_get_matrices_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); + + 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, {{ dims.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, {{ dims.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, {{ dims.np }}); + + 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, {{ dims.np }}); + + 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, {{ dims.np }}); + {% endif %} + + // sim plan & config + sim_solver_plan 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); + bool tmp_bool = {{ solver_options.sim_method_jac_reuse }}; + sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "jac_reuse", &tmp_bool); + +{% 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); +{% 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" %} + {{ 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); + {{ 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); + {{ 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; + + /* initialize parameter values */ + {% if dims.np > 0 %} + // initialize parameters to nominal value + double p[{{ dims.np }}]; + {% for i in range(end=dims.np) %} + p[{{ i }}] = {{ parameter_values[i] }}; + {%- endfor %} + +{%- 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" %} + 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); + capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z[0].set_param(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z, p); + capsule->sim_gnsf_get_matrices_fun[0].set_param(capsule->sim_gnsf_get_matrices_fun, p); +{% endif %} + {% 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" %} + 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); + external_function_param_casadi_free(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z); + 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 = {{ dims.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" %} + 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); + capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z[0].set_param(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z, p); + capsule->sim_gnsf_get_matrices_fun[0].set_param(capsule->sim_gnsf_get_matrices_fun, p); +{% endif %} + + return status; +} + +/* getters pointers to C objects*/ +sim_config * {{ model.name }}_acados_get_sim_config(sim_solver_capsule *capsule) +{ + return capsule->acados_sim_config; +}; + +sim_in * {{ model.name }}_acados_get_sim_in(sim_solver_capsule *capsule) +{ + return capsule->acados_sim_in; +}; + +sim_out * {{ model.name }}_acados_get_sim_out(sim_solver_capsule *capsule) +{ + return capsule->acados_sim_out; +}; + +void * {{ model.name }}_acados_get_sim_dims(sim_solver_capsule *capsule) +{ + return capsule->acados_sim_dims; +}; + +sim_opts * {{ model.name }}_acados_get_sim_opts(sim_solver_capsule *capsule) +{ + return capsule->acados_sim_opts; +}; + +sim_solver * {{ model.name }}_acados_get_sim_solver(sim_solver_capsule *capsule) +{ + return capsule->acados_sim_solver; +}; + diff --git a/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.h b/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.h new file mode 100644 index 0000000000..88a1787416 --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.h @@ -0,0 +1,98 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + +#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" + +#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; + + +int {{ model.name }}_acados_sim_create(sim_solver_capsule *capsule); +int {{ model.name }}_acados_sim_solve(sim_solver_capsule *capsule); +int {{ model.name }}_acados_sim_free(sim_solver_capsule *capsule); +int {{ model.name }}_acados_sim_update_params(sim_solver_capsule *capsule, double *value, int np); + +sim_config * {{ model.name }}_acados_get_sim_config(sim_solver_capsule *capsule); +sim_in * {{ model.name }}_acados_get_sim_in(sim_solver_capsule *capsule); +sim_out * {{ model.name }}_acados_get_sim_out(sim_solver_capsule *capsule); +void * {{ model.name }}_acados_get_sim_dims(sim_solver_capsule *capsule); +sim_opts * {{ model.name }}_acados_get_sim_opts(sim_solver_capsule *capsule); +sim_solver * {{ model.name }}_acados_get_sim_solver(sim_solver_capsule *capsule); + + +sim_solver_capsule * {{ model.name }}_acados_sim_solver_create_capsule(); +int {{ model.name }}_acados_sim_solver_free_capsule(sim_solver_capsule *capsule); + +#ifdef __cplusplus +} +#endif + +#endif // ACADOS_SIM_{{ model.name }}_H_ diff --git a/pyextra/acados_template/c_templates_tera/acados_sim_solver_sfun.in.c b/pyextra/acados_template/c_templates_tera/acados_sim_solver_sfun.in.c new file mode 100644 index 0000000000..234295fa29 --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/acados_sim_solver_sfun.in.c @@ -0,0 +1,233 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + +#define S_FUNCTION_NAME acados_sim_solver_sfunction_{{ model.name }} +#define S_FUNCTION_LEVEL 2 + +#define MDL_START + +// acados +#include "acados/utils/print.h" +#include "acados_c/ocp_nlp_interface.h" +#include "acados_c/external_function_interface.h" + +// example specific +#include "{{ model.name }}_model/{{ model.name }}_model.h" +#include "acados_sim_solver_{{ model.name }}.h" + +#include "simstruc.h" + +#define SAMPLINGTIME {{ solver_options.Tsim }} + + +static void mdlInitializeSizes (SimStruct *S) +{ + // specify the number of continuous and discrete states + ssSetNumContStates(S, 0); + ssSetNumDiscStates(S, 0); + + {# compute number of input ports #} + {%- set n_inputs = 1 %} {# x0 #} + {%- if dims.nu > 0 %} {# u0 -#} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif %} + {%- if dims.np > 0 %} {# parameters #} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif %} + + // specify the number of input ports + if ( !ssSetNumInputPorts(S, {{ n_inputs }}) ) + return; + + // specify the number of output ports + if ( !ssSetNumOutputPorts(S, 1) ) + return; + + // specify dimension information for the input ports + {%- set i_input = 0 %} + // x0 + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nx }}); + + {%- if dims.nu > 0 %} + {%- set i_input = i_input + 1 %} + // u0 + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nu }}); + {%- endif %} + + {%- if dims.np > 0 %} + {%- set i_input = i_input + 1 %} + // parameters + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.np }}); + {%- endif %} + + // specify dimension information for the output ports + ssSetOutputPortVectorDimension(S, 0, {{ dims.nx }} ); // xnext + + // specify the direct feedthrough status + // should be set to 1 for all inputs used in mdlOutputs + {%- for i in range(end=n_inputs) %} + ssSetInputPortDirectFeedThrough(S, {{ i }}, 1); + {%- endfor %} + + // one sample time + ssSetNumSampleTimes(S, 1); +} + + +#if defined(MATLAB_MEX_FILE) + +#define MDL_SET_INPUT_PORT_DIMENSION_INFO +#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO + +static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo) +{ + if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) ) + return; +} + +static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo) +{ + if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) ) + return; +} + +#endif /* MATLAB_MEX_FILE */ + + +static void mdlInitializeSampleTimes(SimStruct *S) +{ + ssSetSampleTime(S, 0, SAMPLINGTIME); + ssSetOffsetTime(S, 0, 0.0); +} + + +static void mdlStart(SimStruct *S) +{ + sim_solver_capsule *capsule = {{ model.name }}_acados_sim_solver_create_capsule(); + {{ model.name }}_acados_sim_create(capsule); + + ssSetUserData(S, (void*)capsule); +} + +static void mdlOutputs(SimStruct *S, int_T tid) +{ + sim_solver_capsule *capsule = ssGetUserData(S); + + sim_config *acados_sim_config = {{ model.name }}_acados_get_sim_config(capsule); + sim_in *acados_sim_in = {{ model.name }}_acados_get_sim_in(capsule); + sim_out *acados_sim_out = {{ model.name }}_acados_get_sim_out(capsule); + void *acados_sim_dims = {{ model.name }}_acados_get_sim_dims(capsule); + // sim_opts * {{ model.name }}_acados_get_sim_opts(capsule); + // sim_solver * {{ model.name }}_acados_get_sim_solver(capsule); + + InputRealPtrsType in_sign; + {% set input_sizes = [dims.nx, dims.nu, dims.np] %} + + // local buffer + {%- set buffer_size = input_sizes | sort | last %} + real_t buffer[{{ buffer_size }}]; + + + /* go through inputs */ + {%- set i_input = 0 %} + // initial condition + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + for (int i = 0; i < {{ dims.nx }}; i++) + buffer[i] = (double)(*in_sign[i]); + + sim_in_set(acados_sim_config, acados_sim_dims, + acados_sim_in, "x", buffer); + + + // ssPrintf("\nin acados sim:\n"); + // for (int i = 0; i < {{ dims.nx }}; i++) ssPrintf("x0[%d] = %f\n", i, buffer[i]); + // ssPrintf("\n"); + +{% if dims.nu > 0 %} + // control input - u + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + + for (int i = 0; i < {{ dims.nu }}; i++) + buffer[i] = (double)(*in_sign[i]); + + sim_in_set(acados_sim_config, acados_sim_dims, + acados_sim_in, "u", buffer); +{%- endif %} + + +{% if dims.np > 0 %} + // parameters + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + + for (int i = 0; i < {{ dims.np }}; i++) + buffer[i] = (double)(*in_sign[i]); + + // update value of parameters + {{ model.name }}_acados_sim_update_params(capsule, buffer, {{ dims.np }}); +{%- endif %} + + + /* call solver */ + int acados_status = {{ model.name }}_acados_sim_solve(capsule); + + + /* set outputs */ + real_t *out_x = ssGetOutputPortRealSignal(S, 0); + + // get simulated state + sim_out_get(acados_sim_config, acados_sim_dims, acados_sim_out, + "xn", (void *) out_x); + + // ssPrintf("\nacados sim solve: returned %d\n", acados_status); + // for (int i = 0; i < {{ dims.nx }}; i++) ssPrintf("x_sim[%d] = %f\n", i, out_x[i]); + // ssPrintf("\n"); + +} + + +static void mdlTerminate(SimStruct *S) +{ + sim_solver_capsule *capsule = ssGetUserData(S); + + {{ model.name }}_acados_sim_free(capsule); + {{ model.name }}_acados_sim_solver_free_capsule(capsule); +} + + +#ifdef MATLAB_MEX_FILE +#include "simulink.c" +#else +#include "cg_sfun.h" +#endif diff --git a/pyextra/acados_template/c_templates_tera/acados_solver.in.c b/pyextra/acados_template/c_templates_tera/acados_solver.in.c new file mode 100644 index 0000000000..c79e826365 --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/acados_solver.in.c @@ -0,0 +1,2115 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + +// standard +#include +#include +// acados +#include "acados/utils/print.h" +#include "acados_c/ocp_nlp_interface.h" +#include "acados_c/external_function_interface.h" + +// example specific +#include "{{ model.name }}_model/{{ model.name }}_model.h" +{% if constraints.constr_type == "BGP" and dims.nphi %} +#include "{{ model.name }}_constraints/{{ model.name }}_phi_constraint.h" +{% endif %} +{% if constraints.constr_type_e == "BGP" and dims.nphi_e > 0 %} +#include "{{ model.name }}_constraints/{{ model.name }}_phi_e_constraint.h" +{% endif %} +{% if constraints.constr_type == "BGH" and dims.nh > 0 %} +#include "{{ model.name }}_constraints/{{ model.name }}_h_constraint.h" +{% endif %} +{% if constraints.constr_type_e == "BGH" and dims.nh_e > 0 %} +#include "{{ model.name }}_constraints/{{ model.name }}_h_e_constraint.h" +{% endif %} +{%- if cost.cost_type == "NONLINEAR_LS" %} +#include "{{ model.name }}_cost/{{ model.name }}_cost_y_fun.h" +{%- elif cost.cost_type == "EXTERNAL" %} +#include "{{ model.name }}_cost/{{ model.name }}_external_cost.h" +{%- endif %} +{%- if cost.cost_type_0 == "NONLINEAR_LS" %} +#include "{{ model.name }}_cost/{{ model.name }}_cost_y_0_fun.h" +{%- elif cost.cost_type_0 == "EXTERNAL" %} +#include "{{ model.name }}_cost/{{ model.name }}_external_cost_0.h" +{%- endif %} +{%- if cost.cost_type_e == "NONLINEAR_LS" %} +#include "{{ model.name }}_cost/{{ model.name }}_cost_y_e_fun.h" +{%- elif cost.cost_type_e == "EXTERNAL" %} +#include "{{ model.name }}_cost/{{ model.name }}_external_cost_e.h" +{%- endif %} + +#include "acados_solver_{{ model.name }}.h" + +#define NX {{ dims.nx }} +#define NZ {{ dims.nz }} +#define NU {{ dims.nu }} +#define NP {{ dims.np }} +#define NBX {{ dims.nbx }} +#define NBX0 {{ dims.nbx_0 }} +#define NBU {{ dims.nbu }} +#define NSBX {{ dims.nsbx }} +#define NSBU {{ dims.nsbu }} +#define NSH {{ dims.nsh }} +#define NSG {{ dims.nsg }} +#define NSPHI {{ dims.nsphi }} +#define NSHN {{ dims.nsh_e }} +#define NSGN {{ dims.nsg_e }} +#define NSPHIN {{ dims.nsphi_e }} +#define NSBXN {{ dims.nsbx_e }} +#define NS {{ dims.ns }} +#define NSN {{ dims.ns_e }} +#define NG {{ dims.ng }} +#define NBXN {{ dims.nbx_e }} +#define NGN {{ dims.ng_e }} +#define NY0 {{ dims.ny_0 }} +#define NY {{ dims.ny }} +#define NYN {{ dims.ny_e }} +#define N {{ dims.N }} +#define NH {{ dims.nh }} +#define NPHI {{ dims.nphi }} +#define NHN {{ dims.nh_e }} +#define NPHIN {{ dims.nphi_e }} +#define NR {{ dims.nr }} + + +// ** solver data ** + +nlp_solver_capsule * {{ model.name }}_acados_create_capsule() +{ + void* capsule_mem = malloc(sizeof(nlp_solver_capsule)); + nlp_solver_capsule *capsule = (nlp_solver_capsule *) capsule_mem; + + return capsule; +} + + +int {{ model.name }}_acados_free_capsule(nlp_solver_capsule *capsule) +{ + free(capsule); + return 0; +} + + +int {{ model.name }}_acados_create(nlp_solver_capsule * capsule) +{ + int status = 0; + + // number of expected runtime parameters + capsule->nlp_np = NP; + + /************************************************ + * plan & config + ************************************************/ + ocp_nlp_plan * nlp_solver_plan = ocp_nlp_plan_create(N); + capsule->nlp_solver_plan = nlp_solver_plan; + + {%- if solver_options.nlp_solver_type == "SQP" %} + nlp_solver_plan->nlp_solver = SQP; + {% else %} + nlp_solver_plan->nlp_solver = SQP_RTI; + {%- endif %} + + nlp_solver_plan->ocp_qp_solver_plan.qp_solver = {{ solver_options.qp_solver }}; + + nlp_solver_plan->nlp_cost[0] = {{ cost.cost_type_0 }}; + for (int i = 1; i < N; i++) + nlp_solver_plan->nlp_cost[i] = {{ cost.cost_type }}; + + nlp_solver_plan->nlp_cost[N] = {{ cost.cost_type_e }}; + + for (int i = 0; i < N; i++) + { + {% if solver_options.integrator_type == "DISCRETE" %} + nlp_solver_plan->nlp_dynamics[i] = DISCRETE_MODEL; + // discrete dynamics does not need sim solver option, this field is ignored + nlp_solver_plan->sim_solver_plan[i].sim_solver = INVALID_SIM_SOLVER; + {% else %} + nlp_solver_plan->nlp_dynamics[i] = CONTINUOUS_MODEL; + nlp_solver_plan->sim_solver_plan[i].sim_solver = {{ solver_options.integrator_type }}; + {%- endif %} + } + + for (int i = 0; i < N; i++) + { + {% if constraints.constr_type == "BGP" %} + nlp_solver_plan->nlp_constraints[i] = BGP; + {%- else -%} + nlp_solver_plan->nlp_constraints[i] = BGH; + {%- endif %} + } + + {%- if constraints.constr_type_e == "BGP" %} + nlp_solver_plan->nlp_constraints[N] = BGP; + {% else %} + nlp_solver_plan->nlp_constraints[N] = BGH; + {%- endif %} + +{%- if solver_options.hessian_approx == "EXACT" %} + {%- if solver_options.regularize_method == "NO_REGULARIZE" %} + nlp_solver_plan->regularization = NO_REGULARIZE; + {%- elif solver_options.regularize_method == "MIRROR" %} + nlp_solver_plan->regularization = MIRROR; + {%- elif solver_options.regularize_method == "PROJECT" %} + nlp_solver_plan->regularization = PROJECT; + {%- elif solver_options.regularize_method == "PROJECT_REDUC_HESS" %} + nlp_solver_plan->regularization = PROJECT_REDUC_HESS; + {%- elif solver_options.regularize_method == "CONVEXIFY" %} + nlp_solver_plan->regularization = CONVEXIFY; + {%- endif %} +{%- endif %} + ocp_nlp_config * nlp_config = ocp_nlp_config_create(*nlp_solver_plan); + capsule->nlp_config = nlp_config; + + + /************************************************ + * dimensions + ************************************************/ + int nx[N+1]; + int nu[N+1]; + int nbx[N+1]; + int nbu[N+1]; + int nsbx[N+1]; + int nsbu[N+1]; + int nsg[N+1]; + int nsh[N+1]; + int nsphi[N+1]; + int ns[N+1]; + int ng[N+1]; + int nh[N+1]; + int nphi[N+1]; + int nz[N+1]; + int ny[N+1]; + int nr[N+1]; + int nbxe[N+1]; + + for (int i = 0; i < N+1; i++) + { + // common + nx[i] = NX; + nu[i] = NU; + nz[i] = NZ; + ns[i] = NS; + // cost + ny[i] = NY; + // constraints + nbx[i] = NBX; + nbu[i] = NBU; + nsbx[i] = NSBX; + nsbu[i] = NSBU; + nsg[i] = NSG; + nsh[i] = NSH; + nsphi[i] = NSPHI; + ng[i] = NG; + nh[i] = NH; + nphi[i] = NPHI; + nr[i] = NR; + nbxe[i] = 0; + } + + // for initial state + nbx[0] = NBX0; + nsbx[0] = 0; + ns[0] = NS - NSBX; + nbxe[0] = {{ dims.nbxe_0 }}; + ny[0] = NY0; + + // terminal - common + nu[N] = 0; + nz[N] = 0; + ns[N] = NSN; + // cost + ny[N] = NYN; + // constraint + nbx[N] = NBXN; + nbu[N] = 0; + ng[N] = NGN; + nh[N] = NHN; + nphi[N] = NPHIN; + nr[N] = {{ dims.nr_e }}; + + nsbx[N] = NSBXN; + nsbu[N] = 0; + nsg[N] = NSGN; + nsh[N] = NSHN; + nsphi[N] = NSPHIN; + + /* create and set ocp_nlp_dims */ + ocp_nlp_dims * nlp_dims = ocp_nlp_dims_create(nlp_config); + capsule->nlp_dims = nlp_dims; + + ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "nx", nx); + ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "nu", nu); + ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "nz", nz); + ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "ns", ns); + + for (int i = 0; i <= N; i++) + { + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nbx", &nbx[i]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nbu", &nbu[i]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsbx", &nsbx[i]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsbu", &nsbu[i]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "ng", &ng[i]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsg", &nsg[i]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nbxe", &nbxe[i]); + } + + {%- if cost.cost_type_0 == "NONLINEAR_LS" or cost.cost_type_0 == "LINEAR_LS" %} + ocp_nlp_dims_set_cost(nlp_config, nlp_dims, 0, "ny", &ny[0]); + {%- endif %} + + {%- if cost.cost_type == "NONLINEAR_LS" or cost.cost_type == "LINEAR_LS" %} + for (int i = 1; i < N; i++) + ocp_nlp_dims_set_cost(nlp_config, nlp_dims, i, "ny", &ny[i]); + {%- endif %} + + for (int i = 0; i < N; i++) + { + {%- if constraints.constr_type == "BGH" and dims.nh > 0 %} + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nh", &nh[i]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsh", &nsh[i]); + {%- elif constraints.constr_type == "BGP" and dims.nphi > 0 %} + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nr", &nr[i]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nphi", &nphi[i]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsphi", &nsphi[i]); + {%- endif %} + } + + {%- if constraints.constr_type_e == "BGH" %} + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nh", &nh[N]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nsh", &nsh[N]); + {%- elif constraints.constr_type_e == "BGP" %} + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nr", &nr[N]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nphi", &nphi[N]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nsphi", &nsphi[N]); + {%- endif %} + {%- if cost.cost_type_e == "NONLINEAR_LS" or cost.cost_type_e == "LINEAR_LS" %} + ocp_nlp_dims_set_cost(nlp_config, nlp_dims, N, "ny", &ny[N]); + {%- endif %} + +{% if solver_options.integrator_type == "GNSF" -%} + // GNSF specific dimensions + int gnsf_nx1 = {{ dims.gnsf_nx1 }}; + int gnsf_nz1 = {{ dims.gnsf_nz1 }}; + int gnsf_nout = {{ dims.gnsf_nout }}; + int gnsf_ny = {{ dims.gnsf_ny }}; + int gnsf_nuhat = {{ dims.gnsf_nuhat }}; + + for (int i = 0; i < N; i++) + { + if (nlp_solver_plan->sim_solver_plan[i].sim_solver == GNSF) + { + ocp_nlp_dims_set_dynamics(nlp_config, nlp_dims, i, "gnsf_nx1", &gnsf_nx1); + ocp_nlp_dims_set_dynamics(nlp_config, nlp_dims, i, "gnsf_nz1", &gnsf_nz1); + ocp_nlp_dims_set_dynamics(nlp_config, nlp_dims, i, "gnsf_nout", &gnsf_nout); + ocp_nlp_dims_set_dynamics(nlp_config, nlp_dims, i, "gnsf_ny", &gnsf_ny); + ocp_nlp_dims_set_dynamics(nlp_config, nlp_dims, i, "gnsf_nuhat", &gnsf_nuhat); + } + } +{%- endif %} + + /************************************************ + * external functions + ************************************************/ + {%- if constraints.constr_type == "BGP" %} + capsule->phi_constraint = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N; i++) + { + // nonlinear part of convex-composite constraint + capsule->phi_constraint[i].casadi_fun = &{{ model.name }}_phi_constraint; + capsule->phi_constraint[i].casadi_n_in = &{{ model.name }}_phi_constraint_n_in; + capsule->phi_constraint[i].casadi_n_out = &{{ model.name }}_phi_constraint_n_out; + capsule->phi_constraint[i].casadi_sparsity_in = &{{ model.name }}_phi_constraint_sparsity_in; + capsule->phi_constraint[i].casadi_sparsity_out = &{{ model.name }}_phi_constraint_sparsity_out; + capsule->phi_constraint[i].casadi_work = &{{ model.name }}_phi_constraint_work; + + external_function_param_casadi_create(&capsule->phi_constraint[i], {{ dims.np }}); + } + {%- endif %} + + {%- if constraints.constr_type_e == "BGP" %} + // nonlinear part of convex-composite constraint + capsule->phi_e_constraint.casadi_fun = &{{ model.name }}_phi_e_constraint; + capsule->phi_e_constraint.casadi_n_in = &{{ model.name }}_phi_e_constraint_n_in; + capsule->phi_e_constraint.casadi_n_out = &{{ model.name }}_phi_e_constraint_n_out; + capsule->phi_e_constraint.casadi_sparsity_in = &{{ model.name }}_phi_e_constraint_sparsity_in; + capsule->phi_e_constraint.casadi_sparsity_out = &{{ model.name }}_phi_e_constraint_sparsity_out; + capsule->phi_e_constraint.casadi_work = &{{ model.name }}_phi_e_constraint_work; + + external_function_param_casadi_create(&capsule->phi_e_constraint, {{ dims.np }}); + {% endif %} + + {%- if constraints.constr_type == "BGH" and dims.nh > 0 %} + capsule->nl_constr_h_fun_jac = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N; i++) { + capsule->nl_constr_h_fun_jac[i].casadi_fun = &{{ model.name }}_constr_h_fun_jac_uxt_zt; + capsule->nl_constr_h_fun_jac[i].casadi_n_in = &{{ model.name }}_constr_h_fun_jac_uxt_zt_n_in; + capsule->nl_constr_h_fun_jac[i].casadi_n_out = &{{ model.name }}_constr_h_fun_jac_uxt_zt_n_out; + capsule->nl_constr_h_fun_jac[i].casadi_sparsity_in = &{{ model.name }}_constr_h_fun_jac_uxt_zt_sparsity_in; + capsule->nl_constr_h_fun_jac[i].casadi_sparsity_out = &{{ model.name }}_constr_h_fun_jac_uxt_zt_sparsity_out; + capsule->nl_constr_h_fun_jac[i].casadi_work = &{{ model.name }}_constr_h_fun_jac_uxt_zt_work; + external_function_param_casadi_create(&capsule->nl_constr_h_fun_jac[i], {{ dims.np }}); + } + capsule->nl_constr_h_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N; i++) { + capsule->nl_constr_h_fun[i].casadi_fun = &{{ model.name }}_constr_h_fun; + capsule->nl_constr_h_fun[i].casadi_n_in = &{{ model.name }}_constr_h_fun_n_in; + capsule->nl_constr_h_fun[i].casadi_n_out = &{{ model.name }}_constr_h_fun_n_out; + capsule->nl_constr_h_fun[i].casadi_sparsity_in = &{{ model.name }}_constr_h_fun_sparsity_in; + capsule->nl_constr_h_fun[i].casadi_sparsity_out = &{{ model.name }}_constr_h_fun_sparsity_out; + capsule->nl_constr_h_fun[i].casadi_work = &{{ model.name }}_constr_h_fun_work; + external_function_param_casadi_create(&capsule->nl_constr_h_fun[i], {{ dims.np }}); + } + {% if solver_options.hessian_approx == "EXACT" %} + capsule->nl_constr_h_fun_jac_hess = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N; i++) { + capsule->nl_constr_h_fun_jac_hess[i].casadi_fun = &{{ model.name }}_constr_h_fun_jac_uxt_zt_hess; + capsule->nl_constr_h_fun_jac_hess[i].casadi_n_in = &{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_n_in; + capsule->nl_constr_h_fun_jac_hess[i].casadi_n_out = &{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_n_out; + capsule->nl_constr_h_fun_jac_hess[i].casadi_sparsity_in = &{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_sparsity_in; + capsule->nl_constr_h_fun_jac_hess[i].casadi_sparsity_out = &{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_sparsity_out; + capsule->nl_constr_h_fun_jac_hess[i].casadi_work = &{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_work; + + external_function_param_casadi_create(&capsule->nl_constr_h_fun_jac_hess[i], {{ dims.np }}); + } + {% endif %} + {% endif %} + + {%- if constraints.constr_type_e == "BGH" and dims.nh_e > 0 %} + capsule->nl_constr_h_e_fun_jac.casadi_fun = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt; + capsule->nl_constr_h_e_fun_jac.casadi_n_in = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_n_in; + capsule->nl_constr_h_e_fun_jac.casadi_n_out = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_n_out; + capsule->nl_constr_h_e_fun_jac.casadi_sparsity_in = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_sparsity_in; + capsule->nl_constr_h_e_fun_jac.casadi_sparsity_out = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_sparsity_out; + capsule->nl_constr_h_e_fun_jac.casadi_work = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_work; + external_function_param_casadi_create(&capsule->nl_constr_h_e_fun_jac, {{ dims.np }}); + + capsule->nl_constr_h_e_fun.casadi_fun = &{{ model.name }}_constr_h_e_fun; + capsule->nl_constr_h_e_fun.casadi_n_in = &{{ model.name }}_constr_h_e_fun_n_in; + capsule->nl_constr_h_e_fun.casadi_n_out = &{{ model.name }}_constr_h_e_fun_n_out; + capsule->nl_constr_h_e_fun.casadi_sparsity_in = &{{ model.name }}_constr_h_e_fun_sparsity_in; + capsule->nl_constr_h_e_fun.casadi_sparsity_out = &{{ model.name }}_constr_h_e_fun_sparsity_out; + capsule->nl_constr_h_e_fun.casadi_work = &{{ model.name }}_constr_h_e_fun_work; + external_function_param_casadi_create(&capsule->nl_constr_h_e_fun, {{ dims.np }}); + + {% if solver_options.hessian_approx == "EXACT" %} + capsule->nl_constr_h_e_fun_jac_hess.casadi_fun = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess; + capsule->nl_constr_h_e_fun_jac_hess.casadi_n_in = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_n_in; + capsule->nl_constr_h_e_fun_jac_hess.casadi_n_out = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_n_out; + capsule->nl_constr_h_e_fun_jac_hess.casadi_sparsity_in = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_sparsity_in; + capsule->nl_constr_h_e_fun_jac_hess.casadi_sparsity_out = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_sparsity_out; + capsule->nl_constr_h_e_fun_jac_hess.casadi_work = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_work; + external_function_param_casadi_create(&capsule->nl_constr_h_e_fun_jac_hess, {{ dims.np }}); + {% endif %} + {%- endif %} + +{% if solver_options.integrator_type == "ERK" %} + // explicit ode + capsule->forw_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N; i++) { + capsule->forw_vde_casadi[i].casadi_fun = &{{ model.name }}_expl_vde_forw; + capsule->forw_vde_casadi[i].casadi_n_in = &{{ model.name }}_expl_vde_forw_n_in; + capsule->forw_vde_casadi[i].casadi_n_out = &{{ model.name }}_expl_vde_forw_n_out; + capsule->forw_vde_casadi[i].casadi_sparsity_in = &{{ model.name }}_expl_vde_forw_sparsity_in; + capsule->forw_vde_casadi[i].casadi_sparsity_out = &{{ model.name }}_expl_vde_forw_sparsity_out; + capsule->forw_vde_casadi[i].casadi_work = &{{ model.name }}_expl_vde_forw_work; + external_function_param_casadi_create(&capsule->forw_vde_casadi[i], {{ dims.np }}); + } + + capsule->expl_ode_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N; i++) { + capsule->expl_ode_fun[i].casadi_fun = &{{ model.name }}_expl_ode_fun; + capsule->expl_ode_fun[i].casadi_n_in = &{{ model.name }}_expl_ode_fun_n_in; + capsule->expl_ode_fun[i].casadi_n_out = &{{ model.name }}_expl_ode_fun_n_out; + capsule->expl_ode_fun[i].casadi_sparsity_in = &{{ model.name }}_expl_ode_fun_sparsity_in; + capsule->expl_ode_fun[i].casadi_sparsity_out = &{{ model.name }}_expl_ode_fun_sparsity_out; + capsule->expl_ode_fun[i].casadi_work = &{{ model.name }}_expl_ode_fun_work; + external_function_param_casadi_create(&capsule->expl_ode_fun[i], {{ dims.np }}); + } + + {%- if solver_options.hessian_approx == "EXACT" %} + capsule->hess_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N; i++) { + capsule->hess_vde_casadi[i].casadi_fun = &{{ model.name }}_expl_ode_hess; + capsule->hess_vde_casadi[i].casadi_n_in = &{{ model.name }}_expl_ode_hess_n_in; + capsule->hess_vde_casadi[i].casadi_n_out = &{{ model.name }}_expl_ode_hess_n_out; + capsule->hess_vde_casadi[i].casadi_sparsity_in = &{{ model.name }}_expl_ode_hess_sparsity_in; + capsule->hess_vde_casadi[i].casadi_sparsity_out = &{{ model.name }}_expl_ode_hess_sparsity_out; + capsule->hess_vde_casadi[i].casadi_work = &{{ model.name }}_expl_ode_hess_work; + external_function_param_casadi_create(&capsule->hess_vde_casadi[i], {{ dims.np }}); + } + {%- endif %} + +{% elif solver_options.integrator_type == "IRK" %} + // implicit dae + capsule->impl_dae_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N; i++) { + capsule->impl_dae_fun[i].casadi_fun = &{{ model.name }}_impl_dae_fun; + capsule->impl_dae_fun[i].casadi_work = &{{ model.name }}_impl_dae_fun_work; + capsule->impl_dae_fun[i].casadi_sparsity_in = &{{ model.name }}_impl_dae_fun_sparsity_in; + capsule->impl_dae_fun[i].casadi_sparsity_out = &{{ model.name }}_impl_dae_fun_sparsity_out; + capsule->impl_dae_fun[i].casadi_n_in = &{{ model.name }}_impl_dae_fun_n_in; + capsule->impl_dae_fun[i].casadi_n_out = &{{ model.name }}_impl_dae_fun_n_out; + external_function_param_casadi_create(&capsule->impl_dae_fun[i], {{ dims.np }}); + } + + capsule->impl_dae_fun_jac_x_xdot_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N; i++) { + capsule->impl_dae_fun_jac_x_xdot_z[i].casadi_fun = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z; + capsule->impl_dae_fun_jac_x_xdot_z[i].casadi_work = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_work; + capsule->impl_dae_fun_jac_x_xdot_z[i].casadi_sparsity_in = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_sparsity_in; + capsule->impl_dae_fun_jac_x_xdot_z[i].casadi_sparsity_out = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_sparsity_out; + capsule->impl_dae_fun_jac_x_xdot_z[i].casadi_n_in = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_in; + capsule->impl_dae_fun_jac_x_xdot_z[i].casadi_n_out = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_out; + external_function_param_casadi_create(&capsule->impl_dae_fun_jac_x_xdot_z[i], {{ dims.np }}); + } + + capsule->impl_dae_jac_x_xdot_u_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N; i++) { + capsule->impl_dae_jac_x_xdot_u_z[i].casadi_fun = &{{ model.name }}_impl_dae_jac_x_xdot_u_z; + capsule->impl_dae_jac_x_xdot_u_z[i].casadi_work = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_work; + capsule->impl_dae_jac_x_xdot_u_z[i].casadi_sparsity_in = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_sparsity_in; + capsule->impl_dae_jac_x_xdot_u_z[i].casadi_sparsity_out = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_sparsity_out; + capsule->impl_dae_jac_x_xdot_u_z[i].casadi_n_in = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_n_in; + capsule->impl_dae_jac_x_xdot_u_z[i].casadi_n_out = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_n_out; + external_function_param_casadi_create(&capsule->impl_dae_jac_x_xdot_u_z[i], {{ dims.np }}); + } + + {%- if solver_options.hessian_approx == "EXACT" %} + capsule->impl_dae_hess = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N; i++) { + capsule->impl_dae_hess[i].casadi_fun = &{{ model.name }}_impl_dae_hess; + capsule->impl_dae_hess[i].casadi_work = &{{ model.name }}_impl_dae_hess_work; + capsule->impl_dae_hess[i].casadi_sparsity_in = &{{ model.name }}_impl_dae_hess_sparsity_in; + capsule->impl_dae_hess[i].casadi_sparsity_out = &{{ model.name }}_impl_dae_hess_sparsity_out; + capsule->impl_dae_hess[i].casadi_n_in = &{{ model.name }}_impl_dae_hess_n_in; + capsule->impl_dae_hess[i].casadi_n_out = &{{ model.name }}_impl_dae_hess_n_out; + external_function_param_casadi_create(&capsule->impl_dae_hess[i], {{ dims.np }}); + } + {%- endif %} + +{% elif solver_options.integrator_type == "GNSF" %} + capsule->gnsf_phi_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N; i++) { + capsule->gnsf_phi_fun[i].casadi_fun = &{{ model.name }}_gnsf_phi_fun; + capsule->gnsf_phi_fun[i].casadi_work = &{{ model.name }}_gnsf_phi_fun_work; + capsule->gnsf_phi_fun[i].casadi_sparsity_in = &{{ model.name }}_gnsf_phi_fun_sparsity_in; + capsule->gnsf_phi_fun[i].casadi_sparsity_out = &{{ model.name }}_gnsf_phi_fun_sparsity_out; + capsule->gnsf_phi_fun[i].casadi_n_in = &{{ model.name }}_gnsf_phi_fun_n_in; + capsule->gnsf_phi_fun[i].casadi_n_out = &{{ model.name }}_gnsf_phi_fun_n_out; + external_function_param_casadi_create(&capsule->gnsf_phi_fun[i], {{ dims.np }}); + } + + capsule->gnsf_phi_fun_jac_y = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N; i++) { + capsule->gnsf_phi_fun_jac_y[i].casadi_fun = &{{ model.name }}_gnsf_phi_fun_jac_y; + capsule->gnsf_phi_fun_jac_y[i].casadi_work = &{{ model.name }}_gnsf_phi_fun_jac_y_work; + capsule->gnsf_phi_fun_jac_y[i].casadi_sparsity_in = &{{ model.name }}_gnsf_phi_fun_jac_y_sparsity_in; + capsule->gnsf_phi_fun_jac_y[i].casadi_sparsity_out = &{{ model.name }}_gnsf_phi_fun_jac_y_sparsity_out; + capsule->gnsf_phi_fun_jac_y[i].casadi_n_in = &{{ model.name }}_gnsf_phi_fun_jac_y_n_in; + capsule->gnsf_phi_fun_jac_y[i].casadi_n_out = &{{ model.name }}_gnsf_phi_fun_jac_y_n_out; + external_function_param_casadi_create(&capsule->gnsf_phi_fun_jac_y[i], {{ dims.np }}); + } + + capsule->gnsf_phi_jac_y_uhat = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N; i++) { + capsule->gnsf_phi_jac_y_uhat[i].casadi_fun = &{{ model.name }}_gnsf_phi_jac_y_uhat; + capsule->gnsf_phi_jac_y_uhat[i].casadi_work = &{{ model.name }}_gnsf_phi_jac_y_uhat_work; + capsule->gnsf_phi_jac_y_uhat[i].casadi_sparsity_in = &{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_in; + capsule->gnsf_phi_jac_y_uhat[i].casadi_sparsity_out = &{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_out; + capsule->gnsf_phi_jac_y_uhat[i].casadi_n_in = &{{ model.name }}_gnsf_phi_jac_y_uhat_n_in; + capsule->gnsf_phi_jac_y_uhat[i].casadi_n_out = &{{ model.name }}_gnsf_phi_jac_y_uhat_n_out; + external_function_param_casadi_create(&capsule->gnsf_phi_jac_y_uhat[i], {{ dims.np }}); + } + + capsule->gnsf_f_lo_jac_x1_x1dot_u_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N; i++) { + capsule->gnsf_f_lo_jac_x1_x1dot_u_z[i].casadi_fun = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz; + capsule->gnsf_f_lo_jac_x1_x1dot_u_z[i].casadi_work = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_work; + capsule->gnsf_f_lo_jac_x1_x1dot_u_z[i].casadi_sparsity_in = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_in; + capsule->gnsf_f_lo_jac_x1_x1dot_u_z[i].casadi_sparsity_out = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_out; + capsule->gnsf_f_lo_jac_x1_x1dot_u_z[i].casadi_n_in = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_in; + capsule->gnsf_f_lo_jac_x1_x1dot_u_z[i].casadi_n_out = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_out; + external_function_param_casadi_create(&capsule->gnsf_f_lo_jac_x1_x1dot_u_z[i], {{ dims.np }}); + } + + capsule->gnsf_get_matrices_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N; i++) { + capsule->gnsf_get_matrices_fun[i].casadi_fun = &{{ model.name }}_gnsf_get_matrices_fun; + capsule->gnsf_get_matrices_fun[i].casadi_work = &{{ model.name }}_gnsf_get_matrices_fun_work; + capsule->gnsf_get_matrices_fun[i].casadi_sparsity_in = &{{ model.name }}_gnsf_get_matrices_fun_sparsity_in; + capsule->gnsf_get_matrices_fun[i].casadi_sparsity_out = &{{ model.name }}_gnsf_get_matrices_fun_sparsity_out; + capsule->gnsf_get_matrices_fun[i].casadi_n_in = &{{ model.name }}_gnsf_get_matrices_fun_n_in; + capsule->gnsf_get_matrices_fun[i].casadi_n_out = &{{ model.name }}_gnsf_get_matrices_fun_n_out; + external_function_param_casadi_create(&capsule->gnsf_get_matrices_fun[i], {{ dims.np }}); + } +{% elif solver_options.integrator_type == "DISCRETE" %} + // discrete dynamics + capsule->discr_dyn_phi_fun = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }})*N); + for (int i = 0; i < N; i++) + { + {%- if model.dyn_ext_fun_type == "casadi" %} + capsule->discr_dyn_phi_fun[i].casadi_fun = &{{ model.name }}_dyn_disc_phi_fun; + capsule->discr_dyn_phi_fun[i].casadi_n_in = &{{ model.name }}_dyn_disc_phi_fun_n_in; + capsule->discr_dyn_phi_fun[i].casadi_n_out = &{{ model.name }}_dyn_disc_phi_fun_n_out; + capsule->discr_dyn_phi_fun[i].casadi_sparsity_in = &{{ model.name }}_dyn_disc_phi_fun_sparsity_in; + capsule->discr_dyn_phi_fun[i].casadi_sparsity_out = &{{ model.name }}_dyn_disc_phi_fun_sparsity_out; + capsule->discr_dyn_phi_fun[i].casadi_work = &{{ model.name }}_dyn_disc_phi_fun_work; + {%- else %} + capsule->discr_dyn_phi_fun[i].fun = &{{ model.dyn_disc_fun }}; + {%- endif %} + external_function_param_{{ model.dyn_ext_fun_type }}_create(&capsule->discr_dyn_phi_fun[i], {{ dims.np }}); + } + + capsule->discr_dyn_phi_fun_jac_ut_xt = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }})*N); + for (int i = 0; i < N; i++) + { + {%- if model.dyn_ext_fun_type == "casadi" %} + capsule->discr_dyn_phi_fun_jac_ut_xt[i].casadi_fun = &{{ model.name }}_dyn_disc_phi_fun_jac; + capsule->discr_dyn_phi_fun_jac_ut_xt[i].casadi_n_in = &{{ model.name }}_dyn_disc_phi_fun_jac_n_in; + capsule->discr_dyn_phi_fun_jac_ut_xt[i].casadi_n_out = &{{ model.name }}_dyn_disc_phi_fun_jac_n_out; + capsule->discr_dyn_phi_fun_jac_ut_xt[i].casadi_sparsity_in = &{{ model.name }}_dyn_disc_phi_fun_jac_sparsity_in; + capsule->discr_dyn_phi_fun_jac_ut_xt[i].casadi_sparsity_out = &{{ model.name }}_dyn_disc_phi_fun_jac_sparsity_out; + capsule->discr_dyn_phi_fun_jac_ut_xt[i].casadi_work = &{{ model.name }}_dyn_disc_phi_fun_jac_work; + {%- else %} + capsule->discr_dyn_phi_fun_jac_ut_xt[i].fun = &{{ model.dyn_disc_fun_jac }}; + {%- endif %} + external_function_param_{{ model.dyn_ext_fun_type }}_create(&capsule->discr_dyn_phi_fun_jac_ut_xt[i], {{ dims.np }}); + } + + {%- if solver_options.hessian_approx == "EXACT" %} + capsule->discr_dyn_phi_fun_jac_ut_xt_hess = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }})*N); + for (int i = 0; i < N; i++) + { + {%- if model.dyn_ext_fun_type == "casadi" %} + capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i].casadi_fun = &{{ model.name }}_dyn_disc_phi_fun_jac_hess; + capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i].casadi_n_in = &{{ model.name }}_dyn_disc_phi_fun_jac_hess_n_in; + capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i].casadi_n_out = &{{ model.name }}_dyn_disc_phi_fun_jac_hess_n_out; + capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i].casadi_sparsity_in = &{{ model.name }}_dyn_disc_phi_fun_jac_hess_sparsity_in; + capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i].casadi_sparsity_out = &{{ model.name }}_dyn_disc_phi_fun_jac_hess_sparsity_out; + capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i].casadi_work = &{{ model.name }}_dyn_disc_phi_fun_jac_hess_work; + {%- else %} + capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i].fun = &{{ model.dyn_disc_fun_jac_hess }}; + {%- endif %} + external_function_param_{{ model.dyn_ext_fun_type }}_create(&capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i], {{ dims.np }}); + } + {%- endif %} +{%- endif %} + + +{%- if cost.cost_type_0 == "NONLINEAR_LS" %} + // nonlinear least square function + capsule->cost_y_0_fun.casadi_fun = &{{ model.name }}_cost_y_0_fun; + capsule->cost_y_0_fun.casadi_n_in = &{{ model.name }}_cost_y_0_fun_n_in; + capsule->cost_y_0_fun.casadi_n_out = &{{ model.name }}_cost_y_0_fun_n_out; + capsule->cost_y_0_fun.casadi_sparsity_in = &{{ model.name }}_cost_y_0_fun_sparsity_in; + capsule->cost_y_0_fun.casadi_sparsity_out = &{{ model.name }}_cost_y_0_fun_sparsity_out; + capsule->cost_y_0_fun.casadi_work = &{{ model.name }}_cost_y_0_fun_work; + external_function_param_casadi_create(&capsule->cost_y_0_fun, {{ dims.np }}); + + capsule->cost_y_0_fun_jac_ut_xt.casadi_fun = &{{ model.name }}_cost_y_0_fun_jac_ut_xt; + capsule->cost_y_0_fun_jac_ut_xt.casadi_n_in = &{{ model.name }}_cost_y_0_fun_jac_ut_xt_n_in; + capsule->cost_y_0_fun_jac_ut_xt.casadi_n_out = &{{ model.name }}_cost_y_0_fun_jac_ut_xt_n_out; + capsule->cost_y_0_fun_jac_ut_xt.casadi_sparsity_in = &{{ model.name }}_cost_y_0_fun_jac_ut_xt_sparsity_in; + capsule->cost_y_0_fun_jac_ut_xt.casadi_sparsity_out = &{{ model.name }}_cost_y_0_fun_jac_ut_xt_sparsity_out; + capsule->cost_y_0_fun_jac_ut_xt.casadi_work = &{{ model.name }}_cost_y_0_fun_jac_ut_xt_work; + external_function_param_casadi_create(&capsule->cost_y_0_fun_jac_ut_xt, {{ dims.np }}); + + capsule->cost_y_0_hess.casadi_fun = &{{ model.name }}_cost_y_0_hess; + capsule->cost_y_0_hess.casadi_n_in = &{{ model.name }}_cost_y_0_hess_n_in; + capsule->cost_y_0_hess.casadi_n_out = &{{ model.name }}_cost_y_0_hess_n_out; + capsule->cost_y_0_hess.casadi_sparsity_in = &{{ model.name }}_cost_y_0_hess_sparsity_in; + capsule->cost_y_0_hess.casadi_sparsity_out = &{{ model.name }}_cost_y_0_hess_sparsity_out; + capsule->cost_y_0_hess.casadi_work = &{{ model.name }}_cost_y_0_hess_work; + external_function_param_casadi_create(&capsule->cost_y_0_hess, {{ dims.np }}); + +{%- elif cost.cost_type_0 == "EXTERNAL" %} + // external cost + {% if cost.cost_ext_fun_type_0 == "casadi" %} + capsule->ext_cost_0_fun.casadi_fun = &{{ model.name }}_cost_ext_cost_0_fun; + capsule->ext_cost_0_fun.casadi_n_in = &{{ model.name }}_cost_ext_cost_0_fun_n_in; + capsule->ext_cost_0_fun.casadi_n_out = &{{ model.name }}_cost_ext_cost_0_fun_n_out; + capsule->ext_cost_0_fun.casadi_sparsity_in = &{{ model.name }}_cost_ext_cost_0_fun_sparsity_in; + capsule->ext_cost_0_fun.casadi_sparsity_out = &{{ model.name }}_cost_ext_cost_0_fun_sparsity_out; + capsule->ext_cost_0_fun.casadi_work = &{{ model.name }}_cost_ext_cost_0_fun_work; + {% else %} + capsule->ext_cost_0_fun.fun = &{{ cost.cost_function_ext_cost_0 }}; + {% endif %} + external_function_param_{{ cost.cost_ext_fun_type_0 }}_create(&capsule->ext_cost_0_fun, {{ dims.np }}); + + // external cost + {% if cost.cost_ext_fun_type_0 == "casadi" %} + capsule->ext_cost_0_fun_jac.casadi_fun = &{{ model.name }}_cost_ext_cost_0_fun_jac; + capsule->ext_cost_0_fun_jac.casadi_n_in = &{{ model.name }}_cost_ext_cost_0_fun_jac_n_in; + capsule->ext_cost_0_fun_jac.casadi_n_out = &{{ model.name }}_cost_ext_cost_0_fun_jac_n_out; + capsule->ext_cost_0_fun_jac.casadi_sparsity_in = &{{ model.name }}_cost_ext_cost_0_fun_jac_sparsity_in; + capsule->ext_cost_0_fun_jac.casadi_sparsity_out = &{{ model.name }}_cost_ext_cost_0_fun_jac_sparsity_out; + capsule->ext_cost_0_fun_jac.casadi_work = &{{ model.name }}_cost_ext_cost_0_fun_jac_work; + {% else %} + capsule->ext_cost_0_fun_jac.fun = &{{ cost.cost_function_ext_cost_0 }}; + {% endif %} + external_function_param_{{ cost.cost_ext_fun_type_0 }}_create(&capsule->ext_cost_0_fun_jac, {{ dims.np }}); + + // external cost + {% if cost.cost_ext_fun_type_0 == "casadi" %} + capsule->ext_cost_0_fun_jac_hess.casadi_fun = &{{ model.name }}_cost_ext_cost_0_fun_jac_hess; + capsule->ext_cost_0_fun_jac_hess.casadi_n_in = &{{ model.name }}_cost_ext_cost_0_fun_jac_hess_n_in; + capsule->ext_cost_0_fun_jac_hess.casadi_n_out = &{{ model.name }}_cost_ext_cost_0_fun_jac_hess_n_out; + capsule->ext_cost_0_fun_jac_hess.casadi_sparsity_in = &{{ model.name }}_cost_ext_cost_0_fun_jac_hess_sparsity_in; + capsule->ext_cost_0_fun_jac_hess.casadi_sparsity_out = &{{ model.name }}_cost_ext_cost_0_fun_jac_hess_sparsity_out; + capsule->ext_cost_0_fun_jac_hess.casadi_work = &{{ model.name }}_cost_ext_cost_0_fun_jac_hess_work; + {% else %} + capsule->ext_cost_0_fun_jac_hess.fun = &{{ cost.cost_function_ext_cost_0 }}; + {% endif %} + external_function_param_{{ cost.cost_ext_fun_type_0 }}_create(&capsule->ext_cost_0_fun_jac_hess, {{ dims.np }}); +{%- endif %} + +{%- if cost.cost_type == "NONLINEAR_LS" %} + // nonlinear least squares cost + capsule->cost_y_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N-1; i++) + { + capsule->cost_y_fun[i].casadi_fun = &{{ model.name }}_cost_y_fun; + capsule->cost_y_fun[i].casadi_n_in = &{{ model.name }}_cost_y_fun_n_in; + capsule->cost_y_fun[i].casadi_n_out = &{{ model.name }}_cost_y_fun_n_out; + capsule->cost_y_fun[i].casadi_sparsity_in = &{{ model.name }}_cost_y_fun_sparsity_in; + capsule->cost_y_fun[i].casadi_sparsity_out = &{{ model.name }}_cost_y_fun_sparsity_out; + capsule->cost_y_fun[i].casadi_work = &{{ model.name }}_cost_y_fun_work; + + external_function_param_casadi_create(&capsule->cost_y_fun[i], {{ dims.np }}); + } + + capsule->cost_y_fun_jac_ut_xt = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N-1; i++) + { + capsule->cost_y_fun_jac_ut_xt[i].casadi_fun = &{{ model.name }}_cost_y_fun_jac_ut_xt; + capsule->cost_y_fun_jac_ut_xt[i].casadi_n_in = &{{ model.name }}_cost_y_fun_jac_ut_xt_n_in; + capsule->cost_y_fun_jac_ut_xt[i].casadi_n_out = &{{ model.name }}_cost_y_fun_jac_ut_xt_n_out; + capsule->cost_y_fun_jac_ut_xt[i].casadi_sparsity_in = &{{ model.name }}_cost_y_fun_jac_ut_xt_sparsity_in; + capsule->cost_y_fun_jac_ut_xt[i].casadi_sparsity_out = &{{ model.name }}_cost_y_fun_jac_ut_xt_sparsity_out; + capsule->cost_y_fun_jac_ut_xt[i].casadi_work = &{{ model.name }}_cost_y_fun_jac_ut_xt_work; + + external_function_param_casadi_create(&capsule->cost_y_fun_jac_ut_xt[i], {{ dims.np }}); + } + + capsule->cost_y_hess = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N-1; i++) + { + capsule->cost_y_hess[i].casadi_fun = &{{ model.name }}_cost_y_hess; + capsule->cost_y_hess[i].casadi_n_in = &{{ model.name }}_cost_y_hess_n_in; + capsule->cost_y_hess[i].casadi_n_out = &{{ model.name }}_cost_y_hess_n_out; + capsule->cost_y_hess[i].casadi_sparsity_in = &{{ model.name }}_cost_y_hess_sparsity_in; + capsule->cost_y_hess[i].casadi_sparsity_out = &{{ model.name }}_cost_y_hess_sparsity_out; + capsule->cost_y_hess[i].casadi_work = &{{ model.name }}_cost_y_hess_work; + + external_function_param_casadi_create(&capsule->cost_y_hess[i], {{ dims.np }}); + } +{%- elif cost.cost_type == "EXTERNAL" %} + // external cost + capsule->ext_cost_fun = (external_function_param_{{ cost.cost_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ cost.cost_ext_fun_type }})*N); + for (int i = 0; i < N-1; i++) + { + {% if cost.cost_ext_fun_type == "casadi" %} + capsule->ext_cost_fun[i].casadi_fun = &{{ model.name }}_cost_ext_cost_fun; + capsule->ext_cost_fun[i].casadi_n_in = &{{ model.name }}_cost_ext_cost_fun_n_in; + capsule->ext_cost_fun[i].casadi_n_out = &{{ model.name }}_cost_ext_cost_fun_n_out; + capsule->ext_cost_fun[i].casadi_sparsity_in = &{{ model.name }}_cost_ext_cost_fun_sparsity_in; + capsule->ext_cost_fun[i].casadi_sparsity_out = &{{ model.name }}_cost_ext_cost_fun_sparsity_out; + capsule->ext_cost_fun[i].casadi_work = &{{ model.name }}_cost_ext_cost_fun_work; + {% else %} + capsule->ext_cost_fun[i].fun = &{{ cost.cost_function_ext_cost }}; + {% endif %} + external_function_param_{{ cost.cost_ext_fun_type }}_create(&capsule->ext_cost_fun[i], {{ dims.np }}); + } + + capsule->ext_cost_fun_jac = (external_function_param_{{ cost.cost_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ cost.cost_ext_fun_type }})*N); + for (int i = 0; i < N-1; i++) + { + {% if cost.cost_ext_fun_type == "casadi" %} + capsule->ext_cost_fun_jac[i].casadi_fun = &{{ model.name }}_cost_ext_cost_fun_jac; + capsule->ext_cost_fun_jac[i].casadi_n_in = &{{ model.name }}_cost_ext_cost_fun_jac_n_in; + capsule->ext_cost_fun_jac[i].casadi_n_out = &{{ model.name }}_cost_ext_cost_fun_jac_n_out; + capsule->ext_cost_fun_jac[i].casadi_sparsity_in = &{{ model.name }}_cost_ext_cost_fun_jac_sparsity_in; + capsule->ext_cost_fun_jac[i].casadi_sparsity_out = &{{ model.name }}_cost_ext_cost_fun_jac_sparsity_out; + capsule->ext_cost_fun_jac[i].casadi_work = &{{ model.name }}_cost_ext_cost_fun_jac_work; + {% else %} + capsule->ext_cost_fun_jac[i].fun = &{{ cost.cost_function_ext_cost }}; + {% endif %} + external_function_param_{{ cost.cost_ext_fun_type }}_create(&capsule->ext_cost_fun_jac[i], {{ dims.np }}); + } + + capsule->ext_cost_fun_jac_hess = (external_function_param_{{ cost.cost_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ cost.cost_ext_fun_type }})*N); + for (int i = 0; i < N-1; i++) + { + {% if cost.cost_ext_fun_type == "casadi" %} + capsule->ext_cost_fun_jac_hess[i].casadi_fun = &{{ model.name }}_cost_ext_cost_fun_jac_hess; + capsule->ext_cost_fun_jac_hess[i].casadi_n_in = &{{ model.name }}_cost_ext_cost_fun_jac_hess_n_in; + capsule->ext_cost_fun_jac_hess[i].casadi_n_out = &{{ model.name }}_cost_ext_cost_fun_jac_hess_n_out; + capsule->ext_cost_fun_jac_hess[i].casadi_sparsity_in = &{{ model.name }}_cost_ext_cost_fun_jac_hess_sparsity_in; + capsule->ext_cost_fun_jac_hess[i].casadi_sparsity_out = &{{ model.name }}_cost_ext_cost_fun_jac_hess_sparsity_out; + capsule->ext_cost_fun_jac_hess[i].casadi_work = &{{ model.name }}_cost_ext_cost_fun_jac_hess_work; + {% else %} + capsule->ext_cost_fun_jac_hess[i].fun = &{{ cost.cost_function_ext_cost }}; + {% endif %} + external_function_param_{{ cost.cost_ext_fun_type }}_create(&capsule->ext_cost_fun_jac_hess[i], {{ dims.np }}); + } +{%- endif %} + +{%- if cost.cost_type_e == "NONLINEAR_LS" %} + // nonlinear least square function + capsule->cost_y_e_fun.casadi_fun = &{{ model.name }}_cost_y_e_fun; + capsule->cost_y_e_fun.casadi_n_in = &{{ model.name }}_cost_y_e_fun_n_in; + capsule->cost_y_e_fun.casadi_n_out = &{{ model.name }}_cost_y_e_fun_n_out; + capsule->cost_y_e_fun.casadi_sparsity_in = &{{ model.name }}_cost_y_e_fun_sparsity_in; + capsule->cost_y_e_fun.casadi_sparsity_out = &{{ model.name }}_cost_y_e_fun_sparsity_out; + capsule->cost_y_e_fun.casadi_work = &{{ model.name }}_cost_y_e_fun_work; + external_function_param_casadi_create(&capsule->cost_y_e_fun, {{ dims.np }}); + + capsule->cost_y_e_fun_jac_ut_xt.casadi_fun = &{{ model.name }}_cost_y_e_fun_jac_ut_xt; + capsule->cost_y_e_fun_jac_ut_xt.casadi_n_in = &{{ model.name }}_cost_y_e_fun_jac_ut_xt_n_in; + capsule->cost_y_e_fun_jac_ut_xt.casadi_n_out = &{{ model.name }}_cost_y_e_fun_jac_ut_xt_n_out; + capsule->cost_y_e_fun_jac_ut_xt.casadi_sparsity_in = &{{ model.name }}_cost_y_e_fun_jac_ut_xt_sparsity_in; + capsule->cost_y_e_fun_jac_ut_xt.casadi_sparsity_out = &{{ model.name }}_cost_y_e_fun_jac_ut_xt_sparsity_out; + capsule->cost_y_e_fun_jac_ut_xt.casadi_work = &{{ model.name }}_cost_y_e_fun_jac_ut_xt_work; + external_function_param_casadi_create(&capsule->cost_y_e_fun_jac_ut_xt, {{ dims.np }}); + + capsule->cost_y_e_hess.casadi_fun = &{{ model.name }}_cost_y_e_hess; + capsule->cost_y_e_hess.casadi_n_in = &{{ model.name }}_cost_y_e_hess_n_in; + capsule->cost_y_e_hess.casadi_n_out = &{{ model.name }}_cost_y_e_hess_n_out; + capsule->cost_y_e_hess.casadi_sparsity_in = &{{ model.name }}_cost_y_e_hess_sparsity_in; + capsule->cost_y_e_hess.casadi_sparsity_out = &{{ model.name }}_cost_y_e_hess_sparsity_out; + capsule->cost_y_e_hess.casadi_work = &{{ model.name }}_cost_y_e_hess_work; + external_function_param_casadi_create(&capsule->cost_y_e_hess, {{ dims.np }}); + +{%- elif cost.cost_type_e == "EXTERNAL" %} + // external cost + {% if cost.cost_ext_fun_type_e == "casadi" %} + capsule->ext_cost_e_fun.casadi_fun = &{{ model.name }}_cost_ext_cost_e_fun; + capsule->ext_cost_e_fun.casadi_n_in = &{{ model.name }}_cost_ext_cost_e_fun_n_in; + capsule->ext_cost_e_fun.casadi_n_out = &{{ model.name }}_cost_ext_cost_e_fun_n_out; + capsule->ext_cost_e_fun.casadi_sparsity_in = &{{ model.name }}_cost_ext_cost_e_fun_sparsity_in; + capsule->ext_cost_e_fun.casadi_sparsity_out = &{{ model.name }}_cost_ext_cost_e_fun_sparsity_out; + capsule->ext_cost_e_fun.casadi_work = &{{ model.name }}_cost_ext_cost_e_fun_work; + {% else %} + capsule->ext_cost_e_fun.fun = &{{ cost.cost_function_ext_cost_e }}; + {% endif %} + external_function_param_{{ cost.cost_ext_fun_type_e }}_create(&capsule->ext_cost_e_fun, {{ dims.np }}); + + // external cost + {% if cost.cost_ext_fun_type_e == "casadi" %} + capsule->ext_cost_e_fun_jac.casadi_fun = &{{ model.name }}_cost_ext_cost_e_fun_jac; + capsule->ext_cost_e_fun_jac.casadi_n_in = &{{ model.name }}_cost_ext_cost_e_fun_jac_n_in; + capsule->ext_cost_e_fun_jac.casadi_n_out = &{{ model.name }}_cost_ext_cost_e_fun_jac_n_out; + capsule->ext_cost_e_fun_jac.casadi_sparsity_in = &{{ model.name }}_cost_ext_cost_e_fun_jac_sparsity_in; + capsule->ext_cost_e_fun_jac.casadi_sparsity_out = &{{ model.name }}_cost_ext_cost_e_fun_jac_sparsity_out; + capsule->ext_cost_e_fun_jac.casadi_work = &{{ model.name }}_cost_ext_cost_e_fun_jac_work; + {% else %} + capsule->ext_cost_e_fun_jac.fun = &{{ cost.cost_function_ext_cost_e }}; + {% endif %} + external_function_param_{{ cost.cost_ext_fun_type_e }}_create(&capsule->ext_cost_e_fun_jac, {{ dims.np }}); + + // external cost + {% if cost.cost_ext_fun_type_e == "casadi" %} + capsule->ext_cost_e_fun_jac_hess.casadi_fun = &{{ model.name }}_cost_ext_cost_e_fun_jac_hess; + capsule->ext_cost_e_fun_jac_hess.casadi_n_in = &{{ model.name }}_cost_ext_cost_e_fun_jac_hess_n_in; + capsule->ext_cost_e_fun_jac_hess.casadi_n_out = &{{ model.name }}_cost_ext_cost_e_fun_jac_hess_n_out; + capsule->ext_cost_e_fun_jac_hess.casadi_sparsity_in = &{{ model.name }}_cost_ext_cost_e_fun_jac_hess_sparsity_in; + capsule->ext_cost_e_fun_jac_hess.casadi_sparsity_out = &{{ model.name }}_cost_ext_cost_e_fun_jac_hess_sparsity_out; + capsule->ext_cost_e_fun_jac_hess.casadi_work = &{{ model.name }}_cost_ext_cost_e_fun_jac_hess_work; + {% else %} + capsule->ext_cost_e_fun_jac_hess.fun = &{{ cost.cost_function_ext_cost_e }}; + {% endif %} + external_function_param_{{ cost.cost_ext_fun_type_e }}_create(&capsule->ext_cost_e_fun_jac_hess, {{ dims.np }}); +{%- endif %} + + /************************************************ + * nlp_in + ************************************************/ + ocp_nlp_in * nlp_in = ocp_nlp_in_create(nlp_config, nlp_dims); + capsule->nlp_in = nlp_in; + + double time_steps[N]; + {%- for j in range(end=dims.N) %} + time_steps[{{ j }}] = {{ solver_options.time_steps[j] }}; + {%- endfor %} + + for (int i = 0; i < N; i++) + { + ocp_nlp_in_set(nlp_config, nlp_dims, nlp_in, i, "Ts", &time_steps[i]); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "scaling", &time_steps[i]); + } + + /**** Dynamics ****/ + for (int i = 0; i < N; i++) + { + {%- if solver_options.integrator_type == "ERK" %} + ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "expl_vde_forw", &capsule->forw_vde_casadi[i]); + ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "expl_ode_fun", &capsule->expl_ode_fun[i]); + {%- if solver_options.hessian_approx == "EXACT" %} + ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "expl_ode_hess", &capsule->hess_vde_casadi[i]); + {%- endif %} + {% elif solver_options.integrator_type == "IRK" %} + ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "impl_dae_fun", &capsule->impl_dae_fun[i]); + ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, + "impl_dae_fun_jac_x_xdot_z", &capsule->impl_dae_fun_jac_x_xdot_z[i]); + ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, + "impl_dae_jac_x_xdot_u", &capsule->impl_dae_jac_x_xdot_u_z[i]); + {%- if solver_options.hessian_approx == "EXACT" %} + ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "impl_dae_hess", &capsule->impl_dae_hess[i]); + {%- endif %} + {% elif solver_options.integrator_type == "GNSF" %} + ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "phi_fun", &capsule->gnsf_phi_fun[i]); + ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "phi_fun_jac_y", &capsule->gnsf_phi_fun_jac_y[i]); + ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "phi_jac_y_uhat", &capsule->gnsf_phi_jac_y_uhat[i]); + ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "f_lo_jac_x1_x1dot_u_z", + &capsule->gnsf_f_lo_jac_x1_x1dot_u_z[i]); + ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "gnsf_get_matrices_fun", + &capsule->gnsf_get_matrices_fun[i]); + {% elif solver_options.integrator_type == "DISCRETE" %} + ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "disc_dyn_fun", &capsule->discr_dyn_phi_fun[i]); + ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "disc_dyn_fun_jac", + &capsule->discr_dyn_phi_fun_jac_ut_xt[i]); + {%- if solver_options.hessian_approx == "EXACT" %} + ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "disc_dyn_fun_jac_hess", + &capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i]); + {%- endif %} + {%- endif %} + } + + + /**** Cost ****/ +{%- if cost.cost_type_0 == "NONLINEAR_LS" or cost.cost_type_0 == "LINEAR_LS" %} +{% if dims.ny_0 > 0 %} + double W_0[NY0*NY0]; + {% for j in range(end=dims.ny_0) %} + {%- for k in range(end=dims.ny_0) %} + W_0[{{ j }}+(NY0) * {{ k }}] = {{ cost.W_0[j][k] }}; + {%- endfor %} + {%- endfor %} + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "W", W_0); + + double yref_0[NY0]; + {% for j in range(end=dims.ny_0) %} + yref_0[{{ j }}] = {{ cost.yref_0[j] }}; + {%- endfor %} + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", yref_0); +{% endif %} +{% endif %} + +{%- if cost.cost_type == "NONLINEAR_LS" or cost.cost_type == "LINEAR_LS" %} +{% if dims.ny > 0 %} + double W[NY*NY]; + {% for j in range(end=dims.ny) %} + {%- for k in range(end=dims.ny) %} + W[{{ j }}+(NY) * {{ k }}] = {{ cost.W[j][k] }}; + {%- endfor %} + {%- endfor %} + + double yref[NY]; + {% for j in range(end=dims.ny) %} + yref[{{ j }}] = {{ cost.yref[j] }}; + {%- endfor %} + + for (int i = 1; i < N; i++) + { + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "W", W); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "yref", yref); + } +{% endif %} +{% endif %} + +{%- if cost.cost_type_0 == "LINEAR_LS" %} + double Vx_0[NY0*NX]; + {% for j in range(end=dims.ny_0) %} + {%- for k in range(end=dims.nx) %} + Vx_0[{{ j }}+(NY0) * {{ k }}] = {{ cost.Vx_0[j][k] }}; + {%- endfor %} + {%- endfor %} + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "Vx", Vx_0); + +{% if dims.ny_0 > 0 and dims.nu > 0 %} + double Vu_0[NY0*NU]; + {% for j in range(end=dims.ny_0) %} + {%- for k in range(end=dims.nu) %} + Vu_0[{{ j }}+(NY0) * {{ k }}] = {{ cost.Vu_0[j][k] }}; + {%- endfor %} + {%- endfor %} + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "Vu", Vu_0); +{% endif %} +{% if dims.ny_0 > 0 and dims.nz > 0 %} + double Vz_0[NY0*NZ]; + {% for j in range(end=dims.ny_0) %} + {%- for k in range(end=dims.nz) %} + Vz_0[{{ j }}+(NY0) * {{ k }}] = {{ cost.Vz_0[j][k] }}; + {%- endfor %} + {%- endfor %} + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "Vz", Vz_0); +{%- endif %} +{%- endif %}{# LINEAR LS #} + + +{%- if cost.cost_type == "LINEAR_LS" %} + double Vx[NY*NX]; + {% for j in range(end=dims.ny) %} + {%- for k in range(end=dims.nx) %} + Vx[{{ j }}+(NY) * {{ k }}] = {{ cost.Vx[j][k] }}; + {%- endfor %} + {%- endfor %} + for (int i = 1; i < N; i++) + { + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "Vx", Vx); + } + +{% if dims.ny > 0 and dims.nu > 0 %} + double Vu[NY*NU]; + {% for j in range(end=dims.ny) %} + {%- for k in range(end=dims.nu) %} + Vu[{{ j }}+(NY) * {{ k }}] = {{ cost.Vu[j][k] }}; + {%- endfor %} + {%- endfor %} + + for (int i = 1; i < N; i++) + { + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "Vu", Vu); + } +{% endif %} + +{% if dims.ny > 0 and dims.nz > 0 %} + double Vz[NY*NZ]; + {% for j in range(end=dims.ny) %} + {%- for k in range(end=dims.nz) %} + Vz[{{ j }}+(NY) * {{ k }}] = {{ cost.Vz[j][k] }}; + {%- endfor %} + {%- endfor %} + + for (int i = 1; i < N; i++) + { + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "Vz", Vz); + } +{%- endif %} +{%- endif %}{# LINEAR LS #} + + +{%- if cost.cost_type_0 == "NONLINEAR_LS" %} + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "nls_y_fun", &capsule->cost_y_0_fun); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "nls_y_fun_jac", &capsule->cost_y_0_fun_jac_ut_xt); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "nls_y_hess", &capsule->cost_y_0_hess); +{%- elif cost.cost_type_0 == "EXTERNAL" %} + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "ext_cost_fun", &capsule->ext_cost_0_fun); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "ext_cost_fun_jac", &capsule->ext_cost_0_fun_jac); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "ext_cost_fun_jac_hess", &capsule->ext_cost_0_fun_jac_hess); +{%- endif %} + +{%- if cost.cost_type == "NONLINEAR_LS" %} + for (int i = 1; i < N; i++) + { + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "nls_y_fun", &capsule->cost_y_fun[i-1]); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "nls_y_fun_jac", &capsule->cost_y_fun_jac_ut_xt[i-1]); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "nls_y_hess", &capsule->cost_y_hess[i-1]); + } +{%- elif cost.cost_type == "EXTERNAL" %} + for (int i = 1; i < N; i++) + { + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "ext_cost_fun", &capsule->ext_cost_fun[i-1]); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "ext_cost_fun_jac", &capsule->ext_cost_fun_jac[i-1]); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "ext_cost_fun_jac_hess", &capsule->ext_cost_fun_jac_hess[i-1]); + } +{%- endif %} + + +{% if dims.ns > 0 %} + double Zl[NS]; + double Zu[NS]; + double zl[NS]; + double zu[NS]; + {% for j in range(end=dims.ns) %} + Zl[{{ j }}] = {{ cost.Zl[j] }}; + {%- endfor %} + + {% for j in range(end=dims.ns) %} + Zu[{{ j }}] = {{ cost.Zu[j] }}; + {%- endfor %} + + {% for j in range(end=dims.ns) %} + zl[{{ j }}] = {{ cost.zl[j] }}; + {%- endfor %} + + {% for j in range(end=dims.ns) %} + zu[{{ j }}] = {{ cost.zu[j] }}; + {%- endfor %} + + for (int i = 0; i < N; i++) + { + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "Zl", Zl); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "Zu", Zu); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "zl", zl); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "zu", zu); + } +{% endif %} + + // terminal cost +{% if cost.cost_type_e == "LINEAR_LS" or cost.cost_type_e == "NONLINEAR_LS" %} +{% if dims.ny_e > 0 %} + double yref_e[NYN]; + {% for j in range(end=dims.ny_e) %} + yref_e[{{ j }}] = {{ cost.yref_e[j] }}; + {%- endfor %} + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "yref", yref_e); + + double W_e[NYN*NYN]; + {% for j in range(end=dims.ny_e) %} + {%- for k in range(end=dims.ny_e) %} + W_e[{{ j }}+(NYN) * {{ k }}] = {{ cost.W_e[j][k] }}; + {%- endfor %} + {%- endfor %} + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "W", W_e); + + {%- if cost.cost_type_e == "LINEAR_LS" %} + double Vx_e[NYN*NX]; + {% for j in range(end=dims.ny_e) %} + {%- for k in range(end=dims.nx) %} + Vx_e[{{ j }}+(NYN) * {{ k }}] = {{ cost.Vx_e[j][k] }}; + {%- endfor %} + {%- endfor %} + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "Vx", Vx_e); + {%- endif %} + + {%- if cost.cost_type_e == "NONLINEAR_LS" %} + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_fun", &capsule->cost_y_e_fun); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_fun_jac", &capsule->cost_y_e_fun_jac_ut_xt); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_hess", &capsule->cost_y_e_hess); + {%- endif %} +{%- endif %}{# ny_e > 0 #} + +{%- elif cost.cost_type_e == "EXTERNAL" %} + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "ext_cost_fun", &capsule->ext_cost_e_fun); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "ext_cost_fun_jac", &capsule->ext_cost_e_fun_jac); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "ext_cost_fun_jac_hess", &capsule->ext_cost_e_fun_jac_hess); +{%- endif %} + +{% if dims.ns_e > 0 %} + double Zl_e[NSN]; + double Zu_e[NSN]; + double zl_e[NSN]; + double zu_e[NSN]; + + {% for j in range(end=dims.ns_e) %} + Zl_e[{{ j }}] = {{ cost.Zl_e[j] }}; + {%- endfor %} + + {% for j in range(end=dims.ns_e) %} + Zu_e[{{ j }}] = {{ cost.Zu_e[j] }}; + {%- endfor %} + + {% for j in range(end=dims.ns_e) %} + zl_e[{{ j }}] = {{ cost.zl_e[j] }}; + {%- endfor %} + + {% for j in range(end=dims.ns_e) %} + zu_e[{{ j }}] = {{ cost.zu_e[j] }}; + {%- endfor %} + + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "Zl", Zl_e); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "Zu", Zu_e); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "zl", zl_e); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "zu", zu_e); +{%- endif %} + + /**** Constraints ****/ + + // bounds for initial stage +{% if dims.nbx_0 > 0 %} + // x0 + int idxbx0[{{ dims.nbx_0 }}]; + {% for i in range(end=dims.nbx_0) %} + idxbx0[{{ i }}] = {{ constraints.idxbx_0[i] }}; + {%- endfor %} + + double lbx0[{{ dims.nbx_0 }}]; + double ubx0[{{ dims.nbx_0 }}]; + {% for i in range(end=dims.nbx_0) %} + lbx0[{{ i }}] = {{ constraints.lbx_0[i] }}; + ubx0[{{ i }}] = {{ constraints.ubx_0[i] }}; + {%- endfor %} + + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxbx", idxbx0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", lbx0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", ubx0); +{% endif %} +{% if dims.nbxe_0 > 0 %} + // idxbxe_0 + int idxbxe_0[{{ dims.nbxe_0 }}]; + {% for i in range(end=dims.nbxe_0) %} + idxbxe_0[{{ i }}] = {{ constraints.idxbxe_0[i] }}; + {%- endfor %} + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxbxe", idxbxe_0); +{% endif %} + + /* constraints that are the same for initial and intermediate */ +{%- if dims.nsbx > 0 %} +{# TODO: introduce nsbx0 & move this block down!! #} + // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxsbx", idxsbx); + // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lsbx", lsbx); + // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "usbx", usbx); + + // soft bounds on x + int idxsbx[NSBX]; + {% for i in range(end=dims.nsbx) %} + idxsbx[{{ i }}] = {{ constraints.idxsbx[i] }}; + {%- endfor %} + double lsbx[NSBX]; + double usbx[NSBX]; + {% for i in range(end=dims.nsbx) %} + lsbx[{{ i }}] = {{ constraints.lsbx[i] }}; + usbx[{{ i }}] = {{ constraints.usbx[i] }}; + {%- endfor %} + + for (int i = 1; i < N; i++) + { + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxsbx", idxsbx); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsbx", lsbx); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "usbx", usbx); + } +{%- endif %} + + +{% if dims.nbu > 0 %} + // u + int idxbu[NBU]; + {% for i in range(end=dims.nbu) %} + idxbu[{{ i }}] = {{ constraints.idxbu[i] }}; + {%- endfor %} + double lbu[NBU]; + double ubu[NBU]; + {% for i in range(end=dims.nbu) %} + lbu[{{ i }}] = {{ constraints.lbu[i] }}; + ubu[{{ i }}] = {{ constraints.ubu[i] }}; + {%- endfor %} + + for (int i = 0; i < N; i++) + { + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxbu", idxbu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lbu", lbu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "ubu", ubu); + } +{% endif %} + +{% if dims.nsbu > 0 %} + // set up soft bounds for u + int idxsbu[NSBU]; + {% for i in range(end=dims.nsbu) %} + idxsbu[{{ i }}] = {{ constraints.idxsbu[i] }}; + {%- endfor %} + double lsbu[NSBU]; + double usbu[NSBU]; + {% for i in range(end=dims.nsbu) %} + lsbu[{{ i }}] = {{ constraints.lsbu[i] }}; + usbu[{{ i }}] = {{ constraints.usbu[i] }}; + {%- endfor %} + for (int i = 0; i < N; i++) + { + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxsbu", idxsbu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsbu", lsbu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "usbu", usbu); + } +{% endif %} + +{% if dims.nsg > 0 %} + // set up soft bounds for general linear constraints + int idxsg[NSG]; + {% for i in range(end=dims.nsg) %} + idxsg[{{ i }}] = {{ constraints.idxsg[i] }}; + {%- endfor %} + double lsg[NSG]; + double usg[NSG]; + {% for i in range(end=dims.nsg) %} + lsg[{{ i }}] = {{ constraints.lsg[i] }}; + usg[{{ i }}] = {{ constraints.usg[i] }}; + {%- endfor %} + + for (int i = 0; i < N; i++) + { + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxsg", idxsg); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsg", lsg); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "usg", usg); + } +{% endif %} + +{% if dims.nsh > 0 %} + // set up soft bounds for nonlinear constraints + int idxsh[NSH]; + {% for i in range(end=dims.nsh) %} + idxsh[{{ i }}] = {{ constraints.idxsh[i] }}; + {%- endfor %} + double lsh[NSH]; + double ush[NSH]; + {% for i in range(end=dims.nsh) %} + lsh[{{ i }}] = {{ constraints.lsh[i] }}; + ush[{{ i }}] = {{ constraints.ush[i] }}; + {%- endfor %} + + for (int i = 0; i < N; i++) + { + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxsh", idxsh); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsh", lsh); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "ush", ush); + } +{% endif %} + +{% if dims.nsphi > 0 %} + // set up soft bounds for convex-over-nonlinear constraints + int idxsphi[NSPHI]; + {% for i in range(end=dims.nsphi) %} + idxsphi[{{ i }}] = {{ constraints.idxsphi[i] }}; + {%- endfor %} + double lsphi[NSPHI]; + double usphi[NSPHI]; + {% for i in range(end=dims.nsphi) %} + lsphi[{{ i }}] = {{ constraints.lsphi[i] }}; + usphi[{{ i }}] = {{ constraints.usphi[i] }}; + {%- endfor %} + + for (int i = 0; i < N; i++) + { + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxsphi", idxsphi); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsphi", lsphi); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "usphi", usphi); + } +{% endif %} + +{% if dims.nbx > 0 %} + // x + int idxbx[NBX]; + {% for i in range(end=dims.nbx) %} + idxbx[{{ i }}] = {{ constraints.idxbx[i] }}; + {%- endfor %} + double lbx[NBX]; + double ubx[NBX]; + {% for i in range(end=dims.nbx) %} + lbx[{{ i }}] = {{ constraints.lbx[i] }}; + ubx[{{ i }}] = {{ constraints.ubx[i] }}; + {%- endfor %} + + for (int i = 1; i < N; i++) + { + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxbx", idxbx); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lbx", lbx); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "ubx", ubx); + } +{% endif %} + +{% if dims.ng > 0 %} + // set up general constraints for stage 0 to N-1 + double D[NG*NU]; + double C[NG*NX]; + double lg[NG]; + double ug[NG]; + + {% for j in range(end=dims.ng) %} + {%- for k in range(end=dims.nu) %} + D[{{ j }}+NG * {{ k }}] = {{ constraints.D[j][k] }}; + {%- endfor %} + {%- endfor %} + + {% for j in range(end=dims.ng) %} + {%- for k in range(end=dims.nx) %} + C[{{ j }}+NG * {{ k }}] = {{ constraints.C[j][k] }}; + {%- endfor %} + {%- endfor %} + + {% for i in range(end=dims.ng) %} + lg[{{ i }}] = {{ constraints.lg[i] }}; + {%- endfor %} + + {% for i in range(end=dims.ng) %} + ug[{{ i }}] = {{ constraints.ug[i] }}; + {%- endfor %} + + for (int i = 0; i < N; i++) + { + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "D", D); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "C", C); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lg", lg); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "ug", ug); + } +{% endif %} + +{% if dims.nh > 0 %} + // set up nonlinear constraints for stage 0 to N-1 + double lh[NH]; + double uh[NH]; + + {% for i in range(end=dims.nh) %} + lh[{{ i }}] = {{ constraints.lh[i] }}; + {%- endfor %} + + {% for i in range(end=dims.nh) %} + uh[{{ i }}] = {{ constraints.uh[i] }}; + {%- endfor %} + + for (int i = 0; i < N; i++) + { + // nonlinear constraints for stages 0 to N-1 + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "nl_constr_h_fun_jac", + &capsule->nl_constr_h_fun_jac[i]); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "nl_constr_h_fun", + &capsule->nl_constr_h_fun[i]); + {% if solver_options.hessian_approx == "EXACT" %} + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, + "nl_constr_h_fun_jac_hess", &capsule->nl_constr_h_fun_jac_hess[i]); + {% endif %} + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lh", lh); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "uh", uh); + } +{% endif %} + +{% if dims.nphi > 0 and constraints.constr_type == "BGP" %} + // set up convex-over-nonlinear constraints for stage 0 to N-1 + double lphi[NPHI]; + double uphi[NPHI]; + + {% for i in range(end=dims.nphi) %} + lphi[{{ i }}] = {{ constraints.lphi[i] }}; + {%- endfor %} + + {% for i in range(end=dims.nphi) %} + uphi[{{ i }}] = {{ constraints.uphi[i] }}; + {%- endfor %} + + for (int i = 0; i < N; i++) + { + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, + "nl_constr_phi_o_r_fun_phi_jac_ux_z_phi_hess_r_jac_ux", &capsule->phi_constraint[i]); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lphi", lphi); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "uphi", uphi); + } +{% endif %} + + /* terminal constraints */ +{% if dims.nbx_e > 0 %} + // set up bounds for last stage + // x + int idxbx_e[NBXN]; + {% for i in range(end=dims.nbx_e) %} + idxbx_e[{{ i }}] = {{ constraints.idxbx_e[i] }}; + {%- endfor %} + double lbx_e[NBXN]; + double ubx_e[NBXN]; + {% for i in range(end=dims.nbx_e) %} + lbx_e[{{ i }}] = {{ constraints.lbx_e[i] }}; + ubx_e[{{ i }}] = {{ constraints.ubx_e[i] }}; + {%- endfor %} + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "idxbx", idxbx_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lbx", lbx_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ubx", ubx_e); +{%- endif %} + +{% if dims.nsg_e > 0 %} + // set up soft bounds for general linear constraints + int idxsg_e[NSGN]; + {% for i in range(end=dims.nsg_e) %} + idxsg_e[{{ i }}] = {{ constraints.idxsg_e[i] }}; + {%- endfor %} + double lsg_e[NSGN]; + double usg_e[NSGN]; + {% for i in range(end=dims.nsg_e) %} + lsg_e[{{ i }}] = {{ constraints.lsg_e[i] }}; + usg_e[{{ i }}] = {{ constraints.usg_e[i] }}; + {%- endfor %} + + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "idxsg", idxsg_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lsg", lsg_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "usg", usg_e); +{%- endif %} + +{% if dims.nsh_e > 0 %} + // set up soft bounds for nonlinear constraints + int idxsh_e[NSHN]; + {% for i in range(end=dims.nsh_e) %} + idxsh_e[{{ i }}] = {{ constraints.idxsh_e[i] }}; + {%- endfor %} + double lsh_e[NSHN]; + double ush_e[NSHN]; + {% for i in range(end=dims.nsh_e) %} + lsh_e[{{ i }}] = {{ constraints.lsh_e[i] }}; + ush_e[{{ i }}] = {{ constraints.ush_e[i] }}; + {%- endfor %} + + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "idxsh", idxsh_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lsh", lsh_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ush", ush_e); +{%- endif %} + +{% if dims.nsphi_e > 0 %} + // set up soft bounds for convex-over-nonlinear constraints + int idxsphi_e[NSPHIN]; + {% for i in range(end=dims.nsphi_e) %} + idxsphi_e[{{ i }}] = {{ constraints.idxsphi_e[i] }}; + {%- endfor %} + double lsphi_e[NSPHIN]; + double usphi_e[NSPHIN]; + {% for i in range(end=dims.nsphi_e) %} + lsphi_e[{{ i }}] = {{ constraints.lsphi_e[i] }}; + usphi_e[{{ i }}] = {{ constraints.usphi_e[i] }}; + {%- endfor %} + + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "idxsphi", idxsphi_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lsphi", lsphi_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "usphi", usphi_e); +{%- endif %} + +{% if dims.nsbx_e > 0 %} + // soft bounds on x + int idxsbx_e[NSBXN]; + {% for i in range(end=dims.nsbx_e) %} + idxsbx_e[{{ i }}] = {{ constraints.idxsbx_e[i] }}; + {%- endfor %} + double lsbx_e[NSBXN]; + double usbx_e[NSBXN]; + {% for i in range(end=dims.nsbx_e) %} + lsbx_e[{{ i }}] = {{ constraints.lsbx_e[i] }}; + usbx_e[{{ i }}] = {{ constraints.usbx_e[i] }}; + {%- endfor %} + + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "idxsbx", idxsbx_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lsbx", lsbx_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "usbx", usbx_e); +{% endif %} + +{% if dims.ng_e > 0 %} + // set up general constraints for last stage + double C_e[NGN*NX]; + double lg_e[NGN]; + double ug_e[NGN]; + + {% for j in range(end=dims.ng) %} + {%- for k in range(end=dims.nx) %} + C_e[{{ j }}+NG * {{ k }}] = {{ constraints.C_e[j][k] }}; + {%- endfor %} + {%- endfor %} + + {% for i in range(end=dims.ng_e) %} + lg_e[{{ i }}] = {{ constraints.lg_e[i] }}; + ug_e[{{ i }}] = {{ constraints.ug_e[i] }}; + {%- endfor %} + + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "C", C_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lg", lg_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ug", ug_e); +{%- endif %} + +{% if dims.nh_e > 0 %} + // set up nonlinear constraints for last stage + double lh_e[NHN]; + double uh_e[NHN]; + + {% for i in range(end=dims.nh_e) %} + lh_e[{{ i }}] = {{ constraints.lh_e[i] }}; + {%- endfor %} + + {% for i in range(end=dims.nh_e) %} + uh_e[{{ i }}] = {{ constraints.uh_e[i] }}; + {%- endfor %} + + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "nl_constr_h_fun_jac", &capsule->nl_constr_h_e_fun_jac); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "nl_constr_h_fun", &capsule->nl_constr_h_e_fun); + {% if solver_options.hessian_approx == "EXACT" %} + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "nl_constr_h_fun_jac_hess", + &capsule->nl_constr_h_e_fun_jac_hess); + {% endif %} + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lh", lh_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "uh", uh_e); +{%- endif %} + +{% if dims.nphi_e > 0 and constraints.constr_type_e == "BGP" %} + // set up convex-over-nonlinear constraints for last stage + double lphi_e[NPHIN]; + double uphi_e[NPHIN]; + + {% for i in range(end=dims.nphi_e) %} + lphi_e[{{ i }}] = {{ constraints.lphi_e[i] }}; + {%- endfor %} + + {% for i in range(end=dims.nphi_e) %} + uphi_e[{{ i }}] = {{ constraints.uphi_e[i] }}; + {%- endfor %} + + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lphi", lphi_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "uphi", uphi_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, + "nl_constr_phi_o_r_fun_phi_jac_ux_z_phi_hess_r_jac_ux", &capsule->phi_e_constraint); +{% endif %} + + + /************************************************ + * opts + ************************************************/ + + capsule->nlp_opts = ocp_nlp_solver_opts_create(nlp_config, nlp_dims); + +{% if solver_options.hessian_approx == "EXACT" %} + bool nlp_solver_exact_hessian = true; + // TODO: this if should not be needed! however, calling the setter with false leads to weird behavior. Investigate! + if (nlp_solver_exact_hessian) + { + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "exact_hess", &nlp_solver_exact_hessian); + } + int exact_hess_dyn = {{ solver_options.exact_hess_dyn }}; + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "exact_hess_dyn", &exact_hess_dyn); + + int exact_hess_cost = {{ solver_options.exact_hess_cost }}; + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "exact_hess_cost", &exact_hess_cost); + + int exact_hess_constr = {{ solver_options.exact_hess_constr }}; + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "exact_hess_constr", &exact_hess_constr); +{%- endif -%} + +{%- if solver_options.globalization == "FIXED_STEP" %} + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "globalization", "fixed_step"); +{%- elif solver_options.globalization == "MERIT_BACKTRACKING" %} + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "globalization", "merit_backtracking"); + + double alpha_min = {{ solver_options.alpha_min }}; + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "alpha_min", &alpha_min); + + double alpha_reduction = {{ solver_options.alpha_reduction }}; + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "alpha_reduction", &alpha_reduction); +{%- endif -%} + +{%- if dims.nz > 0 %} + // TODO: these options are lower level -> should be encapsulated! maybe through hessian approx option. + bool output_z_val = true; + bool sens_algebraic_val = true; + + for (int i = 0; i < N; i++) + ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_output_z", &output_z_val); + for (int i = 0; i < N; i++) + ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_sens_algebraic", &sens_algebraic_val); +{%- endif %} + +{%- if solver_options.integrator_type != "DISCRETE" %} + + int sim_method_num_steps[N]; + {%- for j in range(end=dims.N) %} + sim_method_num_steps[{{ j }}] = {{ solver_options.sim_method_num_steps[j] }}; + {%- endfor %} + + for (int i = 0; i < N; i++) + ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_num_steps", &sim_method_num_steps[i]); + + int sim_method_num_stages[N]; + {%- for j in range(end=dims.N) %} + sim_method_num_stages[{{ j }}] = {{ solver_options.sim_method_num_stages[j] }}; + {%- endfor %} + + for (int i = 0; i < N; i++) + ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_num_stages", &sim_method_num_stages[i]); + + int newton_iter_val = {{ solver_options.sim_method_newton_iter }}; + for (int i = 0; i < N; i++) + ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_newton_iter", &newton_iter_val); + + bool tmp_bool = {{ solver_options.sim_method_jac_reuse }}; + for (int i = 0; i < N; i++) + ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_jac_reuse", &tmp_bool); + +{%- endif %} + + double nlp_solver_step_length = {{ solver_options.nlp_solver_step_length }}; + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "step_length", &nlp_solver_step_length); + + {%- if solver_options.nlp_solver_warm_start_first_qp %} + int nlp_solver_warm_start_first_qp = {{ solver_options.nlp_solver_warm_start_first_qp }}; + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "warm_start_first_qp", &nlp_solver_warm_start_first_qp); + {%- endif %} + + double levenberg_marquardt = {{ solver_options.levenberg_marquardt }}; + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "levenberg_marquardt", &levenberg_marquardt); + + /* options QP solver */ +{%- if solver_options.qp_solver is starting_with("PARTIAL_CONDENSING") %} + int qp_solver_cond_N; + + {%- if solver_options.qp_solver_cond_N %} + qp_solver_cond_N = {{ solver_options.qp_solver_cond_N }}; + {% else %} + // NOTE: there is no condensing happening here! + qp_solver_cond_N = N; + {%- endif %} + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "qp_cond_N", &qp_solver_cond_N); +{% endif %} + + int qp_solver_iter_max = {{ solver_options.qp_solver_iter_max }}; + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "qp_iter_max", &qp_solver_iter_max); + + {%- if solver_options.qp_solver_tol_stat %} + double qp_solver_tol_stat = {{ solver_options.qp_solver_tol_stat }}; + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "qp_tol_stat", &qp_solver_tol_stat); + {%- endif -%} + + {%- if solver_options.qp_solver_tol_eq %} + double qp_solver_tol_eq = {{ solver_options.qp_solver_tol_eq }}; + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "qp_tol_eq", &qp_solver_tol_eq); + {%- endif -%} + + {%- if solver_options.qp_solver_tol_ineq %} + double qp_solver_tol_ineq = {{ solver_options.qp_solver_tol_ineq }}; + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "qp_tol_ineq", &qp_solver_tol_ineq); + {%- endif -%} + + {%- if solver_options.qp_solver_tol_comp %} + double qp_solver_tol_comp = {{ solver_options.qp_solver_tol_comp }}; + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "qp_tol_comp", &qp_solver_tol_comp); + {%- endif -%} + + {%- if solver_options.qp_solver_warm_start %} + int qp_solver_warm_start = {{ solver_options.qp_solver_warm_start }}; + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "qp_warm_start", &qp_solver_warm_start); + {%- endif -%} + +{% if solver_options.nlp_solver_type == "SQP" %} + // set SQP specific options + double nlp_solver_tol_stat = {{ solver_options.nlp_solver_tol_stat }}; + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "tol_stat", &nlp_solver_tol_stat); + + double nlp_solver_tol_eq = {{ solver_options.nlp_solver_tol_eq }}; + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "tol_eq", &nlp_solver_tol_eq); + + double nlp_solver_tol_ineq = {{ solver_options.nlp_solver_tol_ineq }}; + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "tol_ineq", &nlp_solver_tol_ineq); + + double nlp_solver_tol_comp = {{ solver_options.nlp_solver_tol_comp }}; + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "tol_comp", &nlp_solver_tol_comp); + + int nlp_solver_max_iter = {{ solver_options.nlp_solver_max_iter }}; + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "max_iter", &nlp_solver_max_iter); + + int initialize_t_slacks = {{ solver_options.initialize_t_slacks }}; + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "initialize_t_slacks", &initialize_t_slacks); +{%- endif %} + + int print_level = {{ solver_options.print_level }}; + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "print_level", &print_level); + + + int ext_cost_num_hess = {{ solver_options.ext_cost_num_hess }}; +{%- if cost.cost_type == "EXTERNAL" %} + for (int i = 0; i < N; i++) + { + ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "cost_numerical_hessian", &ext_cost_num_hess); + } +{%- endif %} +{%- if cost.cost_type_e == "EXTERNAL" %} + ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, N, "cost_numerical_hessian", &ext_cost_num_hess); +{%- endif %} + + + /* out */ + ocp_nlp_out * nlp_out = ocp_nlp_out_create(nlp_config, nlp_dims); + capsule->nlp_out = nlp_out; + + // initialize primal solution + double x0[{{ dims.nx }}]; +{% if dims.nbx_0 == dims.nx %} + // initialize with x0 + {% for item in constraints.lbx_0 %} + x0[{{ loop.index0 }}] = {{ item }}; + {%- endfor %} +{% else %} + // initialize with zeros + {% for i in range(end=dims.nx) %} + x0[{{ i }}] = 0.0; + {%- endfor %} +{%- endif %} + + double u0[NU]; + {% for i in range(end=dims.nu) %} + u0[{{ i }}] = 0.0; + {%- endfor %} + + for (int i = 0; i < N; i++) + { + // x0 + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "x", x0); + // u0 + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "u", u0); + } + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, N, "x", x0); + + capsule->nlp_solver = ocp_nlp_solver_create(nlp_config, nlp_dims, capsule->nlp_opts); + + +{% if dims.np > 0 %} + // initialize parameters to nominal value + double p[{{ dims.np }}]; + {% for i in range(end=dims.np) %} + p[{{ i }}] = {{ parameter_values[i] }}; + {%- endfor %} + + for (int i = 0; i <= N; i++) + { + {{ model.name }}_acados_update_params(capsule, i, p, NP); + } +{%- endif %}{# if dims.np #} + + status = ocp_nlp_precompute(capsule->nlp_solver, nlp_in, nlp_out); + + if (status != ACADOS_SUCCESS) + { + printf("\nocp_precompute failed!\n\n"); + exit(1); + } + + return status; +} + + +int {{ model.name }}_acados_update_params(nlp_solver_capsule * capsule, int stage, double *p, int np) +{ + int solver_status = 0; + + int casadi_np = {{ dims.np }}; + if (casadi_np != np) { + printf("acados_update_params: trying to set %i parameters for external functions." + " External function has %i parameters. Exiting.\n", np, casadi_np); + exit(1); + } + +{%- if dims.np > 0 %} + if (stage < {{ dims.N }} && stage >= 0) + { + {%- if solver_options.integrator_type == "IRK" %} + capsule->impl_dae_fun[stage].set_param(capsule->impl_dae_fun+stage, p); + capsule->impl_dae_fun_jac_x_xdot_z[stage].set_param(capsule->impl_dae_fun_jac_x_xdot_z+stage, p); + capsule->impl_dae_jac_x_xdot_u_z[stage].set_param(capsule->impl_dae_jac_x_xdot_u_z+stage, p); + + {%- if solver_options.hessian_approx == "EXACT" %} + capsule->impl_dae_hess[stage].set_param(capsule->impl_dae_hess+stage, p); + {%- endif %} + {% elif solver_options.integrator_type == "ERK" %} + capsule->forw_vde_casadi[stage].set_param(capsule->forw_vde_casadi+stage, p); + capsule->expl_ode_fun[stage].set_param(capsule->expl_ode_fun+stage, p); + + {%- if solver_options.hessian_approx == "EXACT" %} + capsule->hess_vde_casadi[stage].set_param(capsule->hess_vde_casadi+stage, p); + {%- endif %} + {% elif solver_options.integrator_type == "GNSF" %} + capsule->gnsf_phi_fun[stage].set_param(capsule->gnsf_phi_fun+stage, p); + capsule->gnsf_phi_fun_jac_y[stage].set_param(capsule->gnsf_phi_fun_jac_y+stage, p); + capsule->gnsf_phi_jac_y_uhat[stage].set_param(capsule->gnsf_phi_jac_y_uhat+stage, p); + + capsule->gnsf_f_lo_jac_x1_x1dot_u_z[stage].set_param(capsule->gnsf_f_lo_jac_x1_x1dot_u_z+stage, p); + {% elif solver_options.integrator_type == "DISCRETE" %} + capsule->discr_dyn_phi_fun[stage].set_param(capsule->discr_dyn_phi_fun+stage, p); + capsule->discr_dyn_phi_fun_jac_ut_xt[stage].set_param(capsule->discr_dyn_phi_fun_jac_ut_xt+stage, p); + {%- if solver_options.hessian_approx == "EXACT" %} + capsule->discr_dyn_phi_fun_jac_ut_xt_hess[stage].set_param(capsule->discr_dyn_phi_fun_jac_ut_xt_hess+stage, p); + {% endif %} + {%- endif %}{# integrator_type #} + + // constraints + {% if constraints.constr_type == "BGP" %} + capsule->phi_constraint[stage].set_param(capsule->phi_constraint+stage, p); + {% elif constraints.constr_type == "BGH" and dims.nh > 0 %} + capsule->nl_constr_h_fun_jac[stage].set_param(capsule->nl_constr_h_fun_jac+stage, p); + capsule->nl_constr_h_fun[stage].set_param(capsule->nl_constr_h_fun+stage, p); + {%- if solver_options.hessian_approx == "EXACT" %} + capsule->nl_constr_h_fun_jac_hess[stage].set_param(capsule->nl_constr_h_fun_jac_hess+stage, p); + {%- endif %} + {%- endif %} + + // cost + if (stage == 0) + { + {%- if cost.cost_type_0 == "NONLINEAR_LS" %} + capsule->cost_y_0_fun.set_param(&capsule->cost_y_0_fun, p); + capsule->cost_y_0_fun_jac_ut_xt.set_param(&capsule->cost_y_0_fun_jac_ut_xt, p); + capsule->cost_y_0_hess.set_param(&capsule->cost_y_0_hess, p); + {%- elif cost.cost_type_0 == "EXTERNAL" %} + capsule->ext_cost_0_fun.set_param(&capsule->ext_cost_0_fun, p); + capsule->ext_cost_0_fun_jac.set_param(&capsule->ext_cost_0_fun_jac, p); + capsule->ext_cost_0_fun_jac_hess.set_param(&capsule->ext_cost_0_fun_jac_hess, p); + {% endif %} + } + else // 0 < stage < N + { + {%- if cost.cost_type == "NONLINEAR_LS" %} + capsule->cost_y_fun[stage-1].set_param(capsule->cost_y_fun+stage-1, p); + capsule->cost_y_fun_jac_ut_xt[stage-1].set_param(capsule->cost_y_fun_jac_ut_xt+stage-1, p); + capsule->cost_y_hess[stage-1].set_param(capsule->cost_y_hess+stage-1, p); + {%- elif cost.cost_type == "EXTERNAL" %} + capsule->ext_cost_fun[stage-1].set_param(capsule->ext_cost_fun+stage-1, p); + capsule->ext_cost_fun_jac[stage-1].set_param(capsule->ext_cost_fun_jac+stage-1, p); + capsule->ext_cost_fun_jac_hess[stage-1].set_param(capsule->ext_cost_fun_jac_hess+stage-1, p); + {%- endif %} + } + } + + else // stage == N + { + // terminal shooting node has no dynamics + // cost + {%- if cost.cost_type_e == "NONLINEAR_LS" %} + capsule->cost_y_e_fun.set_param(&capsule->cost_y_e_fun, p); + capsule->cost_y_e_fun_jac_ut_xt.set_param(&capsule->cost_y_e_fun_jac_ut_xt, p); + capsule->cost_y_e_hess.set_param(&capsule->cost_y_e_hess, p); + {%- elif cost.cost_type_e == "EXTERNAL" %} + capsule->ext_cost_e_fun.set_param(&capsule->ext_cost_e_fun, p); + capsule->ext_cost_e_fun_jac.set_param(&capsule->ext_cost_e_fun_jac, p); + capsule->ext_cost_e_fun_jac_hess.set_param(&capsule->ext_cost_e_fun_jac_hess, p); + {% endif %} + // constraints + {% if constraints.constr_type_e == "BGP" %} + capsule->phi_e_constraint.set_param(&capsule->phi_e_constraint, p); + {% elif constraints.constr_type_e == "BGH" and dims.nh_e > 0 %} + capsule->nl_constr_h_e_fun_jac.set_param(&capsule->nl_constr_h_e_fun_jac, p); + capsule->nl_constr_h_e_fun.set_param(&capsule->nl_constr_h_e_fun, p); + {%- if solver_options.hessian_approx == "EXACT" %} + capsule->nl_constr_h_e_fun_jac_hess.set_param(&capsule->nl_constr_h_e_fun_jac_hess, p); + {%- endif %} + {% endif %} + } +{% endif %}{# if dims.np #} + + return solver_status; +} + + + +int {{ model.name }}_acados_solve(nlp_solver_capsule * capsule) +{ + // solve NLP + int solver_status = ocp_nlp_solve(capsule->nlp_solver, capsule->nlp_in, capsule->nlp_out); + + return solver_status; +} + + +int {{ model.name }}_acados_free(nlp_solver_capsule * capsule) +{ + // free memory + ocp_nlp_solver_opts_destroy(capsule->nlp_opts); + ocp_nlp_in_destroy(capsule->nlp_in); + ocp_nlp_out_destroy(capsule->nlp_out); + ocp_nlp_solver_destroy(capsule->nlp_solver); + ocp_nlp_dims_destroy(capsule->nlp_dims); + ocp_nlp_config_destroy(capsule->nlp_config); + ocp_nlp_plan_destroy(capsule->nlp_solver_plan); + + /* free external function */ + // dynamics +{%- if solver_options.integrator_type == "IRK" %} + for (int i = 0; i < {{ dims.N }}; i++) + { + external_function_param_casadi_free(&capsule->impl_dae_fun[i]); + external_function_param_casadi_free(&capsule->impl_dae_fun_jac_x_xdot_z[i]); + external_function_param_casadi_free(&capsule->impl_dae_jac_x_xdot_u_z[i]); + {%- if solver_options.hessian_approx == "EXACT" %} + external_function_param_casadi_free(&capsule->impl_dae_hess[i]); + {%- endif %} + } + free(capsule->impl_dae_fun); + free(capsule->impl_dae_fun_jac_x_xdot_z); + free(capsule->impl_dae_jac_x_xdot_u_z); + {%- if solver_options.hessian_approx == "EXACT" %} + free(capsule->impl_dae_hess); + {%- endif %} + +{%- elif solver_options.integrator_type == "ERK" %} + for (int i = 0; i < {{ dims.N }}; i++) + { + external_function_param_casadi_free(&capsule->forw_vde_casadi[i]); + external_function_param_casadi_free(&capsule->expl_ode_fun[i]); + {%- if solver_options.hessian_approx == "EXACT" %} + external_function_param_casadi_free(&capsule->hess_vde_casadi[i]); + {%- endif %} + } + free(capsule->forw_vde_casadi); + free(capsule->expl_ode_fun); + {%- if solver_options.hessian_approx == "EXACT" %} + free(capsule->hess_vde_casadi); + {%- endif %} + +{%- elif solver_options.integrator_type == "GNSF" %} + for (int i = 0; i < {{ dims.N }}; i++) + { + external_function_param_casadi_free(&capsule->gnsf_phi_fun[i]); + external_function_param_casadi_free(&capsule->gnsf_phi_fun_jac_y[i]); + external_function_param_casadi_free(&capsule->gnsf_phi_jac_y_uhat[i]); + external_function_param_casadi_free(&capsule->gnsf_f_lo_jac_x1_x1dot_u_z[i]); + external_function_param_casadi_free(&capsule->gnsf_get_matrices_fun[i]); + } + free(capsule->gnsf_phi_fun); + free(capsule->gnsf_phi_fun_jac_y); + free(capsule->gnsf_phi_jac_y_uhat); + free(capsule->gnsf_f_lo_jac_x1_x1dot_u_z); + free(capsule->gnsf_get_matrices_fun); +{%- elif solver_options.integrator_type == "DISCRETE" %} + for (int i = 0; i < {{ dims.N }}; i++) + { + external_function_param_{{ model.dyn_ext_fun_type }}_free(&capsule->discr_dyn_phi_fun[i]); + external_function_param_{{ model.dyn_ext_fun_type }}_free(&capsule->discr_dyn_phi_fun_jac_ut_xt[i]); + {%- if solver_options.hessian_approx == "EXACT" %} + external_function_param_{{ model.dyn_ext_fun_type }}_free(&capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i]); + {%- endif %} + } + free(capsule->discr_dyn_phi_fun); + free(capsule->discr_dyn_phi_fun_jac_ut_xt); + {%- if solver_options.hessian_approx == "EXACT" %} + free(capsule->discr_dyn_phi_fun_jac_ut_xt_hess); + {%- endif %} + +{%- endif %} + + // cost +{%- if cost.cost_type_0 == "NONLINEAR_LS" %} + external_function_param_casadi_free(&capsule->cost_y_0_fun); + external_function_param_casadi_free(&capsule->cost_y_0_fun_jac_ut_xt); + external_function_param_casadi_free(&capsule->cost_y_0_hess); +{%- elif cost.cost_type_0 == "EXTERNAL" %} + external_function_param_{{ cost.cost_ext_fun_type_0 }}_free(&capsule->ext_cost_0_fun); + external_function_param_{{ cost.cost_ext_fun_type_0 }}_free(&capsule->ext_cost_0_fun_jac); + external_function_param_{{ cost.cost_ext_fun_type_0 }}_free(&capsule->ext_cost_0_fun_jac_hess); +{%- endif %} +{%- if cost.cost_type == "NONLINEAR_LS" %} + for (int i = 0; i < {{ dims.N }} - 1; i++) + { + external_function_param_casadi_free(&capsule->cost_y_fun[i]); + external_function_param_casadi_free(&capsule->cost_y_fun_jac_ut_xt[i]); + external_function_param_casadi_free(&capsule->cost_y_hess[i]); + } + free(capsule->cost_y_fun); + free(capsule->cost_y_fun_jac_ut_xt); + free(capsule->cost_y_hess); +{%- elif cost.cost_type == "EXTERNAL" %} + for (int i = 0; i < {{ dims.N }} - 1; i++) + { + external_function_param_{{ cost.cost_ext_fun_type }}_free(&capsule->ext_cost_fun[i]); + external_function_param_{{ cost.cost_ext_fun_type }}_free(&capsule->ext_cost_fun_jac[i]); + external_function_param_{{ cost.cost_ext_fun_type }}_free(&capsule->ext_cost_fun_jac_hess[i]); + } + free(capsule->ext_cost_fun); + free(capsule->ext_cost_fun_jac); + free(capsule->ext_cost_fun_jac_hess); +{%- endif %} +{%- if cost.cost_type_e == "NONLINEAR_LS" %} + external_function_param_casadi_free(&capsule->cost_y_e_fun); + external_function_param_casadi_free(&capsule->cost_y_e_fun_jac_ut_xt); + external_function_param_casadi_free(&capsule->cost_y_e_hess); +{%- elif cost.cost_type_e == "EXTERNAL" %} + external_function_param_{{ cost.cost_ext_fun_type_e }}_free(&capsule->ext_cost_e_fun); + external_function_param_{{ cost.cost_ext_fun_type_e }}_free(&capsule->ext_cost_e_fun_jac); + external_function_param_{{ cost.cost_ext_fun_type_e }}_free(&capsule->ext_cost_e_fun_jac_hess); +{%- endif %} + + // constraints +{%- if constraints.constr_type == "BGH" and dims.nh > 0 %} + for (int i = 0; i < {{ dims.N }}; i++) + { + external_function_param_casadi_free(&capsule->nl_constr_h_fun_jac[i]); + external_function_param_casadi_free(&capsule->nl_constr_h_fun[i]); + } + {%- if solver_options.hessian_approx == "EXACT" %} + for (int i = 0; i < {{ dims.N }}; i++) + { + external_function_param_casadi_free(&capsule->nl_constr_h_fun_jac_hess[i]); + } + {%- endif %} + free(capsule->nl_constr_h_fun_jac); + free(capsule->nl_constr_h_fun); + {%- if solver_options.hessian_approx == "EXACT" %} + free(capsule->nl_constr_h_fun_jac_hess); + {%- endif %} + +{%- elif constraints.constr_type == "BGP" and dims.nphi > 0 %} + for (int i = 0; i < {{ dims.N }}; i++) + { + external_function_param_casadi_free(&capsule->phi_constraint[i]); + } + free(capsule->phi_constraint); +{%- endif %} + +{%- if constraints.constr_type_e == "BGH" and dims.nh_e > 0 %} + external_function_param_casadi_free(&capsule->nl_constr_h_e_fun_jac); + external_function_param_casadi_free(&capsule->nl_constr_h_e_fun); +{%- if solver_options.hessian_approx == "EXACT" %} + external_function_param_casadi_free(&capsule->nl_constr_h_e_fun_jac_hess); +{%- endif %} +{%- elif constraints.constr_type_e == "BGP" and dims.nphi_e > 0 %} + external_function_param_casadi_free(&capsule->phi_e_constraint); +{%- endif %} + + return 0; +} + +ocp_nlp_in *{{ model.name }}_acados_get_nlp_in(nlp_solver_capsule * capsule) { return capsule->nlp_in; } +ocp_nlp_out *{{ model.name }}_acados_get_nlp_out(nlp_solver_capsule * capsule) { return capsule->nlp_out; } +ocp_nlp_solver *{{ model.name }}_acados_get_nlp_solver(nlp_solver_capsule * capsule) { return capsule->nlp_solver; } +ocp_nlp_config *{{ model.name }}_acados_get_nlp_config(nlp_solver_capsule * capsule) { return capsule->nlp_config; } +void *{{ model.name }}_acados_get_nlp_opts(nlp_solver_capsule * capsule) { return capsule->nlp_opts; } +ocp_nlp_dims *{{ model.name }}_acados_get_nlp_dims(nlp_solver_capsule * capsule) { return capsule->nlp_dims; } +ocp_nlp_plan *{{ model.name }}_acados_get_nlp_plan(nlp_solver_capsule * capsule) { return capsule->nlp_solver_plan; } + + +void {{ model.name }}_acados_print_stats(nlp_solver_capsule * capsule) +{ + int sqp_iter, stat_m, stat_n, tmp_int; + ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "sqp_iter", &sqp_iter); + ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "stat_n", &stat_n); + ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "stat_m", &stat_m); + + {% set stat_n_max = 10 %} + double stat[{{ solver_options.nlp_solver_max_iter * stat_n_max }}]; + ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "statistics", stat); + + int nrow = sqp_iter+1 < stat_m ? sqp_iter+1 : stat_m; + + printf("iter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter\n"); + for (int i = 0; i < nrow; i++) + { + for (int j = 0; j < stat_n + 1; j++) + { + if (j == 0 || j > 4) + { + tmp_int = (int) stat[i + j * nrow]; + printf("%d\t", tmp_int); + } + else + { + printf("%e\t", stat[i + j * nrow]); + } + } + printf("\n"); + } +} + diff --git a/pyextra/acados_template/c_templates_tera/acados_solver.in.h b/pyextra/acados_template/c_templates_tera/acados_solver.in.h new file mode 100644 index 0000000000..e630b8fe4e --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/acados_solver.in.h @@ -0,0 +1,132 @@ +/* + * 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_c/ocp_nlp_interface.h" +#include "acados_c/external_function_interface.h" + +#ifdef __cplusplus +extern "C" { +#endif + +// ** capsule for solver data ** +typedef struct nlp_solver_capsule +{ + // acados objects + ocp_nlp_in *nlp_in; + ocp_nlp_out *nlp_out; + ocp_nlp_solver *nlp_solver; + void *nlp_opts; + ocp_nlp_plan *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 + external_function_param_casadi *forw_vde_casadi; + external_function_param_casadi *expl_ode_fun; + external_function_param_casadi *hess_vde_casadi; + 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; + external_function_param_casadi *impl_dae_hess; + 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; + 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; + external_function_param_{{ model.dyn_ext_fun_type }} *discr_dyn_phi_fun_jac_ut_xt_hess; + + // cost + external_function_param_casadi *cost_y_fun; + external_function_param_casadi *cost_y_fun_jac_ut_xt; + external_function_param_casadi *cost_y_hess; + 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; + + 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; + 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; + + 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; + 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; + + // constraints + external_function_param_casadi *phi_constraint; + 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; + + external_function_param_casadi phi_e_constraint; + 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; +} nlp_solver_capsule; + +nlp_solver_capsule * {{ model.name }}_acados_create_capsule(); +int {{ model.name }}_acados_free_capsule(nlp_solver_capsule *capsule); + +int {{ model.name }}_acados_create(nlp_solver_capsule * capsule); +int {{ model.name }}_acados_update_params(nlp_solver_capsule * capsule, int stage, double *value, int np); +int {{ model.name }}_acados_solve(nlp_solver_capsule * capsule); +int {{ model.name }}_acados_free(nlp_solver_capsule * capsule); +void {{ model.name }}_acados_print_stats(nlp_solver_capsule * capsule); + +ocp_nlp_in *{{ model.name }}_acados_get_nlp_in(nlp_solver_capsule * capsule); +ocp_nlp_out *{{ model.name }}_acados_get_nlp_out(nlp_solver_capsule * capsule); +ocp_nlp_solver *{{ model.name }}_acados_get_nlp_solver(nlp_solver_capsule * capsule); +ocp_nlp_config *{{ model.name }}_acados_get_nlp_config(nlp_solver_capsule * capsule); +void *{{ model.name }}_acados_get_nlp_opts(nlp_solver_capsule * capsule); +ocp_nlp_dims *{{ model.name }}_acados_get_nlp_dims(nlp_solver_capsule * capsule); +ocp_nlp_plan *{{ model.name }}_acados_get_nlp_plan(nlp_solver_capsule * capsule); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_SOLVER_{{ model.name }}_H_ diff --git a/pyextra/acados_template/c_templates_tera/acados_solver_sfun.in.c b/pyextra/acados_template/c_templates_tera/acados_solver_sfun.in.c new file mode 100644 index 0000000000..93635d9848 --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/acados_solver_sfun.in.c @@ -0,0 +1,728 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + +#define S_FUNCTION_NAME acados_solver_sfunction_{{ model.name }} +#define S_FUNCTION_LEVEL 2 + +#define MDL_START + +// acados +#include "acados/utils/print.h" +#include "acados_c/sim_interface.h" +#include "acados_c/external_function_interface.h" + +// example specific +#include "{{ model.name }}_model/{{ model.name }}_model.h" +#include "acados_solver_{{ model.name }}.h" + +#include "simstruc.h" + +{% if simulink_opts.samplingtime == "t0" -%} +#define SAMPLINGTIME {{ solver_options.time_steps[0] }} +{%- elif simulink_opts.samplingtime == "-1" -%} +#define SAMPLINGTIME -1 +{%- else -%} + {{ throw(message = "simulink_opts.samplingtime must be '-1' or 't0', got val") }} +{%- endif %} + +static void mdlInitializeSizes (SimStruct *S) +{ + // specify the number of continuous and discrete states + ssSetNumContStates(S, 0); + ssSetNumDiscStates(S, 0); + + {%- for key, val in simulink_opts.inputs -%} + {%- if val != 0 and val != 1 -%} + {{ throw(message = "simulink_opts.inputs must be 0 or 1, got val") }} + {%- endif -%} + {%- endfor -%} + + {#- compute number of input ports #} + {%- set n_inputs = 0 -%} + {%- if dims.nbx_0 > 0 and simulink_opts.inputs.lbx_0 -%} {#- lbx_0 #} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif -%} + {%- if dims.nbx_0 > 0 and simulink_opts.inputs.ubx_0 -%} {#- ubx_0 #} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif -%} + {%- if dims.np > 0 and simulink_opts.inputs.parameter_traj -%} {#- parameter_traj #} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif -%} + {%- if dims.ny_0 > 0 and simulink_opts.inputs.y_ref_0 -%} {#- y_ref_0 -#} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif -%} + {%- if dims.ny > 0 and dims.N > 1 and simulink_opts.inputs.y_ref -%} {#- y_ref -#} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif -%} + {%- if dims.ny_e > 0 and dims.N > 0 and simulink_opts.inputs.y_ref_e -%} {#- y_ref_e #} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif -%} + {%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.lbx -%} {#- lbx #} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif -%} + {%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.ubx -%} {#- ubx #} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif -%} + {%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.lbx_e -%} {#- lbx_e #} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif -%} + {%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.ubx_e -%} {#- ubx_e #} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif -%} + {%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.lbu -%} {#- lbu #} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif -%} + {%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.ubu -%} {#- ubu #} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif -%} + {%- if dims.ng > 0 and simulink_opts.inputs.lg -%} {#- lg #} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif -%} + {%- if dims.ng > 0 and simulink_opts.inputs.ug -%} {#- ug #} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif -%} + {%- if dims.nh > 0 and simulink_opts.inputs.lh -%} {#- lh #} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif -%} + {%- if dims.nh > 0 and simulink_opts.inputs.uh -%} {#- uh #} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif -%} + + {%- for key, val in simulink_opts.inputs -%} + {%- if val != 0 and val != 1 -%} + {{ throw(message = "simulink_opts.inputs must be 0 or 1, got val") }} + {%- endif -%} + {%- endfor -%} + {%- if dims.ny_0 > 0 and simulink_opts.inputs.cost_W_0 %} {#- cost_W_0 #} + {%- set n_inputs = n_inputs + 1 %} + {%- endif -%} + {%- if dims.ny > 0 and simulink_opts.inputs.cost_W -%} {#- cost_W #} + {%- set n_inputs = n_inputs + 1 %} + {%- endif -%} + {%- if dims.ny_e > 0 and simulink_opts.inputs.cost_W_e -%} {#- cost_W_e #} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif -%} + + {%- if simulink_opts.inputs.x_init -%} {#- x_init #} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif -%} + + {%- if simulink_opts.inputs.u_init -%} {#- u_init #} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif -%} + + // specify the number of input ports + if ( !ssSetNumInputPorts(S, {{ n_inputs }}) ) + return; + + // specify the number of output ports + {%- set_global n_outputs = 0 %} + {%- for key, val in simulink_opts.outputs %} + {%- if val == 1 %} + {%- set_global n_outputs = n_outputs + val %} + {%- elif val != 0 %} + {{ throw(message = "simulink_opts.outputs must be 0 or 1, got val") }} + {%- endif %} + {%- endfor %} + if ( !ssSetNumOutputPorts(S, {{ n_outputs }}) ) + return; + + // specify dimension information for the input ports + {%- set i_input = -1 %}{# note here i_input is 0-based #} + {%- if dims.nbx_0 > 0 and simulink_opts.inputs.lbx_0 -%} {#- lbx_0 #} + {%- set i_input = i_input + 1 %} + // lbx_0 + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nbx_0 }}); + {%- endif %} + {%- if dims.nbx_0 > 0 and simulink_opts.inputs.ubx_0 -%} {#- ubx_0 #} + {%- set i_input = i_input + 1 %} + // ubx_0 + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nbx_0 }}); + {%- endif %} + + {%- if dims.np > 0 and simulink_opts.inputs.parameter_traj -%} {#- parameter_traj #} + {%- set i_input = i_input + 1 %} + // parameters + ssSetInputPortVectorDimension(S, {{ i_input }}, ({{ dims.N }}+1) * {{ dims.np }}); + {%- endif %} + + {%- if dims.ny > 0 and simulink_opts.inputs.y_ref_0 %} + {%- set i_input = i_input + 1 %} + // y_ref_0 + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.ny_0 }}); + {%- endif %} + + {%- if dims.ny > 0 and dims.N > 1 and simulink_opts.inputs.y_ref %} + {%- set i_input = i_input + 1 %} + // y_ref + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ (dims.N-1) * dims.ny }}); + {%- endif %} + + {%- if dims.ny_e > 0 and dims.N > 0 and simulink_opts.inputs.y_ref_e %} + {%- set i_input = i_input + 1 %} + // y_ref_e + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.ny_e }}); + {%- endif %} + + {%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.lbx -%} {#- lbx #} + {%- set i_input = i_input + 1 %} + // lbx + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ (dims.N-1) * dims.nbx }}); + {%- endif %} + {%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.ubx -%} {#- ubx #} + {%- set i_input = i_input + 1 %} + // ubx + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ (dims.N-1) * dims.nbx }}); + {%- endif %} + + {%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.lbx_e -%} {#- lbx_e #} + {%- set i_input = i_input + 1 %} + // lbx_e + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nbx_e }}); + {%- endif %} + {%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.ubx_e -%} {#- ubx_e #} + {%- set i_input = i_input + 1 %} + // ubx_e + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nbx_e }}); + {%- endif %} + + {%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.lbu -%} {#- lbu #} + {%- set i_input = i_input + 1 %} + // lbu + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.N*dims.nbu }}); + {%- endif -%} + {%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.ubu -%} {#- ubu #} + {%- set i_input = i_input + 1 %} + // ubu + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.N*dims.nbu }}); + {%- endif -%} + + + {%- if dims.ng > 0 and simulink_opts.inputs.lg -%} {#- lg #} + {%- set i_input = i_input + 1 %} + // lg + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.ng }}); + {%- endif -%} + {%- if dims.ng > 0 and simulink_opts.inputs.ug -%} {#- ug #} + {%- set i_input = i_input + 1 %} + // ug + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.ng }}); + {%- endif -%} + + {%- if dims.nh > 0 and simulink_opts.inputs.lh -%} {#- lh #} + {%- set i_input = i_input + 1 %} + // lh + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nh }}); + {%- endif -%} + {%- if dims.nh > 0 and simulink_opts.inputs.uh -%} {#- uh #} + {%- set i_input = i_input + 1 %} + // uh + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nh }}); + {%- endif -%} + + {%- if dims.ny_0 > 0 and simulink_opts.inputs.cost_W_0 %} {#- cost_W_0 #} + {%- set i_input = i_input + 1 %} + // cost_W_0 + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.ny_0 * dims.ny_0 }}); + {%- endif %} + + {%- if dims.ny_0 > 0 and simulink_opts.inputs.cost_W %} {#- cost_W #} + {%- set i_input = i_input + 1 %} + // cost_W + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.ny * dims.ny }}); + {%- endif %} + + {%- if dims.ny_e > 0 and simulink_opts.inputs.cost_W_e %} {#- cost_W_e #} + {%- set i_input = i_input + 1 %} + // cost_W_e + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.ny_e * dims.ny_e }}); + {%- endif %} + + {%- if simulink_opts.inputs.x_init -%} {#- x_init #} + {%- set i_input = i_input + 1 %} + // x_init + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nx * (dims.N+1) }}); + {%- endif -%} + + {%- if simulink_opts.inputs.u_init -%} {#- u_init #} + {%- set i_input = i_input + 1 %} + // u_init + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nu * (dims.N) }}); + {%- endif -%} + + /* specify dimension information for the OUTPUT ports */ + {%- set i_output = -1 %}{# note here i_output is 0-based #} + {%- if dims.nu > 0 and simulink_opts.outputs.u0 == 1 %} + {%- set i_output = i_output + 1 %} + ssSetOutputPortVectorDimension(S, {{ i_output }}, {{ dims.nu }} ); + {%- endif %} + + {%- if simulink_opts.outputs.utraj == 1 %} + {%- set i_output = i_output + 1 %} + ssSetOutputPortVectorDimension(S, {{ i_output }}, {{ dims.nu * dims.N }} ); + {%- endif %} + + {%- if simulink_opts.outputs.xtraj == 1 %} + {%- set i_output = i_output + 1 %} + ssSetOutputPortVectorDimension(S, {{ i_output }}, {{ dims.nx * (dims.N+1) }} ); + {%- endif %} + + {%- if simulink_opts.outputs.solver_status == 1 %} + {%- set i_output = i_output + 1 %} + ssSetOutputPortVectorDimension(S, {{ i_output }}, 1 ); + {%- endif %} + + {%- if simulink_opts.outputs.KKT_residual == 1 %} + {%- set i_output = i_output + 1 %} + ssSetOutputPortVectorDimension(S, {{ i_output }}, 1 ); + {%- endif %} + + {%- if dims.N > 0 and simulink_opts.outputs.x1 == 1 %} + {%- set i_output = i_output + 1 %} + ssSetOutputPortVectorDimension(S, {{ i_output }}, {{ dims.nx }} ); // state at shooting node 1 + {%- endif %} + + {%- if simulink_opts.outputs.CPU_time == 1 %} + {%- set i_output = i_output + 1 %} + ssSetOutputPortVectorDimension(S, {{ i_output }}, 1); + {%- endif %} + + {%- if simulink_opts.outputs.sqp_iter == 1 %} + {%- set i_output = i_output + 1 %} + ssSetOutputPortVectorDimension(S, {{ i_output }}, 1 ); + {%- endif %} + + // specify the direct feedthrough status + // should be set to 1 for all inputs used in mdlOutputs + {%- for i in range(end=n_inputs) %} + ssSetInputPortDirectFeedThrough(S, {{ i }}, 1); + {%- endfor %} + + // one sample time + ssSetNumSampleTimes(S, 1); +} + + +#if defined(MATLAB_MEX_FILE) + +#define MDL_SET_INPUT_PORT_DIMENSION_INFO +#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO + +static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo) +{ + if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) ) + return; +} + +static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo) +{ + if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) ) + return; +} + +#endif /* MATLAB_MEX_FILE */ + + +static void mdlInitializeSampleTimes(SimStruct *S) +{ + ssSetSampleTime(S, 0, SAMPLINGTIME); + ssSetOffsetTime(S, 0, 0.0); +} + + +static void mdlStart(SimStruct *S) +{ + nlp_solver_capsule *capsule = {{ model.name }}_acados_create_capsule(); + {{ model.name }}_acados_create(capsule); + + ssSetUserData(S, (void*)capsule); +} + + +static void mdlOutputs(SimStruct *S, int_T tid) +{ + nlp_solver_capsule *capsule = ssGetUserData(S); + ocp_nlp_config *nlp_config = {{ model.name }}_acados_get_nlp_config(capsule); + ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims(capsule); + ocp_nlp_in *nlp_in = {{ model.name }}_acados_get_nlp_in(capsule); + ocp_nlp_out *nlp_out = {{ model.name }}_acados_get_nlp_out(capsule); + + InputRealPtrsType in_sign; + + {%- set buffer_sizes = [dims.nbx_0, dims.np, dims.nbx, dims.nbu, dims.ng, dims.nh, dims.nx] -%} + + {%- if dims.ny_0 > 0 and simulink_opts.inputs.y_ref_0 %} {# y_ref_0 #} + {%- set buffer_sizes = buffer_sizes | concat(with=(dims.ny_0)) %} + {%- endif %} + {%- if dims.ny > 0 and dims.N > 1 and simulink_opts.inputs.y_ref %} {# y_ref #} + {%- set buffer_sizes = buffer_sizes | concat(with=(dims.ny)) %} + {%- endif %} + {%- if dims.ny_e > 0 and dims.N > 0 and simulink_opts.inputs.y_ref_e %} {# y_ref_e #} + {%- set buffer_sizes = buffer_sizes | concat(with=(dims.ny_e)) %} + {%- endif %} + + {%- if dims.ny_0 > 0 and simulink_opts.inputs.cost_W_0 %} {# cost_W_0 #} + {%- set buffer_sizes = buffer_sizes | concat(with=(dims.ny_0 * dims.ny_0)) %} + {%- endif %} + {%- if dims.ny > 0 and simulink_opts.inputs.cost_W %} {# cost_W #} + {%- set buffer_sizes = buffer_sizes | concat(with=(dims.ny * dims.ny)) %} + {%- endif %} + {%- if dims.ny_e > 0 and simulink_opts.inputs.cost_W_e %} {# cost_W_e #} + {%- set buffer_sizes = buffer_sizes | concat(with=(dims.ny_e * dims.ny_e)) %} + {%- endif %} + + // local buffer + {%- set buffer_size = buffer_sizes | sort | last %} + real_t buffer[{{ buffer_size }}]; + + /* go through inputs */ + {%- set i_input = -1 %} + {%- if dims.nbx_0 > 0 and simulink_opts.inputs.lbx_0 -%} {#- lbx_0 #} + // lbx_0 + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + for (int i = 0; i < {{ dims.nbx_0 }}; i++) + buffer[i] = (double)(*in_sign[i]); + + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", buffer); + {%- endif %} + + {%- if dims.nbx_0 > 0 and simulink_opts.inputs.ubx_0 -%} {#- ubx_0 #} + // ubx_0 + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + for (int i = 0; i < {{ dims.nbx_0 }}; i++) + buffer[i] = (double)(*in_sign[i]); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", buffer); + {%- endif %} + + {%- if dims.np > 0 and simulink_opts.inputs.parameter_traj -%} {#- parameter_traj #} + // parameters - stage-variant !!! + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + + // update value of parameters + for (int ii = 0; ii <= {{ dims.N }}; ii++) + { + for (int jj = 0; jj < {{ dims.np }}; jj++) + buffer[jj] = (double)(*in_sign[ii*{{dims.np}}+jj]); + {{ model.name }}_acados_update_params(capsule, ii, buffer, {{ dims.np }}); + } + {%- endif %} + + {% if dims.ny_0 > 0 and simulink_opts.inputs.y_ref_0 %} + // y_ref_0 + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + + for (int i = 0; i < {{ dims.ny_0 }}; i++) + buffer[i] = (double)(*in_sign[i]); + + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", (void *) buffer); + {%- endif %} + + {% if dims.ny > 0 and dims.N > 1 and simulink_opts.inputs.y_ref %} + // y_ref - for stages 1 to N-1 + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + + for (int ii = 1; ii < {{ dims.N }}; ii++) + { + for (int jj = 0; jj < {{ dims.ny }}; jj++) + buffer[jj] = (double)(*in_sign[(ii-1)*{{ dims.ny }}+jj]); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, ii, "yref", (void *) buffer); + } + {%- endif %} + + {% if dims.ny_e > 0 and dims.N > 0 and simulink_opts.inputs.y_ref_e %} + // y_ref_e + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + + for (int i = 0; i < {{ dims.ny_e }}; i++) + buffer[i] = (double)(*in_sign[i]); + + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, {{ dims.N }}, "yref", (void *) buffer); + {%- endif %} + + {%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.lbx -%} {#- lbx #} + // lbx + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + for (int ii = 1; ii < {{ dims.N }}; ii++) + { + for (int jj = 0; jj < {{ dims.nbx }}; jj++) + buffer[jj] = (double)(*in_sign[(ii-1)*{{ dims.nbx }}+jj]); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lbx", (void *) buffer); + } + {%- endif %} + {%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.ubx -%} {#- ubx #} + // ubx + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + for (int ii = 1; ii < {{ dims.N }}; ii++) + { + for (int jj = 0; jj < {{ dims.nbx }}; jj++) + buffer[jj] = (double)(*in_sign[(ii-1)*{{ dims.nbx }}+jj]); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ubx", (void *) buffer); + } + {%- endif %} + + + {%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.lbx_e -%} {#- lbx_e #} + // lbx_e + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + + for (int i = 0; i < {{ dims.nbx_e }}; i++) + buffer[i] = (double)(*in_sign[i]); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, {{ dims.N }}, "lbx", buffer); + {%- endif %} + {%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.ubx_e -%} {#- ubx_e #} + // ubx_e + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + + for (int i = 0; i < {{ dims.nbx_e }}; i++) + buffer[i] = (double)(*in_sign[i]); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, {{ dims.N }}, "ubx", buffer); + {%- endif %} + + + {%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.lbu -%} {#- lbu #} + // lbu + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + for (int ii = 0; ii < {{ dims.N }}; ii++) + { + for (int jj = 0; jj < {{ dims.nbu }}; jj++) + buffer[jj] = (double)(*in_sign[ii*{{ dims.nbu }}+jj]); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lbu", (void *) buffer); + } + {%- endif -%} + {%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.ubu -%} {#- ubu #} + // ubu + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + for (int ii = 0; ii < {{ dims.N }}; ii++) + { + for (int jj = 0; jj < {{ dims.nbu }}; jj++) + buffer[jj] = (double)(*in_sign[ii*{{ dims.nbu }}+jj]); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ubu", (void *) buffer); + } + {%- endif -%} + + {%- if dims.ng > 0 and simulink_opts.inputs.lg -%} {#- lg #} + // lg + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + + for (int i = 0; i < {{ dims.ng }}; i++) + buffer[i] = (double)(*in_sign[i]); + + for (int ii = 0; ii < {{ dims.N }}; ii++) + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lg", buffer); + {%- endif -%} + {%- if dims.ng > 0 and simulink_opts.inputs.ug -%} {#- ug #} + // ug + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + + for (int i = 0; i < {{ dims.ng }}; i++) + buffer[i] = (double)(*in_sign[i]); + + for (int ii = 0; ii < {{ dims.N }}; ii++) + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ug", buffer); + {%- endif -%} + {%- if dims.nh > 0 and simulink_opts.inputs.lh -%} {#- lh #} + // lh + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + + for (int i = 0; i < {{ dims.nh }}; i++) + buffer[i] = (double)(*in_sign[i]); + + for (int ii = 0; ii < {{ dims.N }}; ii++) + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lh", buffer); + {%- endif -%} + {%- if dims.nh > 0 and simulink_opts.inputs.uh -%} {#- uh #} + // uh + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + + for (int i = 0; i < {{ dims.nh }}; i++) + buffer[i] = (double)(*in_sign[i]); + + for (int ii = 0; ii < {{ dims.N }}; ii++) + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "uh", buffer); + {%- endif -%} + + {%- if dims.ny_0 > 0 and simulink_opts.inputs.cost_W_0 %} {# cost_W_0 #} + // cost_W_0 + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + for (int i = 0; i < {{ dims.ny_0 * dims.ny_0 }}; i++) + buffer[i] = (double)(*in_sign[i]); + + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "W", buffer); + {%- endif %} + + {%- if dims.ny > 0 and simulink_opts.inputs.cost_W %} {# cost_W #} + // cost_W + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + for (int i = 0; i < {{ dims.ny * dims.ny }}; i++) + buffer[i] = (double)(*in_sign[i]); + + for (int ii = 1; ii < {{ dims.N }}; ii++) + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, ii, "W", buffer); + {%- endif %} + + {%- if dims.ny_e > 0 and simulink_opts.inputs.cost_W_e %} {#- cost_W_e #} + // cost_W_e + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + for (int i = 0; i < {{ dims.ny_e * dims.ny_e }}; i++) + buffer[i] = (double)(*in_sign[i]); + + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, {{ dims.N }}, "W", buffer); + {%- endif %} + + {%- if simulink_opts.inputs.x_init %} {#- x_init #} + // x_init + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + for (int ii = 0; ii < {{ dims.N + 1 }}; ii++) + { + for (int jj = 0; jj < {{ dims.nx }}; jj++) + buffer[jj] = (double)(*in_sign[(ii)*{{ dims.nx }}+jj]); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, ii, "x", (void *) buffer); + } + {%- endif %} + + {%- if simulink_opts.inputs.u_init %} {#- u_init #} + // u_init + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + for (int ii = 0; ii < {{ dims.N }}; ii++) + { + for (int jj = 0; jj < {{ dims.nu }}; jj++) + buffer[jj] = (double)(*in_sign[(ii)*{{ dims.nu }}+jj]); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, ii, "u", (void *) buffer); + } + {%- endif %} + + /* call solver */ + int rti_phase = 0; + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "rti_phase", &rti_phase); + int acados_status = {{ model.name }}_acados_solve(capsule); + + + /* set outputs */ + // assign pointers to output signals + real_t *out_u0, *out_utraj, *out_xtraj, *out_status, *out_sqp_iter, *out_KKT_res, *out_x1, *out_cpu_time; + int tmp_int; + + {%- set i_output = -1 -%}{# note here i_output is 0-based #} + {%- if dims.nu > 0 and simulink_opts.outputs.u0 == 1 %} + {%- set i_output = i_output + 1 %} + out_u0 = ssGetOutputPortRealSignal(S, {{ i_output }}); + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "u", (void *) out_u0); + {%- endif %} + + {%- if simulink_opts.outputs.utraj == 1 %} + {%- set i_output = i_output + 1 %} + out_utraj = ssGetOutputPortRealSignal(S, {{ i_output }}); + for (int ii = 0; ii < {{ dims.N }}; ii++) + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, + "u", (void *) (out_utraj + ii * {{ dims.nu }})); + {%- endif %} + + {% if simulink_opts.outputs.xtraj == 1 %} + {%- set i_output = i_output + 1 %} + + out_xtraj = ssGetOutputPortRealSignal(S, {{ i_output }}); + for (int ii = 0; ii < {{ dims.N + 1 }}; ii++) + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, + "x", (void *) (out_xtraj + ii * {{ dims.nx }})); + {%- endif %} + + {%- if simulink_opts.outputs.solver_status == 1 %} + {%- set i_output = i_output + 1 %} + out_status = ssGetOutputPortRealSignal(S, {{ i_output }}); + *out_status = (real_t) acados_status; + {%- endif %} + + {%- if simulink_opts.outputs.KKT_residual == 1 %} + {%- set i_output = i_output + 1 %} + out_KKT_res = ssGetOutputPortRealSignal(S, {{ i_output }}); + *out_KKT_res = (real_t) nlp_out->inf_norm_res; + {%- endif %} + + {%- if dims.N > 0 and simulink_opts.outputs.x1 == 1 %} + {%- set i_output = i_output + 1 %} + out_x1 = ssGetOutputPortRealSignal(S, {{ i_output }}); + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 1, "x", (void *) out_x1); + {%- endif %} + + {%- if simulink_opts.outputs.CPU_time == 1 %} + {%- set i_output = i_output + 1 %} + out_cpu_time = ssGetOutputPortRealSignal(S, {{ i_output }}); + // get solution time + ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_tot", (void *) out_cpu_time); + {%- endif -%} + + {%- if simulink_opts.outputs.sqp_iter == 1 %} + {%- set i_output = i_output + 1 %} + out_sqp_iter = ssGetOutputPortRealSignal(S, {{ i_output }}); + // get sqp iter + ocp_nlp_get(nlp_config, capsule->nlp_solver, "sqp_iter", (void *) &tmp_int); + *out_sqp_iter = (real_t) tmp_int; + {%- endif %} + +} + +static void mdlTerminate(SimStruct *S) +{ + nlp_solver_capsule *capsule = ssGetUserData(S); + + {{ model.name }}_acados_free(capsule); + {{ model.name }}_acados_free_capsule(capsule); +} + + +#ifdef MATLAB_MEX_FILE +#include "simulink.c" +#else +#include "cg_sfun.h" +#endif diff --git a/pyextra/acados_template/c_templates_tera/cost_y_0_fun.in.h b/pyextra/acados_template/c_templates_tera/cost_y_0_fun.in.h new file mode 100644 index 0000000000..5dc611dd38 --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/cost_y_0_fun.in.h @@ -0,0 +1,69 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef {{ model.name }}_Y_0_COST +#define {{ model.name }}_Y_0_COST + +#ifdef __cplusplus +extern "C" { +#endif + +{% if cost.cost_type_0 == "NONLINEAR_LS" %} +int {{ model.name }}_cost_y_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_y_0_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_y_0_fun_sparsity_in(int); +const int *{{ model.name }}_cost_y_0_fun_sparsity_out(int); +int {{ model.name }}_cost_y_0_fun_n_in(); +int {{ model.name }}_cost_y_0_fun_n_out(); + +int {{ model.name }}_cost_y_0_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_y_0_fun_jac_ut_xt_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_y_0_fun_jac_ut_xt_sparsity_in(int); +const int *{{ model.name }}_cost_y_0_fun_jac_ut_xt_sparsity_out(int); +int {{ model.name }}_cost_y_0_fun_jac_ut_xt_n_in(); +int {{ model.name }}_cost_y_0_fun_jac_ut_xt_n_out(); + +int {{ model.name }}_cost_y_0_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_y_0_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_y_0_hess_sparsity_in(int); +const int *{{ model.name }}_cost_y_0_hess_sparsity_out(int); +int {{ model.name }}_cost_y_0_hess_n_in(); +int {{ model.name }}_cost_y_0_hess_n_out(); +{% endif %} + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // {{ model.name }}_Y_0_COST diff --git a/pyextra/acados_template/c_templates_tera/cost_y_e_fun.in.h b/pyextra/acados_template/c_templates_tera/cost_y_e_fun.in.h new file mode 100644 index 0000000000..041f356939 --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/cost_y_e_fun.in.h @@ -0,0 +1,69 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef {{ model.name }}_Y_E_COST +#define {{ model.name }}_Y_E_COST + +#ifdef __cplusplus +extern "C" { +#endif + +{% if cost.cost_type_e == "NONLINEAR_LS" %} +int {{ model.name }}_cost_y_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_y_e_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_y_e_fun_sparsity_in(int); +const int *{{ model.name }}_cost_y_e_fun_sparsity_out(int); +int {{ model.name }}_cost_y_e_fun_n_in(); +int {{ model.name }}_cost_y_e_fun_n_out(); + +int {{ model.name }}_cost_y_e_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_y_e_fun_jac_ut_xt_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_y_e_fun_jac_ut_xt_sparsity_in(int); +const int *{{ model.name }}_cost_y_e_fun_jac_ut_xt_sparsity_out(int); +int {{ model.name }}_cost_y_e_fun_jac_ut_xt_n_in(); +int {{ model.name }}_cost_y_e_fun_jac_ut_xt_n_out(); + +int {{ model.name }}_cost_y_e_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_y_e_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_y_e_hess_sparsity_in(int); +const int *{{ model.name }}_cost_y_e_hess_sparsity_out(int); +int {{ model.name }}_cost_y_e_hess_n_in(); +int {{ model.name }}_cost_y_e_hess_n_out(); +{% endif %} + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // {{ model.name }}_Y_E_COST diff --git a/pyextra/acados_template/c_templates_tera/cost_y_fun.in.h b/pyextra/acados_template/c_templates_tera/cost_y_fun.in.h new file mode 100644 index 0000000000..ec2119603f --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/cost_y_fun.in.h @@ -0,0 +1,69 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef {{ model.name }}_Y_COST +#define {{ model.name }}_Y_COST + +#ifdef __cplusplus +extern "C" { +#endif + +{% if cost.cost_type == "NONLINEAR_LS" %} +int {{ model.name }}_cost_y_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_y_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_y_fun_sparsity_in(int); +const int *{{ model.name }}_cost_y_fun_sparsity_out(int); +int {{ model.name }}_cost_y_fun_n_in(); +int {{ model.name }}_cost_y_fun_n_out(); + +int {{ model.name }}_cost_y_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_y_fun_jac_ut_xt_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_y_fun_jac_ut_xt_sparsity_in(int); +const int *{{ model.name }}_cost_y_fun_jac_ut_xt_sparsity_out(int); +int {{ model.name }}_cost_y_fun_jac_ut_xt_n_in(); +int {{ model.name }}_cost_y_fun_jac_ut_xt_n_out(); + +int {{ model.name }}_cost_y_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_y_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_y_hess_sparsity_in(int); +const int *{{ model.name }}_cost_y_hess_sparsity_out(int); +int {{ model.name }}_cost_y_hess_n_in(); +int {{ model.name }}_cost_y_hess_n_out(); +{% endif %} + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // {{ model.name }}_Y_COST diff --git a/pyextra/acados_template/c_templates_tera/external_cost.in.h b/pyextra/acados_template/c_templates_tera/external_cost.in.h new file mode 100644 index 0000000000..19280182e5 --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/external_cost.in.h @@ -0,0 +1,74 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef {{ model.name }}_EXT_COST +#define {{ model.name }}_EXT_COST + +#ifdef __cplusplus +extern "C" { +#endif + +{% if cost.cost_ext_fun_type == "casadi" %} +{% if cost.cost_type == "EXTERNAL" %} +int {{ model.name }}_cost_ext_cost_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_ext_cost_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_ext_cost_fun_sparsity_in(int); +const int *{{ model.name }}_cost_ext_cost_fun_sparsity_out(int); +int {{ model.name }}_cost_ext_cost_fun_n_in(); +int {{ model.name }}_cost_ext_cost_fun_n_out(); + +int {{ model.name }}_cost_ext_cost_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_ext_cost_fun_jac_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_ext_cost_fun_jac_hess_sparsity_in(int); +const int *{{ model.name }}_cost_ext_cost_fun_jac_hess_sparsity_out(int); +int {{ model.name }}_cost_ext_cost_fun_jac_hess_n_in(); +int {{ model.name }}_cost_ext_cost_fun_jac_hess_n_out(); + +int {{ model.name }}_cost_ext_cost_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_ext_cost_fun_jac_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_ext_cost_fun_jac_sparsity_in(int); +const int *{{ model.name }}_cost_ext_cost_fun_jac_sparsity_out(int); +int {{ model.name }}_cost_ext_cost_fun_jac_n_in(); +int {{ model.name }}_cost_ext_cost_fun_jac_n_out(); +{% endif %} + +{% else %} +int {{ cost.cost_function_ext_cost }}(void **, void **, void *); +{% endif %} + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // {{ model.name }}_EXT_COST diff --git a/pyextra/acados_template/c_templates_tera/external_cost_0.in.h b/pyextra/acados_template/c_templates_tera/external_cost_0.in.h new file mode 100644 index 0000000000..0367e3d0c6 --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/external_cost_0.in.h @@ -0,0 +1,75 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef {{ model.name }}_EXT_COST_0 +#define {{ model.name }}_EXT_COST_0 + +#ifdef __cplusplus +extern "C" { +#endif + +{% if cost.cost_ext_fun_type_0 == "casadi" %} + +{% if cost.cost_type_0 == "EXTERNAL" %} +int {{ model.name }}_cost_ext_cost_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_ext_cost_0_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_ext_cost_0_fun_sparsity_in(int); +const int *{{ model.name }}_cost_ext_cost_0_fun_sparsity_out(int); +int {{ model.name }}_cost_ext_cost_0_fun_n_in(); +int {{ model.name }}_cost_ext_cost_0_fun_n_out(); + +int {{ model.name }}_cost_ext_cost_0_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_ext_cost_0_fun_jac_hess_sparsity_in(int); +const int *{{ model.name }}_cost_ext_cost_0_fun_jac_hess_sparsity_out(int); +int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_n_in(); +int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_n_out(); + +int {{ model.name }}_cost_ext_cost_0_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_ext_cost_0_fun_jac_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_ext_cost_0_fun_jac_sparsity_in(int); +const int *{{ model.name }}_cost_ext_cost_0_fun_jac_sparsity_out(int); +int {{ model.name }}_cost_ext_cost_0_fun_jac_n_in(); +int {{ model.name }}_cost_ext_cost_0_fun_jac_n_out(); +{% endif %} + +{% else %} +int {{ cost.cost_function_ext_cost_0 }}(void **, void **, void *); +{% endif %} + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // {{ model.name }}_EXT_COST_0 diff --git a/pyextra/acados_template/c_templates_tera/external_cost_e.in.h b/pyextra/acados_template/c_templates_tera/external_cost_e.in.h new file mode 100644 index 0000000000..de4f608b62 --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/external_cost_e.in.h @@ -0,0 +1,74 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef {{ model.name }}_EXT_COST_E +#define {{ model.name }}_EXT_COST_E + +#ifdef __cplusplus +extern "C" { +#endif + +{% if cost.cost_ext_fun_type_e == "casadi" %} +{% if cost.cost_type_e == "EXTERNAL" %} +int {{ model.name }}_cost_ext_cost_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_ext_cost_e_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_ext_cost_e_fun_sparsity_in(int); +const int *{{ model.name }}_cost_ext_cost_e_fun_sparsity_out(int); +int {{ model.name }}_cost_ext_cost_e_fun_n_in(); +int {{ model.name }}_cost_ext_cost_e_fun_n_out(); + +int {{ model.name }}_cost_ext_cost_e_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_ext_cost_e_fun_jac_hess_sparsity_in(int); +const int *{{ model.name }}_cost_ext_cost_e_fun_jac_hess_sparsity_out(int); +int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_n_in(); +int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_n_out(); + +int {{ model.name }}_cost_ext_cost_e_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_ext_cost_e_fun_jac_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_ext_cost_e_fun_jac_sparsity_in(int); +const int *{{ model.name }}_cost_ext_cost_e_fun_jac_sparsity_out(int); +int {{ model.name }}_cost_ext_cost_e_fun_jac_n_in(); +int {{ model.name }}_cost_ext_cost_e_fun_jac_n_out(); +{% endif %} + +{% else %} +int {{ cost.cost_function_ext_cost_e }}(void **, void **, void *); +{% endif %} + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // {{ model.name }}_EXT_COST_E diff --git a/pyextra/acados_template/c_templates_tera/h_constraint.in.h b/pyextra/acados_template/c_templates_tera/h_constraint.in.h new file mode 100644 index 0000000000..82e3d60840 --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/h_constraint.in.h @@ -0,0 +1,70 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + +#ifndef {{ model.name }}_H_CONSTRAINT +#define {{ model.name }}_H_CONSTRAINT + +#ifdef __cplusplus +extern "C" { +#endif + +{% if dims.nh > 0 %} +int {{ model.name }}_constr_h_fun_jac_uxt_zt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_constr_h_fun_jac_uxt_zt_work(int *, int *, int *, int *); +const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_sparsity_in(int); +const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_sparsity_out(int); +int {{ model.name }}_constr_h_fun_jac_uxt_zt_n_in(); +int {{ model.name }}_constr_h_fun_jac_uxt_zt_n_out(); + +int {{ model.name }}_constr_h_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_constr_h_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_constr_h_fun_sparsity_in(int); +const int *{{ model.name }}_constr_h_fun_sparsity_out(int); +int {{ model.name }}_constr_h_fun_n_in(); +int {{ model.name }}_constr_h_fun_n_out(); + +{% if solver_options.hessian_approx == "EXACT" -%} +int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_sparsity_in(int); +const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_sparsity_out(int); +int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess_n_in(); +int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess_n_out(); +{% endif %} +{% endif %} + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // {{ model.name }}_H_CONSTRAINT diff --git a/pyextra/acados_template/c_templates_tera/h_e_constraint.in.h b/pyextra/acados_template/c_templates_tera/h_e_constraint.in.h new file mode 100644 index 0000000000..7ac689faa6 --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/h_e_constraint.in.h @@ -0,0 +1,71 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef {{ model.name }}_H_E_CONSTRAINT +#define {{ model.name }}_H_E_CONSTRAINT + +#ifdef __cplusplus +extern "C" { +#endif + +{% if dims.nh_e > 0 %} +int {{ model.name }}_constr_h_e_fun_jac_uxt_zt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_work(int *, int *, int *, int *); +const int *{{ model.name }}_constr_h_e_fun_jac_uxt_zt_sparsity_in(int); +const int *{{ model.name }}_constr_h_e_fun_jac_uxt_zt_sparsity_out(int); +int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_n_in(); +int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_n_out(); + +int {{ model.name }}_constr_h_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_constr_h_e_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_constr_h_e_fun_sparsity_in(int); +const int *{{ model.name }}_constr_h_e_fun_sparsity_out(int); +int {{ model.name }}_constr_h_e_fun_n_in(); +int {{ model.name }}_constr_h_e_fun_n_out(); + +{% if solver_options.hessian_approx == "EXACT" -%} +int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_sparsity_in(int); +const int *{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_sparsity_out(int); +int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_n_in(); +int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_n_out(); +{% endif %} +{% endif %} + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // {{ model.name }}_H_E_CONSTRAINT diff --git a/pyextra/acados_template/c_templates_tera/main.in.c b/pyextra/acados_template/c_templates_tera/main.in.c new file mode 100644 index 0000000000..1cae69aea6 --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/main.in.c @@ -0,0 +1,181 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +// standard +#include +#include +// acados +#include "acados/utils/print.h" +#include "acados/utils/math.h" +#include "acados_c/ocp_nlp_interface.h" +#include "acados_c/external_function_interface.h" +#include "acados_solver_{{ model.name }}.h" + + +int main() +{ + + nlp_solver_capsule *acados_ocp_capsule = {{ model.name }}_acados_create_capsule(); + int status = {{ model.name }}_acados_create(acados_ocp_capsule); + + if (status) + { + printf("{{ model.name }}_acados_create() returned status %d. Exiting.\n", status); + exit(1); + } + + ocp_nlp_config *nlp_config = {{ model.name }}_acados_get_nlp_config(acados_ocp_capsule); + ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims(acados_ocp_capsule); + ocp_nlp_in *nlp_in = {{ model.name }}_acados_get_nlp_in(acados_ocp_capsule); + ocp_nlp_out *nlp_out = {{ model.name }}_acados_get_nlp_out(acados_ocp_capsule); + ocp_nlp_solver *nlp_solver = {{ model.name }}_acados_get_nlp_solver(acados_ocp_capsule); + void *nlp_opts = {{ model.name }}_acados_get_nlp_opts(acados_ocp_capsule); + + // initial condition + int idxbx0[{{ dims.nbx_0 }}]; + {%- for i in range(end=dims.nbx_0) %} + idxbx0[{{ i }}] = {{ constraints.idxbx_0[i] }}; + {%- endfor %} + + double lbx0[{{ dims.nbx_0 }}]; + double ubx0[{{ dims.nbx_0 }}]; + {%- for i in range(end=dims.nbx_0) %} + lbx0[{{ i }}] = {{ constraints.lbx_0[i] }}; + ubx0[{{ i }}] = {{ constraints.ubx_0[i] }}; + {%- endfor %} + + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxbx", idxbx0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", lbx0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", ubx0); + + // initialization for state values + double x_init[{{ dims.nx }}]; + {%- for i in range(end=dims.nx) %} + x_init[{{ i }}] = 0.0; + {%- endfor %} + + // initial value for control input + double u0[{{ dims.nu }}]; + {%- for i in range(end=dims.nu) %} + u0[{{ i }}] = 0.0; + {%- endfor %} + + + {%- if dims.np > 0 %} + // set parameters + double p[{{ dims.np }}]; + {% for item in parameter_values %} + p[{{ loop.index0 }}] = {{ item }}; + {% endfor %} + + for (int ii = 0; ii <= {{ dims.N }}; ii++) + { + {{ model.name }}_acados_update_params(acados_ocp_capsule, ii, p, {{ dims.np }}); + } + {% endif %}{# if np > 0 #} + + // prepare evaluation + int NTIMINGS = 1; + double min_time = 1e12; + double kkt_norm_inf; + double elapsed_time; + int sqp_iter; + + double xtraj[{{ dims.nx }} * ({{ dims.N }}+1)]; + double utraj[{{ dims.nu }} * ({{ dims.N }})]; + + + // solve ocp in loop + int rti_phase = 0; + + for (int ii = 0; ii < NTIMINGS; ii++) + { + // initialize solution + for (int i = 0; i <= nlp_dims->N; i++) + { + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "x", x_init); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "u", u0); + } + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "rti_phase", &rti_phase); + status = {{ model.name }}_acados_solve(acados_ocp_capsule); + ocp_nlp_get(nlp_config, nlp_solver, "time_tot", &elapsed_time); + min_time = MIN(elapsed_time, min_time); + } + + /* print solution and statistics */ + for (int ii = 0; ii <= nlp_dims->N; ii++) + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "x", &xtraj[ii*{{ dims.nx }}]); + for (int ii = 0; ii < nlp_dims->N; ii++) + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "u", &utraj[ii*{{ dims.nu }}]); + + printf("\n--- xtraj ---\n"); + d_print_exp_tran_mat( {{ dims.nx }}, {{ dims.N }}+1, xtraj, {{ dims.nx }} ); + printf("\n--- utraj ---\n"); + d_print_exp_tran_mat( {{ dims.nu }}, {{ dims.N }}, utraj, {{ dims.nu }} ); + // ocp_nlp_out_print(nlp_solver->dims, nlp_out); + + printf("\nsolved ocp %d times, solution printed above\n\n", NTIMINGS); + + if (status == ACADOS_SUCCESS) + { + printf("{{ model.name }}_acados_solve(): SUCCESS!\n"); + } + else + { + printf("{{ model.name }}_acados_solve() failed with status %d.\n", status); + } + + // get solution + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "kkt_norm_inf", &kkt_norm_inf); + ocp_nlp_get(nlp_config, nlp_solver, "sqp_iter", &sqp_iter); + + {{ model.name }}_acados_print_stats(acados_ocp_capsule); + + printf("\nSolver info:\n"); + printf(" SQP iterations %2d\n minimum time for %d solve %f [ms]\n KKT %e\n", + sqp_iter, NTIMINGS, min_time*1000, kkt_norm_inf); + + // free solver + status = {{ model.name }}_acados_free(acados_ocp_capsule); + if (status) { + printf("{{ model.name }}_acados_free() returned status %d. \n", status); + } + // free solver capsule + status = {{ model.name }}_acados_free_capsule(acados_ocp_capsule); + if (status) { + printf("{{ model.name }}_acados_free_capsule() returned status %d. \n", status); + } + + return status; +} diff --git a/pyextra/acados_template/c_templates_tera/main_mex.in.c b/pyextra/acados_template/c_templates_tera/main_mex.in.c new file mode 100644 index 0000000000..8da5db29a0 --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/main_mex.in.c @@ -0,0 +1,184 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +// standard +#include +#include +// acados +#include "acados/utils/print.h" +#include "acados/utils/math.h" +#include "acados_c/ocp_nlp_interface.h" +#include "acados_solver_{{ model.name }}.h" +// mex +#include "mex.h" + +/* auxilary mex */ +// prints a matrix in column-major format (exponential notation) +void MEX_print_exp_mat(int m, int n, double *A, int lda) +{ + for (int i=0; iN; i++) + { + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "x", x_init); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "u", u0); + } + status = {{ model.name }}_acados_solve(); + ocp_nlp_get(nlp_config, nlp_solver, "time_tot", &elapsed_time); + min_time = MIN(elapsed_time, min_time); + } + + /* print solution and statistics */ + for (int ii = 0; ii <= nlp_dims->N; ii++) + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "x", &xtraj[ii*{{ dims.nx }}]); + for (int ii = 0; ii < nlp_dims->N; ii++) + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "u", &utraj[ii*{{ dims.nu }}]); + + mexPrintf("\n--- xtraj ---\n"); + MEX_print_exp_tran_mat( {{ dims.nx }}, {{ dims.N }}+1, xtraj, {{ dims.nx }} ); + mexPrintf("\n--- utraj ---\n"); + MEX_print_exp_tran_mat( {{ dims.nu }}, {{ dims.N }}, utraj, {{ dims.nu }} ); + + mexPrintf("\nsolved ocp %d times, solution printed above\n\n", NTIMINGS); + + if (status == ACADOS_SUCCESS) + mexPrintf("{{ model.name }}_acados_solve(): SUCCESS!\n"); + else + mexPrintf("{{ model.name }}_acados_solve() failed with status %d.\n", status); + + // get solution + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "kkt_norm_inf", &kkt_norm_inf); + ocp_nlp_get(nlp_config, nlp_solver, "sqp_iter", &sqp_iter); + + mexPrintf("\nSolver info:\n"); + mexPrintf(" SQP iterations %2d\n minimum time for 1 solve %f [ms]\n KKT %e\n", + sqp_iter, min_time*1000, kkt_norm_inf); + + // free solver + status = {{ model.name }}_acados_free(); + if (status) + { + mexPrintf("{{ model.name }}_acados_free() returned status %d.\n", status); + } + + return; +} diff --git a/pyextra/acados_template/c_templates_tera/main_sim.in.c b/pyextra/acados_template/c_templates_tera/main_sim.in.c new file mode 100644 index 0000000000..b62bf8f7fd --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/main_sim.in.c @@ -0,0 +1,130 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +// standard +#include +#include +// acados +#include "acados/utils/print.h" +#include "acados/utils/math.h" +#include "acados_c/sim_interface.h" +#include "acados_sim_solver_{{ model.name }}.h" + + +int main() +{ + int status = 0; + sim_solver_capsule *capsule = {{ model.name }}_acados_sim_solver_create_capsule(); + status = {{ model.name }}_acados_sim_create(capsule); + + if (status) + { + printf("acados_create() returned status %d. Exiting.\n", status); + exit(1); + } + + sim_config *acados_sim_config = {{ model.name }}_acados_get_sim_config(capsule); + sim_in *acados_sim_in = {{ model.name }}_acados_get_sim_in(capsule); + sim_out *acados_sim_out = {{ model.name }}_acados_get_sim_out(capsule); + void *acados_sim_dims = {{ model.name }}_acados_get_sim_dims(capsule); + + // initial condition + double x_current[{{ dims.nx }}]; + {%- for i in range(end=dims.nx) %} + x_current[{{ i }}] = 0.0; + {%- endfor %} + + {% if constraints.lbx_0 %} + {%- for i in range(end=dims.nbx_0) %} + x_current[{{ constraints.idxbx_0[i] }}] = {{ constraints.lbx_0[i] }}; + {%- endfor %} + {% if dims.nbx_0 != dims.nx %} + printf("main_sim: NOTE: initial state not fully defined via lbx_0, using 0.0 for indices that are not in idxbx_0."); + {%- endif %} + {% else %} + printf("main_sim: initial state not defined, should be in lbx_0, using zero vector."); + {%- endif %} + + + // initial value for control input + double u0[{{ dims.nu }}]; + {%- for i in range(end=dims.nu) %} + u0[{{ i }}] = 0.0; + {%- endfor %} + + {%- if dims.np > 0 %} + // set parameters + double p[{{ dims.np }}]; + {% for item in parameter_values %} + p[{{ loop.index0 }}] = {{ item }}; + {% endfor %} + + {{ model.name }}_acados_sim_update_params(capsule, p, {{ dims.np }}); + {% endif %}{# if np > 0 #} + + int n_sim_steps = 3; + // solve ocp in loop + for (int ii = 0; ii < n_sim_steps; ii++) + { + sim_in_set(acados_sim_config, acados_sim_dims, + acados_sim_in, "x", x_current); + status = {{ model.name }}_acados_sim_solve(capsule); + + if (status != ACADOS_SUCCESS) + { + printf("acados_solve() failed with status %d.\n", status); + } + + sim_out_get(acados_sim_config, acados_sim_dims, + acados_sim_out, "x", x_current); + + printf("\nx_current, %d\n", ii); + for (int jj = 0; jj < {{ dims.nx }}; jj++) + { + printf("%e\n", x_current[jj]); + } + } + + printf("\nPerformed %d simulation steps with acados integrator successfully.\n\n", n_sim_steps); + + // free solver + status = {{ model.name }}_acados_sim_free(capsule); + if (status) { + printf("{{ model.name }}_acados_sim_free() returned status %d. \n", status); + } + + {{ model.name }}_acados_sim_solver_free_capsule(capsule); + + return status; +} diff --git a/pyextra/acados_template/c_templates_tera/make_main_mex.in.m b/pyextra/acados_template/c_templates_tera/make_main_mex.in.m new file mode 100644 index 0000000000..9188686a0d --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/make_main_mex.in.m @@ -0,0 +1,105 @@ +% +% Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +% Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +% Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +% Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +% +% This file is part of acados. +% +% The 2-Clause BSD License +% +% Redistribution and use in source and binary forms, with or without +% modification, are permitted provided that the following conditions are met: +% +% 1. Redistributions of source code must retain the above copyright notice, +% this list of conditions and the following disclaimer. +% +% 2. Redistributions in binary form must reproduce the above copyright notice, +% this list of conditions and the following disclaimer in the documentation +% and/or other materials provided with the distribution. +% +% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +% POSSIBILITY OF SUCH DAMAGE.; +% + +function make_main_mex_{{ model.name }}() + + opts.output_dir = pwd; + + % get acados folder + acados_folder = getenv('ACADOS_INSTALL_DIR'); + + % set paths + acados_include = ['-I' fullfile(acados_folder, 'include')]; + template_lib_include = ['-l' 'acados_solver_{{ model.name }}']; + template_lib_path = ['-L' fullfile(pwd)]; + + acados_lib_path = ['-L' fullfile(acados_folder, 'lib')]; + external_include = ['-I', fullfile(acados_folder, 'external')]; + blasfeo_include = ['-I', fullfile(acados_folder, 'external', 'blasfeo', 'include')]; + hpipm_include = ['-I', fullfile(acados_folder, 'external', 'hpipm', 'include')]; + + mex_names = { ... + 'main_mex_{{ model.name }}' ... + }; + + mex_files = cell(length(mex_names), 1); + for k=1:length(mex_names) + mex_files{k} = fullfile([mex_names{k}, '.c']); + end + + %% octave C flags + if is_octave() + if ~exist(fullfile(opts.output_dir, 'cflags_octave.txt'), 'file') + diary(fullfile(opts.output_dir, 'cflags_octave.txt')); + diary on + mkoctfile -p CFLAGS + diary off + input_file = fopen(fullfile(opts.output_dir, 'cflags_octave.txt'), 'r'); + cflags_tmp = fscanf(input_file, '%[^\n]s'); + fclose(input_file); + if ~ismac() + cflags_tmp = [cflags_tmp, ' -std=c99 -fopenmp']; + else + cflags_tmp = [cflags_tmp, ' -std=c99']; + end + input_file = fopen(fullfile(opts.output_dir, 'cflags_octave.txt'), 'w'); + fprintf(input_file, '%s', cflags_tmp); + fclose(input_file); + end + % read cflags from file + input_file = fopen(fullfile(opts.output_dir, 'cflags_octave.txt'), 'r'); + cflags_tmp = fscanf(input_file, '%[^\n]s'); + fclose(input_file); + setenv('CFLAGS', cflags_tmp); + end + + %% compile mex + for ii=1:length(mex_files) + disp(['compiling ', mex_files{ii}]) + if is_octave() + % mkoctfile -p CFLAGS + mex(acados_include, template_lib_include, external_include, blasfeo_include, hpipm_include,... + acados_lib_path, template_lib_path, '-lacados', '-lhpipm', '-lblasfeo', mex_files{ii}) + else + if ismac() + FLAGS = 'CFLAGS=$CFLAGS -std=c99'; + else + FLAGS = 'CFLAGS=$CFLAGS -std=c99 -fopenmp'; + end + mex(FLAGS, acados_include, template_lib_include, external_include, blasfeo_include, hpipm_include,... + acados_lib_path, template_lib_path, '-lacados', '-lhpipm', '-lblasfeo', mex_files{ii}) + end + end + + +end \ No newline at end of file diff --git a/pyextra/acados_template/c_templates_tera/make_mex.in.m b/pyextra/acados_template/c_templates_tera/make_mex.in.m new file mode 100644 index 0000000000..cde30f6f41 --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/make_mex.in.m @@ -0,0 +1,110 @@ +% +% Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +% Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +% Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +% Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +% +% This file is part of acados. +% +% The 2-Clause BSD License +% +% Redistribution and use in source and binary forms, with or without +% modification, are permitted provided that the following conditions are met: +% +% 1. Redistributions of source code must retain the above copyright notice, +% this list of conditions and the following disclaimer. +% +% 2. Redistributions in binary form must reproduce the above copyright notice, +% this list of conditions and the following disclaimer in the documentation +% and/or other materials provided with the distribution. +% +% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +% POSSIBILITY OF SUCH DAMAGE.; +% + +function make_mex_{{ model.name }}() + + opts.output_dir = pwd; + + % get acados folder + acados_folder = getenv('ACADOS_INSTALL_DIR'); + + % set paths + acados_include = ['-I' fullfile(acados_folder, 'include')]; + template_lib_include = ['-l' 'acados_ocp_solver_{{ model.name }}']; + template_lib_path = ['-L' fullfile(pwd)]; + + acados_lib_path = ['-L' fullfile(acados_folder, 'lib')]; + external_include = ['-I', fullfile(acados_folder, 'external')]; + blasfeo_include = ['-I', fullfile(acados_folder, 'external', 'blasfeo', 'include')]; + hpipm_include = ['-I', fullfile(acados_folder, 'external', 'hpipm', 'include')]; + + mex_include = ['-I', fullfile(acados_folder, 'interfaces', 'acados_matlab_octave')]; + + mex_names = { ... + 'acados_mex_create_{{ model.name }}' ... + 'acados_mex_free_{{ model.name }}' ... + 'acados_mex_solve_{{ model.name }}' ... + 'acados_mex_set_{{ model.name }}' ... + }; + + mex_files = cell(length(mex_names), 1); + for k=1:length(mex_names) + mex_files{k} = fullfile([mex_names{k}, '.c']); + end + + %% octave C flags + if is_octave() + if ~exist(fullfile(opts.output_dir, 'cflags_octave.txt'), 'file') + diary(fullfile(opts.output_dir, 'cflags_octave.txt')); + diary on + mkoctfile -p CFLAGS + diary off + input_file = fopen(fullfile(opts.output_dir, 'cflags_octave.txt'), 'r'); + cflags_tmp = fscanf(input_file, '%[^\n]s'); + fclose(input_file); + if ~ismac() + cflags_tmp = [cflags_tmp, ' -std=c99 -fopenmp']; + else + cflags_tmp = [cflags_tmp, ' -std=c99']; + end + input_file = fopen(fullfile(opts.output_dir, 'cflags_octave.txt'), 'w'); + fprintf(input_file, '%s', cflags_tmp); + fclose(input_file); + end + % read cflags from file + input_file = fopen(fullfile(opts.output_dir, 'cflags_octave.txt'), 'r'); + cflags_tmp = fscanf(input_file, '%[^\n]s'); + fclose(input_file); + setenv('CFLAGS', cflags_tmp); + end + + %% compile mex + for ii=1:length(mex_files) + disp(['compiling ', mex_files{ii}]) + if is_octave() + % mkoctfile -p CFLAGS + mex(acados_include, template_lib_include, external_include, blasfeo_include, hpipm_include,... + acados_lib_path, template_lib_path, mex_include, '-lacados', '-lhpipm', '-lblasfeo', mex_files{ii}) + else + if ismac() + FLAGS = 'CFLAGS=$CFLAGS -std=c99'; + else + FLAGS = 'CFLAGS=$CFLAGS -std=c99 -fopenmp'; + end + mex(FLAGS, acados_include, template_lib_include, external_include, blasfeo_include, hpipm_include,... + acados_lib_path, template_lib_path, mex_include, '-lacados', '-lhpipm', '-lblasfeo', mex_files{ii}) + end + end + + +end \ No newline at end of file diff --git a/pyextra/acados_template/c_templates_tera/make_sfun.in.m b/pyextra/acados_template/c_templates_tera/make_sfun.in.m new file mode 100644 index 0000000000..3299ceb39d --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/make_sfun.in.m @@ -0,0 +1,320 @@ +% +% Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +% Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +% Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +% Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +% +% This file is part of acados. +% +% The 2-Clause BSD License +% +% Redistribution and use in source and binary forms, with or without +% modification, are permitted provided that the following conditions are met: +% +% 1. Redistributions of source code must retain the above copyright notice, +% this list of conditions and the following disclaimer. +% +% 2. Redistributions in binary form must reproduce the above copyright notice, +% this list of conditions and the following disclaimer in the documentation +% and/or other materials provided with the distribution. +% +% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +% POSSIBILITY OF SUCH DAMAGE.; +% + +SOURCES = { ... + {%- if solver_options.integrator_type == 'ERK' %} + '{{ model.name }}_model/{{ model.name }}_expl_ode_fun.c', ... + '{{ model.name }}_model/{{ model.name }}_expl_vde_forw.c',... + {%- if solver_options.hessian_approx == 'EXACT' %} + '{{ model.name }}_model/{{ model.name }}_expl_ode_hess.c',... + {%- endif %} + {%- elif solver_options.integrator_type == "IRK" %} + '{{ model.name }}_model/{{ model.name }}_impl_dae_fun.c', ... + '{{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_z.c', ... + '{{ model.name }}_model/{{ model.name }}_impl_dae_jac_x_xdot_u_z.c', ... + {%- if solver_options.hessian_approx == 'EXACT' %} + '{{ model.name }}_model/{{ model.name }}_impl_dae_hess.c',... + {%- endif %} + {%- elif solver_options.integrator_type == "GNSF" %} + '{{ 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',... + '{{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c',... + '{{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.c',... + {%- elif solver_options.integrator_type == "DISCRETE" %} + '{{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun.c',... + '{{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac.c',... + {%- if solver_options.hessian_approx == "EXACT" %} + '{{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac_hess.c',... + {%- endif %} + {%- endif %} + {%- if cost.cost_type_0 == "NONLINEAR_LS" %} + '{{ model.name }}_cost/{{ model.name }}_cost_y_0_fun.c',... + '{{ model.name }}_cost/{{ model.name }}_cost_y_0_fun_jac_ut_xt.c',... + '{{ model.name }}_cost/{{ model.name }}_cost_y_0_hess.c',... + {%- elif cost.cost_type_0 == "EXTERNAL" %} + '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun.c',... + '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac.c',... + '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac_hess.c',... + {%- endif %} + + {%- if cost.cost_type == "NONLINEAR_LS" %} + '{{ model.name }}_cost/{{ model.name }}_cost_y_fun.c',... + '{{ model.name }}_cost/{{ model.name }}_cost_y_fun_jac_ut_xt.c',... + '{{ model.name }}_cost/{{ model.name }}_cost_y_hess.c',... + {%- elif cost.cost_type == "EXTERNAL" %} + '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun.c',... + '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac.c',... + '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac_hess.c',... + {%- endif %} + {%- if cost.cost_type_e == "NONLINEAR_LS" %} + '{{ model.name }}_cost/{{ model.name }}_cost_y_e_fun.c',... + '{{ model.name }}_cost/{{ model.name }}_cost_y_e_fun_jac_ut_xt.c',... + '{{ model.name }}_cost/{{ model.name }}_cost_y_e_hess.c',... + {%- elif cost.cost_type_e == "EXTERNAL" %} + '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun.c',... + '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac.c',... + '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac_hess.c',... + {%- endif %} + {%- if constraints.constr_type == "BGH" and dims.nh > 0 %} + '{{ model.name }}_constraints/{{ model.name }}_constr_h_fun.c', ... + '{{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt_hess.c', ... + '{{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt.c', ... + {%- elif constraints.constr_type == "BGP" and dims.nphi > 0 %} + '{{ model.name }}_constraints/{{ model.name }}_phi_constraint.c', ... + {%- endif %} + {%- if constraints.constr_type_e == "BGH" and dims.nh_e > 0 %} + '{{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun.c', ... + '{{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess.c', ... + '{{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt.c', ... + {%- elif constraints.constr_type_e == "BGP" and dims.nphi_e > 0 %} + '{{ model.name }}_constraints/{{ model.name }}_phi_e_constraint.c', ... + {%- endif %} + 'acados_solver_sfunction_{{ model.name }}.c', ... + 'acados_solver_{{ model.name }}.c' + }; + +INC_PATH = '{{ acados_include_path }}'; + +INCS = {['-I', fullfile(INC_PATH, 'blasfeo', 'include')], ... + ['-I', fullfile(INC_PATH, 'hpipm', 'include')], ... + ['-I', fullfile(INC_PATH, 'acados')], ... + ['-I', fullfile(INC_PATH)]}; + +{% if solver_options.qp_solver is containing("QPOASES") %} +INCS{end+1} = ['-I', fullfile(INC_PATH, 'qpOASES_e')]; +{% endif %} + +CFLAGS = 'CFLAGS=$CFLAGS'; +LDFLAGS = 'LDFLAGS=$LDFLAGS'; +COMPFLAGS = 'COMPFLAGS=$COMPFLAGS'; +COMPDEFINES = 'COMPDEFINES=$COMPDEFINES'; + +{% if solver_options.qp_solver is containing("QPOASES") %} +CFLAGS = [ CFLAGS, ' -DACADOS_WITH_QPOASES ' ]; +COMPDEFINES = [ COMPDEFINES, ' -DACADOS_WITH_QPOASES ' ]; +{%- elif solver_options.qp_solver is containing("OSQP") %} +CFLAGS = [ CFLAGS, ' -DACADOS_WITH_OSQP ' ]; +COMPDEFINES = [ COMPDEFINES, ' -DACADOS_WITH_OSQP ' ]; +{%- elif solver_options.qp_solver is containing("QPDUNES") %} +CFLAGS = [ CFLAGS, ' -DACADOS_WITH_QPDUNES ' ]; +COMPDEFINES = [ COMPDEFINES, ' -DACADOS_WITH_QPDUNES ' ]; +{%- elif solver_options.qp_solver is containing("HPMPC") %} +CFLAGS = [ CFLAGS, ' -DACADOS_WITH_HPMPC ' ]; +COMPDEFINES = [ COMPDEFINES, ' -DACADOS_WITH_HPMPC ' ]; +{% endif %} + +LIB_PATH = ['-L', fullfile('{{ acados_lib_path }}')]; + +LIBS = {'-lacados', '-lhpipm', '-lblasfeo'}; + +% acados linking libraries and flags +{%- if acados_link_libs and os and os == "pc" %} +LDFLAGS = [LDFLAGS ' {{ acados_link_libs.openmp }}']; +COMPFLAGS = [COMPFLAGS ' {{ acados_link_libs.openmp }}']; +LIBS{end+1} = '{{ acados_link_libs.qpoases }}'; +LIBS{end+1} = '{{ acados_link_libs.hpmpc }}'; +LIBS{end+1} = '{{ acados_link_libs.osqp }}'; +{%- else %} + {% if solver_options.qp_solver is containing("QPOASES") %} +LIBS{end+1} = '-lqpOASES_e'; + {% endif %} +{%- endif %} + +mex('-v', '-O', CFLAGS, LDFLAGS, COMPFLAGS, COMPDEFINES, INCS{:}, ... + LIB_PATH, LIBS{:}, SOURCES{:}, ... + '-output', 'acados_solver_sfunction_{{ model.name }}' ); + +fprintf( [ '\n\nSuccessfully created sfunction:\nacados_solver_sfunction_{{ model.name }}', '.', ... + eval('mexext')] ); + + +%% print note on usage of s-function +fprintf('\n\nNote: Usage of Sfunction is as follows:\n') +input_note = 'Inputs are:\n'; +i_in = 1; + + +{%- if dims.nbx_0 > 0 and simulink_opts.inputs.lbx_0 -%} {#- lbx_0 #} +input_note = strcat(input_note, num2str(i_in), ') lbx_0 - lower bound on x for stage 0,',... + ' size [{{ dims.nbx_0 }}]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if dims.nbx_0 > 0 and simulink_opts.inputs.ubx_0 -%} {#- ubx_0 #} +input_note = strcat(input_note, num2str(i_in), ') ubx_0 - upper bound on x for stage 0,',... + ' size [{{ dims.nbx_0 }}]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if dims.np > 0 and simulink_opts.inputs.parameter_traj -%} {#- parameter_traj #} +input_note = strcat(input_note, num2str(i_in), ') parameters - concatenated for all shooting nodes 0 to N+1,',... + ' size [{{ (dims.N+1)*dims.np }}]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if dims.ny_0 > 0 and simulink_opts.inputs.y_ref_0 %} +input_note = strcat(input_note, num2str(i_in), ') y_ref_0, size [{{ dims.ny_0 }}]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if dims.ny > 0 and dims.N > 1 and simulink_opts.inputs.y_ref %} +input_note = strcat(input_note, num2str(i_in), ') y_ref - concatenated for shooting nodes 1 to N-1,',... + ' size [{{ (dims.N-1) * dims.ny }}]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if dims.ny_e > 0 and dims.N > 0 and simulink_opts.inputs.y_ref_e %} +input_note = strcat(input_note, num2str(i_in), ') y_ref_e, size [{{ dims.ny_e }}]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.lbx -%} {#- lbx #} +input_note = strcat(input_note, num2str(i_in), ') lbx for shooting nodes 1 to N-1, size [{{ (dims.N-1) * dims.nbx }}]\n '); +i_in = i_in + 1; +{%- endif %} +{%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.ubx -%} {#- ubx #} +input_note = strcat(input_note, num2str(i_in), ') ubx for shooting nodes 1 to N-1, size [{{ (dims.N-1) * dims.nbx }}]\n '); +i_in = i_in + 1; +{%- endif %} + + +{%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.lbx_e -%} {#- lbx_e #} +input_note = strcat(input_note, num2str(i_in), ') lbx_e (lbx at shooting node N), size [{{ dims.nbx_e }}]\n '); +i_in = i_in + 1; +{%- endif %} +{%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.ubx_e -%} {#- ubx_e #} +input_note = strcat(input_note, num2str(i_in), ') ubx_e (ubx at shooting node N), size [{{ dims.nbx_e }}]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.lbu -%} {#- lbu #} +input_note = strcat(input_note, num2str(i_in), ') lbu for shooting nodes 0 to N-1, size [{{ dims.N*dims.nbu }}]\n '); +i_in = i_in + 1; +{%- endif -%} +{%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.ubu -%} {#- ubu #} +input_note = strcat(input_note, num2str(i_in), ') ubu for shooting nodes 0 to N-1, size [{{ dims.N*dims.nbu }}]\n '); +i_in = i_in + 1; +{%- endif -%} + +{%- if dims.ng > 0 and simulink_opts.inputs.lg -%} {#- lg #} +input_note = strcat(input_note, num2str(i_in), ') lg, size [{{ dims.ng }}]\n '); +i_in = i_in + 1; +{%- endif %} +{%- if dims.ng > 0 and simulink_opts.inputs.ug -%} {#- ug #} +input_note = strcat(input_note, num2str(i_in), ') ug, size [{{ dims.ng }}]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if dims.nh > 0 and simulink_opts.inputs.lh -%} {#- lh #} +input_note = strcat(input_note, num2str(i_in), ') lh, size [{{ dims.nh }}]\n '); +i_in = i_in + 1; +{%- endif %} +{%- if dims.nh > 0 and simulink_opts.inputs.uh -%} {#- uh #} +input_note = strcat(input_note, num2str(i_in), ') uh, size [{{ dims.nh }}]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if dims.ny_0 > 0 and simulink_opts.inputs.cost_W_0 %} {#- cost_W_0 #} +input_note = strcat(input_note, num2str(i_in), ') cost_W_0 in column-major format, size [{{ dims.ny_0 * dims.ny_0 }}]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if dims.ny > 0 and simulink_opts.inputs.cost_W %} {#- cost_W #} +input_note = strcat(input_note, num2str(i_in), ') cost_W in column-major format, that is set for all intermediate shooting nodes: 1 to N-1, size [{{ dims.ny * dims.ny }}]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if dims.ny_e > 0 and simulink_opts.inputs.cost_W_e %} {#- cost_W_e #} +input_note = strcat(input_note, num2str(i_in), ') cost_W_e in column-major format, size [{{ dims.ny_e * dims.ny_e }}]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if simulink_opts.inputs.x_init %} {#- x_init #} +input_note = strcat(input_note, num2str(i_in), ') initialization of x for all shooting nodes, size [{{ dims.nx * (dims.N+1) }}]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if simulink_opts.inputs.u_init %} {#- u_init #} +input_note = strcat(input_note, num2str(i_in), ') initialization of u for shooting nodes 0 to N-1, size [{{ dims.nu * (dims.N) }}]\n '); +i_in = i_in + 1; +{%- endif %} + +fprintf(input_note) + +disp(' ') + +output_note = 'Outputs are:\n'; +i_out = 0; + +{%- if dims.nu > 0 and simulink_opts.outputs.u0 == 1 %} +i_out = i_out + 1; +output_note = strcat(output_note, num2str(i_out), ') u0, control input at node 0, size [{{ dims.nu }}]\n '); +{%- endif %} + +{%- if simulink_opts.outputs.utraj == 1 %} +i_out = i_out + 1; +output_note = strcat(output_note, num2str(i_out), ') utraj, control input concatenated for nodes 0 to N-1, size [{{ dims.nu * dims.N }}]\n '); +{%- endif %} + +{%- if simulink_opts.outputs.xtraj == 1 %} +i_out = i_out + 1; +output_note = strcat(output_note, num2str(i_out), ') xtraj, state concatenated for nodes 0 to N, size [{{ dims.nx * (dims.N + 1) }}]\n '); +{%- endif %} + +{%- if simulink_opts.outputs.solver_status == 1 %} +i_out = i_out + 1; +output_note = strcat(output_note, num2str(i_out), ') acados solver status (0 = SUCCESS)\n '); +{%- endif %} + +{%- if simulink_opts.outputs.KKT_residual == 1 %} +i_out = i_out + 1; +output_note = strcat(output_note, num2str(i_out), ') KKT residual\n '); +{%- endif %} + +{%- if dims.N > 0 and simulink_opts.outputs.x1 == 1 %} +i_out = i_out + 1; +output_note = strcat(output_note, num2str(i_out), ') x1, state at node 1\n '); +{%- endif %} + +{%- if simulink_opts.outputs.CPU_time == 1 %} +i_out = i_out + 1; +output_note = strcat(output_note, num2str(i_out), ') CPU time\n '); +{%- endif %} + +{%- if simulink_opts.outputs.sqp_iter == 1 %} +i_out = i_out + 1; +output_note = strcat(output_note, num2str(i_out), ') SQP iterations\n '); +{%- endif %} + +fprintf(output_note) diff --git a/pyextra/acados_template/c_templates_tera/make_sfun_sim.in.m b/pyextra/acados_template/c_templates_tera/make_sfun_sim.in.m new file mode 100644 index 0000000000..1c5cf0b123 --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/make_sfun_sim.in.m @@ -0,0 +1,99 @@ +% +% Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +% Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +% Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +% Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +% +% This file is part of acados. +% +% The 2-Clause BSD License +% +% Redistribution and use in source and binary forms, with or without +% modification, are permitted provided that the following conditions are met: +% +% 1. Redistributions of source code must retain the above copyright notice, +% this list of conditions and the following disclaimer. +% +% 2. Redistributions in binary form must reproduce the above copyright notice, +% this list of conditions and the following disclaimer in the documentation +% and/or other materials provided with the distribution. +% +% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +% POSSIBILITY OF SUCH DAMAGE.; +% + +SOURCES = [ 'acados_sim_solver_sfunction_{{ model.name }}.c ', ... + 'acados_sim_solver_{{ model.name }}.c ', ... + {%- if solver_options.integrator_type == 'ERK' %} + '{{ model.name }}_model/{{ model.name }}_expl_ode_fun.c ', ... + '{{ model.name }}_model/{{ model.name }}_expl_vde_forw.c ',... + {%- if solver_options.hessian_approx == 'EXACT' %} + '{{ model.name }}_model/{{ model.name }}_expl_ode_hess.c ',... + {%- endif %} + {%- elif solver_options.integrator_type == "IRK" %} + '{{ model.name }}_model/{{ model.name }}_impl_dae_fun.c ', ... + '{{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_z.c ', ... + '{{ model.name }}_model/{{ model.name }}_impl_dae_jac_x_xdot_u_z.c ', ... + {%- if solver_options.hessian_approx == 'EXACT' %} + '{{ model.name }}_model/{{ model.name }}_impl_dae_hess.c ',... + {%- endif %} + {%- elif solver_options.integrator_type == "GNSF" %} + '{{ 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 ' + '{{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c ' + '{{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.c ' + {%- endif %} + ]; + +INC_PATH = '{{ acados_include_path }}'; + +INCS = [ ' -I', fullfile(INC_PATH, 'blasfeo', 'include'), ... + ' -I', fullfile(INC_PATH, 'hpipm', 'include'), ... + ' -I', INC_PATH, ' -I', fullfile(INC_PATH, 'acados'), ' ']; + +CFLAGS = ' -O'; + +LIB_PATH = '{{ acados_lib_path }}'; + +LIBS = '-lacados -lblasfeo -lhpipm'; + +eval( [ 'mex -v -output acados_sim_solver_sfunction_{{ model.name }} ', ... + CFLAGS, INCS, ' ', SOURCES, ' -L', LIB_PATH, ' ', LIBS ]); + +fprintf( [ '\n\nSuccessfully created sfunction:\nacados_sim_solver_sfunction_{{ model.name }}', '.', ... + eval('mexext')] ); + + +%% print note on usage of s-function +fprintf('\n\nNote: Usage of Sfunction is as follows:\n') +input_note = 'Inputs are:\n1) x0, initial state, size [{{ dims.nx }}]\n '; +i_in = 2; +{%- if dims.nu > 0 %} +input_note = strcat(input_note, num2str(i_in), ') u, size [{{ dims.nu }}]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if dims.np > 0 %} +input_note = strcat(input_note, num2str(i_in), ') parameters, size [{{ dims.np }}]\n '); +i_in = i_in + 1; +{%- endif %} + + +fprintf(input_note) + +disp(' ') + +output_note = strcat('Outputs are:\n', ... + '1) x1 - simulated state, size [{{ dims.nx }}]\n'); + +fprintf(output_note) diff --git a/pyextra/acados_template/c_templates_tera/mex_solver.in.m b/pyextra/acados_template/c_templates_tera/mex_solver.in.m new file mode 100644 index 0000000000..728741a46e --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/mex_solver.in.m @@ -0,0 +1,166 @@ +% +% Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +% Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +% Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +% Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +% +% This file is part of acados. +% +% The 2-Clause BSD License +% +% Redistribution and use in source and binary forms, with or without +% modification, are permitted provided that the following conditions are met: +% +% 1. Redistributions of source code must retain the above copyright notice, +% this list of conditions and the following disclaimer. +% +% 2. Redistributions in binary form must reproduce the above copyright notice, +% this list of conditions and the following disclaimer in the documentation +% and/or other materials provided with the distribution. +% +% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +% POSSIBILITY OF SUCH DAMAGE.; +% + +classdef {{ model.name }}_mex_solver < handle + + properties + C_ocp + C_ocp_ext_fun + cost_ext_fun_type + cost_ext_fun_type_e + end % properties + + + + methods + + % constructor + function obj = {{ model.name }}_mex_solver() + make_mex_{{ model.name }}(); + [obj.C_ocp, obj.C_ocp_ext_fun] = acados_mex_create_{{ model.name }}(); + % to have path to destructor when changing directory + addpath('.') + obj.cost_ext_fun_type = '{{ cost.cost_ext_fun_type }}'; + obj.cost_ext_fun_type_e = '{{ cost.cost_ext_fun_type_e }}'; + end + + % destructor + function delete(obj) + if ~isempty(obj.C_ocp) + acados_mex_free_{{ model.name }}(obj.C_ocp); + end + end + + % solve + function solve(obj) + acados_mex_solve_{{ model.name }}(obj.C_ocp); + end + + % set -- borrowed from MEX interface + function set(varargin) + obj = varargin{1}; + field = varargin{2}; + value = varargin{3}; + if ~isa(field, 'char') + error('field must be a char vector, use '' '''); + end + if nargin==3 + acados_mex_set_{{ model.name }}(obj.cost_ext_fun_type, obj.cost_ext_fun_type_e, obj.C_ocp, obj.C_ocp_ext_fun, field, value); + elseif nargin==4 + stage = varargin{4}; + acados_mex_set_{{ model.name }}(obj.cost_ext_fun_type, obj.cost_ext_fun_type_e, obj.C_ocp, obj.C_ocp_ext_fun, field, value, stage); + else + disp('acados_ocp.set: wrong number of input arguments (2 or 3 allowed)'); + end + end + + function value = get_cost(obj) + value = ocp_get_cost(obj.C_ocp); + end + + % get -- borrowed from MEX interface + function value = get(varargin) + % usage: + % obj.get(field, value, [stage]) + obj = varargin{1}; + field = varargin{2}; + if any(strfind('sens', field)) + error('field sens* (sensitivities of optimal solution) not yet supported for templated MEX.') + end + if ~isa(field, 'char') + error('field must be a char vector, use '' '''); + end + + if nargin==2 + value = ocp_get(obj.C_ocp, field); + elseif nargin==3 + stage = varargin{3}; + value = ocp_get(obj.C_ocp, field, stage); + else + disp('acados_ocp.get: wrong number of input arguments (1 or 2 allowed)'); + end + end + + + % print + function print(varargin) + if nargin < 2 + field = 'stat'; + else + field = varargin{2}; + end + + obj = varargin{1}; + + if strcmp(field, 'stat') + stat = obj.get('stat'); + {%- if solver_options.nlp_solver_type == "SQP" %} + fprintf('\niter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter'); + if size(stat,2)>7 + fprintf('\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp'); + end + fprintf('\n'); + for jj=1:size(stat,1) + fprintf('%d\t%e\t%e\t%e\t%e\t%d\t%d', stat(jj,1), stat(jj,2), stat(jj,3), stat(jj,4), stat(jj,5), stat(jj,6), stat(jj,7)); + if size(stat,2)>7 + fprintf('\t%e\t%e\t%e\t%e', stat(jj,8), stat(jj,9), stat(jj,10), stat(jj,11)); + end + fprintf('\n'); + end + fprintf('\n'); + {%- else %} + fprintf('\niter\tqp_status\tqp_iter'); + if size(stat,2)>3 + fprintf('\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp'); + end + fprintf('\n'); + for jj=1:size(stat,1) + fprintf('%d\t%d\t\t%d', stat(jj,1), stat(jj,2), stat(jj,3)); + if size(stat,2)>3 + fprintf('\t%e\t%e\t%e\t%e', stat(jj,4), stat(jj,5), stat(jj,6), stat(jj,7)); + end + fprintf('\n'); + end + {% endif %} + + else + fprintf('unsupported field in function print of acados_ocp.print, got %s', field); + keyboard + end + + end + + end % methods + +end % class + diff --git a/pyextra/acados_template/c_templates_tera/model.in.h b/pyextra/acados_template/c_templates_tera/model.in.h new file mode 100644 index 0000000000..fd8871c8b8 --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/model.in.h @@ -0,0 +1,209 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + +#ifndef {{ model.name }}_MODEL +#define {{ model.name }}_MODEL + +#ifdef __cplusplus +extern "C" { +#endif + +{%- if solver_options.hessian_approx %} + {%- set hessian_approx = solver_options.hessian_approx %} +{%- elif solver_options.sens_hess %} + {%- set hessian_approx = "EXACT" %} +{%- else %} + {%- set hessian_approx = "GAUSS_NEWTON" %} +{%- endif %} + +{% if solver_options.integrator_type == "IRK" %} +// implicit ODE +int {{ model.name }}_impl_dae_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_impl_dae_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_impl_dae_fun_sparsity_in(int); +const int *{{ model.name }}_impl_dae_fun_sparsity_out(int); +int {{ model.name }}_impl_dae_fun_n_in(); +int {{ model.name }}_impl_dae_fun_n_out(); + +// implicit ODE +int {{ model.name }}_impl_dae_fun_jac_x_xdot_z(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_impl_dae_fun_jac_x_xdot_z_work(int *, int *, int *, int *); +const int *{{ model.name }}_impl_dae_fun_jac_x_xdot_z_sparsity_in(int); +const int *{{ model.name }}_impl_dae_fun_jac_x_xdot_z_sparsity_out(int); +int {{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_in(); +int {{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_out(); + +// implicit ODE +int {{ model.name }}_impl_dae_jac_x_xdot_u_z(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_impl_dae_jac_x_xdot_u_z_work(int *, int *, int *, int *); +const int *{{ model.name }}_impl_dae_jac_x_xdot_u_z_sparsity_in(int); +const int *{{ model.name }}_impl_dae_jac_x_xdot_u_z_sparsity_out(int); +int {{ model.name }}_impl_dae_jac_x_xdot_u_z_n_in(); +int {{ model.name }}_impl_dae_jac_x_xdot_u_z_n_out(); + +// // implicit ODE - for lifted_irk +// int {{ model.name }}_impl_dae_fun_jac_x_xdot_u(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +// int {{ model.name }}_impl_dae_fun_jac_x_xdot_u_work(int *, int *, int *, int *); +// const int *{{ model.name }}_impl_dae_fun_jac_x_xdot_u_sparsity_in(int); +// const int *{{ model.name }}_impl_dae_fun_jac_x_xdot_u_sparsity_out(int); +// int {{ model.name }}_impl_dae_fun_jac_x_xdot_u_n_in(); +// int {{ model.name }}_impl_dae_fun_jac_x_xdot_u_n_out(); + +{%- if hessian_approx == "EXACT" %} +int {{ model.name }}_impl_dae_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_impl_dae_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_impl_dae_hess_sparsity_in(int); +const int *{{ model.name }}_impl_dae_hess_sparsity_out(int); +int {{ model.name }}_impl_dae_hess_n_in(); +int {{ model.name }}_impl_dae_hess_n_out(); +{%- endif %} + +{% elif solver_options.integrator_type == "GNSF" %} +/* GNSF Functions */ +// used to import model matrices +int {{ model.name }}_gnsf_get_matrices_fun(const double** arg, double** res, int* iw, double* w, void *mem); +int {{ model.name }}_gnsf_get_matrices_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_gnsf_get_matrices_fun_sparsity_in(int); +const int *{{ model.name }}_gnsf_get_matrices_fun_sparsity_out(int); +int {{ model.name }}_gnsf_get_matrices_fun_n_in(); +int {{ model.name }}_gnsf_get_matrices_fun_n_out(); + +// phi_fun +int {{ model.name }}_gnsf_phi_fun(const double** arg, double** res, int* iw, double* w, void *mem); +int {{ model.name }}_gnsf_phi_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_gnsf_phi_fun_sparsity_in(int); +const int *{{ model.name }}_gnsf_phi_fun_sparsity_out(int); +int {{ model.name }}_gnsf_phi_fun_n_in(); +int {{ model.name }}_gnsf_phi_fun_n_out(); + +// phi_fun_jac_y +int {{ model.name }}_gnsf_phi_fun_jac_y(const double** arg, double** res, int* iw, double* w, void *mem); +int {{ model.name }}_gnsf_phi_fun_jac_y_work(int *, int *, int *, int *); +const int *{{ model.name }}_gnsf_phi_fun_jac_y_sparsity_in(int); +const int *{{ model.name }}_gnsf_phi_fun_jac_y_sparsity_out(int); +int {{ model.name }}_gnsf_phi_fun_jac_y_n_in(); +int {{ model.name }}_gnsf_phi_fun_jac_y_n_out(); + +// phi_jac_y_uhat +int {{ model.name }}_gnsf_phi_jac_y_uhat(const double** arg, double** res, int* iw, double* w, void *mem); +int {{ model.name }}_gnsf_phi_jac_y_uhat_work(int *, int *, int *, int *); +const int *{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_in(int); +const int *{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_out(int); +int {{ model.name }}_gnsf_phi_jac_y_uhat_n_in(); +int {{ model.name }}_gnsf_phi_jac_y_uhat_n_out(); + +// f_lo_fun_jac_x1k1uz +int {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz(const double** arg, double** res, int* iw, double* w, void *mem); +int {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_work(int *, int *, int *, int *); +const int *{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_in(int); +const int *{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_out(int); +int {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_in(); +int {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_out(); + +{% elif solver_options.integrator_type == "ERK" %} +/* explicit ODE */ + +// explicit ODE +int {{ model.name }}_expl_ode_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_expl_ode_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_expl_ode_fun_sparsity_in(int); +const int *{{ model.name }}_expl_ode_fun_sparsity_out(int); +int {{ model.name }}_expl_ode_fun_n_in(); +int {{ model.name }}_expl_ode_fun_n_out(); + +// explicit forward VDE +int {{ model.name }}_expl_vde_forw(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_expl_vde_forw_work(int *, int *, int *, int *); +const int *{{ model.name }}_expl_vde_forw_sparsity_in(int); +const int *{{ model.name }}_expl_vde_forw_sparsity_out(int); +int {{ model.name }}_expl_vde_forw_n_in(); +int {{ model.name }}_expl_vde_forw_n_out(); + +// explicit adjoint VDE +int {{ model.name }}_expl_vde_adj(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_expl_vde_adj_work(int *, int *, int *, int *); +const int *{{ model.name }}_expl_vde_adj_sparsity_in(int); +const int *{{ model.name }}_expl_vde_adj_sparsity_out(int); +int {{ model.name }}_expl_vde_adj_n_in(); +int {{ model.name }}_expl_vde_adj_n_out(); + +{%- if hessian_approx == "EXACT" %} +int {{ model.name }}_expl_ode_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_expl_ode_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_expl_ode_hess_sparsity_in(int); +const int *{{ model.name }}_expl_ode_hess_sparsity_out(int); +int {{ model.name }}_expl_ode_hess_n_in(); +int {{ model.name }}_expl_ode_hess_n_out(); +{%- endif %} + +{% elif solver_options.integrator_type == "DISCRETE" %} + +{% if model.dyn_ext_fun_type == "casadi" %} +int {{ model.name }}_dyn_disc_phi_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_dyn_disc_phi_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_dyn_disc_phi_fun_sparsity_in(int); +const int *{{ model.name }}_dyn_disc_phi_fun_sparsity_out(int); +int {{ model.name }}_dyn_disc_phi_fun_n_in(); +int {{ model.name }}_dyn_disc_phi_fun_n_out(); + +int {{ model.name }}_dyn_disc_phi_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_dyn_disc_phi_fun_jac_work(int *, int *, int *, int *); +const int *{{ model.name }}_dyn_disc_phi_fun_jac_sparsity_in(int); +const int *{{ model.name }}_dyn_disc_phi_fun_jac_sparsity_out(int); +int {{ model.name }}_dyn_disc_phi_fun_jac_n_in(); +int {{ model.name }}_dyn_disc_phi_fun_jac_n_out(); + +{%- if hessian_approx == "EXACT" %} +int {{ model.name }}_dyn_disc_phi_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_dyn_disc_phi_fun_jac_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_dyn_disc_phi_fun_jac_hess_sparsity_in(int); +const int *{{ model.name }}_dyn_disc_phi_fun_jac_hess_sparsity_out(int); +int {{ model.name }}_dyn_disc_phi_fun_jac_hess_n_in(); +int {{ model.name }}_dyn_disc_phi_fun_jac_hess_n_out(); +{%- endif %} +{% else %} + {%- if hessian_approx == "EXACT" %} +int {{ model.dyn_disc_fun_jac_hess }}(void **, void **, void *); + {% endif %} +int {{ model.dyn_disc_fun_jac }}(void **, void **, void *); +int {{ model.dyn_disc_fun }}(void **, void **, void *); +{% endif %} + + +{% endif %} + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // {{ model.name }}_MODEL diff --git a/pyextra/acados_template/c_templates_tera/phi_constraint.in.h b/pyextra/acados_template/c_templates_tera/phi_constraint.in.h new file mode 100644 index 0000000000..e1c15938b0 --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/phi_constraint.in.h @@ -0,0 +1,55 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + +#ifndef {{ model.name }}_PHI_CONSTRAINT +#define {{ model.name }}_PHI_CONSTRAINT + +#ifdef __cplusplus +extern "C" { +#endif + +{% if dims.nphi > 0 %} +// implicit ODE +int {{ model.name }}_phi_constraint(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_phi_constraint_work(int *, int *, int *, int *); +const int *{{ model.name }}_phi_constraint_sparsity_in(int); +const int *{{ model.name }}_phi_constraint_sparsity_out(int); +int {{ model.name }}_phi_constraint_n_in(); +int {{ model.name }}_phi_constraint_n_out(); +{% endif %} + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // {{ model.name }}_PHI_CONSTRAINT diff --git a/pyextra/acados_template/c_templates_tera/phi_e_constraint.in.h b/pyextra/acados_template/c_templates_tera/phi_e_constraint.in.h new file mode 100644 index 0000000000..a8fc09475d --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/phi_e_constraint.in.h @@ -0,0 +1,21 @@ +#ifndef {{ model.name }}_PHI_E_CONSTRAINT +#define {{ model.name }}_PHI_E_CONSTRAINT + +#ifdef __cplusplus +extern "C" { +#endif + +{% if dims.nphi_e > 0 %} +int {{ model.name }}_phi_e_constraint(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_phi_e_constraint_work(int *, int *, int *, int *); +const int *{{ model.name }}_phi_e_constraint_sparsity_in(int); +const int *{{ model.name }}_phi_e_constraint_sparsity_out(int); +int {{ model.name }}_phi_e_constraint_n_in(); +int {{ model.name }}_phi_e_constraint_n_out(); +{% endif %} + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // {{ model.name }}_PHI_E_CONSTRAINT diff --git a/pyextra/acados_template/generate_c_code_constraint.py b/pyextra/acados_template/generate_c_code_constraint.py new file mode 100644 index 0000000000..93e919c56a --- /dev/null +++ b/pyextra/acados_template/generate_c_code_constraint.py @@ -0,0 +1,180 @@ +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + +import os +from casadi import * +from .utils import ALLOWED_CASADI_VERSIONS, is_empty, casadi_length, casadi_version_warning + +def generate_c_code_constraint( model, con_name, is_terminal, opts ): + + casadi_version = CasadiMeta.version() + casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double') + + if casadi_version not in (ALLOWED_CASADI_VERSIONS): + casadi_version_warning(casadi_version) + + # load constraint variables and expression + x = model.x + p = model.p + + if isinstance(x, casadi.MX): + symbol = MX.sym + else: + symbol = SX.sym + + if is_terminal: + con_h_expr = model.con_h_expr_e + con_phi_expr = model.con_phi_expr_e + # create dummy u, z + u = symbol('u', 0, 0) + z = symbol('z', 0, 0) + else: + con_h_expr = model.con_h_expr + con_phi_expr = model.con_phi_expr + u = model.u + z = model.z + + if (not is_empty(con_h_expr)) and (not is_empty(con_phi_expr)): + raise Exception("acados: you can either have constraint_h, or constraint_phi, not both.") + + if not (is_empty(con_h_expr) and is_empty(con_phi_expr)): + if is_empty(con_h_expr): + constr_type = 'BGP' + else: + constr_type = 'BGH' + + if is_empty(p): + p = symbol('p', 0, 0) + + if is_empty(z): + z = symbol('z', 0, 0) + + if not (is_empty(con_h_expr)) and opts['generate_hess']: + # multipliers for hessian + nh = casadi_length(con_h_expr) + lam_h = symbol('lam_h', nh, 1) + + # set up & change directory + code_export_dir = opts["code_export_directory"] + if not os.path.exists(code_export_dir): + os.makedirs(code_export_dir) + + cwd = os.getcwd() + os.chdir(code_export_dir) + gen_dir = con_name + '_constraints' + if not os.path.exists(gen_dir): + os.mkdir(gen_dir) + gen_dir_location = './' + gen_dir + os.chdir(gen_dir_location) + + # export casadi functions + if constr_type == 'BGH': + if is_terminal: + fun_name = con_name + '_constr_h_e_fun_jac_uxt_zt' + else: + fun_name = con_name + '_constr_h_fun_jac_uxt_zt' + + jac_ux_t = transpose(jacobian(con_h_expr, vertcat(u,x))) + jac_z_t = jacobian(con_h_expr, z) + constraint_fun_jac_tran = Function(fun_name, [x, u, z, p], \ + [con_h_expr, jac_ux_t, jac_z_t]) + + constraint_fun_jac_tran.generate(fun_name, casadi_opts) + if opts['generate_hess']: + + if is_terminal: + fun_name = con_name + '_constr_h_e_fun_jac_uxt_zt_hess' + else: + fun_name = con_name + '_constr_h_fun_jac_uxt_zt_hess' + + # adjoint + adj_ux = jtimes(con_h_expr, vertcat(u, x), lam_h, True) + # hessian + hess_ux = jacobian(adj_ux, vertcat(u, x)) + + adj_z = jtimes(con_h_expr, z, lam_h, True) + hess_z = jacobian(adj_z, z) + + # set up functions + constraint_fun_jac_tran_hess = \ + Function(fun_name, [x, u, lam_h, z, p], \ + [con_h_expr, jac_ux_t, hess_ux, jac_z_t, hess_z]) + + # generate C code + constraint_fun_jac_tran_hess.generate(fun_name, casadi_opts) + + if is_terminal: + fun_name = con_name + '_constr_h_e_fun' + else: + fun_name = con_name + '_constr_h_fun' + h_fun = Function(fun_name, [x, u, z, p], [con_h_expr]) + h_fun.generate(fun_name, casadi_opts) + + else: # BGP constraint + if is_terminal: + fun_name = con_name + '_phi_e_constraint' + r = model.con_r_in_phi_e + con_r_expr = model.con_r_expr_e + else: + fun_name = con_name + '_phi_constraint' + r = model.con_r_in_phi + con_r_expr = model.con_r_expr + + nphi = casadi_length(con_phi_expr) + con_phi_expr_x_u_z = substitute(con_phi_expr, r, con_r_expr) + phi_jac_u = jacobian(con_phi_expr_x_u_z, u) + phi_jac_x = jacobian(con_phi_expr_x_u_z, x) + phi_jac_z = jacobian(con_phi_expr_x_u_z, z) + + hess = hessian(con_phi_expr[0], r)[0] + for i in range(1, nphi): + hess = vertcat(hess, hessian(con_phi_expr[i], r)[0]) + + r_jac_u = jacobian(con_r_expr, u) + r_jac_x = jacobian(con_r_expr, x) + + constraint_phi = \ + Function(fun_name, [x, u, z, p], \ + [con_phi_expr_x_u_z, \ + vertcat(transpose(phi_jac_u), \ + transpose(phi_jac_x)), \ + transpose(phi_jac_z), \ + hess, vertcat(transpose(r_jac_u), \ + transpose(r_jac_x))]) + + constraint_phi.generate(fun_name, casadi_opts) + + # change directory back + os.chdir(cwd) + + return diff --git a/pyextra/acados_template/generate_c_code_discrete_dynamics.py b/pyextra/acados_template/generate_c_code_discrete_dynamics.py new file mode 100644 index 0000000000..531b5ed9da --- /dev/null +++ b/pyextra/acados_template/generate_c_code_discrete_dynamics.py @@ -0,0 +1,99 @@ +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES LOSS OF USE, DATA, OR PROFITS OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# + +import os +from casadi import * +from .utils import ALLOWED_CASADI_VERSIONS, casadi_length, casadi_version_warning + +def generate_c_code_discrete_dynamics( model, opts ): + + casadi_version = CasadiMeta.version() + casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double') + + if casadi_version not in (ALLOWED_CASADI_VERSIONS): + casadi_version_warning(casadi_version) + + # load model + x = model.x + u = model.u + p = model.p + phi = model.disc_dyn_expr + model_name = model.name + nx = x.size()[0] + + + if isinstance(phi, casadi.MX): + symbol = MX.sym + elif isinstance(phi, casadi.SX): + symbol = SX.sym + else: + Exception("generate_c_code_disc_dyn: disc_dyn_expr must be a CasADi expression, you have type: {}".format(type(phi))) + + # assume nx1 = nx !!! + lam = symbol('lam', nx, 1) + + # generate jacobians + ux = vertcat(u,x) + jac_ux = jacobian(phi, ux) + # generate adjoint + adj_ux = jtimes(phi, ux, lam, True) + # generate hessian + hess_ux = jacobian(adj_ux, ux) + + ## change directory + code_export_dir = opts["code_export_directory"] + if not os.path.exists(code_export_dir): + os.makedirs(code_export_dir) + + cwd = os.getcwd() + os.chdir(code_export_dir) + model_dir = model_name + '_model' + if not os.path.exists(model_dir): + os.mkdir(model_dir) + model_dir_location = './' + model_dir + os.chdir(model_dir_location) + + # set up & generate Functions + fun_name = model_name + '_dyn_disc_phi_fun' + phi_fun = Function(fun_name, [x, u, p], [phi]) + phi_fun.generate(fun_name, casadi_opts) + + fun_name = model_name + '_dyn_disc_phi_fun_jac' + phi_fun_jac_ut_xt = Function(fun_name, [x, u, p], [phi, jac_ux.T]) + phi_fun_jac_ut_xt.generate(fun_name, casadi_opts) + + fun_name = model_name + '_dyn_disc_phi_fun_jac_hess' + phi_fun_jac_ut_xt_hess = Function(fun_name, [x, u, lam, p], [phi, jac_ux.T, hess_ux]) + phi_fun_jac_ut_xt_hess.generate(fun_name, casadi_opts) + + os.chdir(cwd) diff --git a/pyextra/acados_template/generate_c_code_explicit_ode.py b/pyextra/acados_template/generate_c_code_explicit_ode.py new file mode 100644 index 0000000000..fb0c020b0b --- /dev/null +++ b/pyextra/acados_template/generate_c_code_explicit_ode.py @@ -0,0 +1,124 @@ +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + +import os +from casadi import * +from .utils import ALLOWED_CASADI_VERSIONS, is_empty, casadi_version_warning + +def generate_c_code_explicit_ode( model, opts ): + + casadi_version = CasadiMeta.version() + casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double') + if casadi_version not in (ALLOWED_CASADI_VERSIONS): + casadi_version_warning(casadi_version) + + + generate_hess = opts["generate_hess"] + code_export_dir = opts["code_export_directory"] + + # load model + x = model.x + u = model.u + p = model.p + f_expl = model.f_expl_expr + model_name = model.name + + ## get model dimensions + nx = x.size()[0] + nu = u.size()[0] + + if isinstance(f_expl, casadi.MX): + symbol = MX.sym + elif isinstance(f_expl, casadi.SX): + symbol = SX.sym + else: + raise Exception("Invalid type for f_expl! Possible types are 'SX' and 'MX'. Exiting.") + ## set up functions to be exported + Sx = symbol('Sx', nx, nx) + Sp = symbol('Sp', nx, nu) + lambdaX = symbol('lambdaX', nx, 1) + + fun_name = model_name + '_expl_ode_fun' + + ## Set up functions + expl_ode_fun = Function(fun_name, [x, u, p], [f_expl]) + + vdeX = jtimes(f_expl,x,Sx) + vdeP = jacobian(f_expl,u) + jtimes(f_expl,x,Sp) + + fun_name = model_name + '_expl_vde_forw' + + expl_vde_forw = Function(fun_name, [x, Sx, Sp, u, p], [f_expl, vdeX, vdeP]) + + adj = jtimes(f_expl, vertcat(x, u), lambdaX, True) + + fun_name = model_name + '_expl_vde_adj' + expl_vde_adj = Function(fun_name, [x, lambdaX, u, p], [adj]) + + if generate_hess: + S_forw = vertcat(horzcat(Sx, Sp), horzcat(DM.zeros(nu,nx), DM.eye(nu))) + hess = mtimes(transpose(S_forw),jtimes(adj, vertcat(x,u), S_forw)) + hess2 = [] + for j in range(nx+nu): + for i in range(j,nx+nu): + hess2 = vertcat(hess2, hess[i,j]) + + fun_name = model_name + '_expl_ode_hess' + expl_ode_hess = Function(fun_name, [x, Sx, Sp, lambdaX, u, p], [adj, hess2]) + + ## generate C code + if not os.path.exists(code_export_dir): + os.makedirs(code_export_dir) + + cwd = os.getcwd() + os.chdir(code_export_dir) + model_dir = model_name + '_model' + if not os.path.exists(model_dir): + os.mkdir(model_dir) + model_dir_location = './' + model_dir + os.chdir(model_dir_location) + fun_name = model_name + '_expl_ode_fun' + expl_ode_fun.generate(fun_name, casadi_opts) + + fun_name = model_name + '_expl_vde_forw' + expl_vde_forw.generate(fun_name, casadi_opts) + + fun_name = model_name + '_expl_vde_adj' + expl_vde_adj.generate(fun_name, casadi_opts) + + if generate_hess: + fun_name = model_name + '_expl_ode_hess' + expl_ode_hess.generate(fun_name, casadi_opts) + os.chdir(cwd) + + return diff --git a/pyextra/acados_template/generate_c_code_external_cost.py b/pyextra/acados_template/generate_c_code_external_cost.py new file mode 100644 index 0000000000..bc21f85f70 --- /dev/null +++ b/pyextra/acados_template/generate_c_code_external_cost.py @@ -0,0 +1,110 @@ +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + +import os +from casadi import SX, MX, Function, transpose, vertcat, horzcat, hessian, CasadiMeta +from .utils import ALLOWED_CASADI_VERSIONS, casadi_version_warning + + +def generate_c_code_external_cost(model, stage_type, opts): + + casadi_version = CasadiMeta.version() + casadi_opts = dict(mex=False, casadi_int="int", casadi_real="double") + + if casadi_version not in (ALLOWED_CASADI_VERSIONS): + casadi_version_warning(casadi_version) + + x = model.x + p = model.p + + if isinstance(x, MX): + symbol = MX.sym + else: + symbol = SX.sym + + if stage_type == 'terminal': + suffix_name = "_cost_ext_cost_e_fun" + suffix_name_hess = "_cost_ext_cost_e_fun_jac_hess" + suffix_name_jac = "_cost_ext_cost_e_fun_jac" + u = symbol("u", 0, 0) + ext_cost = model.cost_expr_ext_cost_e + + elif stage_type == 'path': + suffix_name = "_cost_ext_cost_fun" + suffix_name_hess = "_cost_ext_cost_fun_jac_hess" + suffix_name_jac = "_cost_ext_cost_fun_jac" + u = model.u + ext_cost = model.cost_expr_ext_cost + + elif stage_type == 'initial': + suffix_name = "_cost_ext_cost_0_fun" + suffix_name_hess = "_cost_ext_cost_0_fun_jac_hess" + suffix_name_jac = "_cost_ext_cost_0_fun_jac" + u = model.u + ext_cost = model.cost_expr_ext_cost_0 + + # set up functions to be exported + fun_name = model.name + suffix_name + fun_name_hess = model.name + suffix_name_hess + fun_name_jac = model.name + suffix_name_jac + + # generate expression for full gradient and Hessian + full_hess, grad = hessian(ext_cost, vertcat(u, x)) + + ext_cost_fun = Function(fun_name, [x, u, p], [ext_cost]) + ext_cost_fun_jac_hess = Function( + fun_name_hess, [x, u, p], [ext_cost, grad, full_hess] + ) + ext_cost_fun_jac = Function( + fun_name_jac, [x, u, p], [ext_cost, grad] + ) + + # generate C code + code_export_dir = opts["code_export_directory"] + if not os.path.exists(code_export_dir): + os.makedirs(code_export_dir) + + cwd = os.getcwd() + os.chdir(code_export_dir) + gen_dir = model.name + '_cost' + if not os.path.exists(gen_dir): + os.mkdir(gen_dir) + gen_dir_location = "./" + gen_dir + os.chdir(gen_dir_location) + + ext_cost_fun.generate(fun_name, casadi_opts) + ext_cost_fun_jac_hess.generate(fun_name_hess, casadi_opts) + ext_cost_fun_jac.generate(fun_name_jac, casadi_opts) + + os.chdir(cwd) + return diff --git a/pyextra/acados_template/generate_c_code_gnsf.py b/pyextra/acados_template/generate_c_code_gnsf.py new file mode 100644 index 0000000000..3203cbbcce --- /dev/null +++ b/pyextra/acados_template/generate_c_code_gnsf.py @@ -0,0 +1,131 @@ +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + +import os +from casadi import * +from .utils import ALLOWED_CASADI_VERSIONS, is_empty, casadi_version_warning + +def generate_c_code_gnsf( model, opts ): + + casadi_version = CasadiMeta.version() + casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double') + if casadi_version not in (ALLOWED_CASADI_VERSIONS): + casadi_version_warning(casadi_version) + + model_name = model.name + code_export_dir = opts["code_export_directory"] + + # set up directory + if not os.path.exists(code_export_dir): + os.makedirs(code_export_dir) + + cwd = os.getcwd() + os.chdir(code_export_dir) + model_dir = model_name + '_model' + if not os.path.exists(model_dir): + os.mkdir(model_dir) + model_dir_location = './' + model_dir + os.chdir(model_dir_location) + + # obtain gnsf dimensions + get_matrices_fun = model.get_matrices_fun + phi_fun = model.phi_fun + + size_gnsf_A = get_matrices_fun.size_out(0) + gnsf_nx1 = size_gnsf_A[1] + gnsf_nz1 = size_gnsf_A[0] - size_gnsf_A[1] + gnsf_nuhat = max(phi_fun.size_in(1)) + gnsf_ny = max(phi_fun.size_in(0)) + gnsf_nout = max(phi_fun.size_out(0)) + + # set up expressions + # if the model uses MX because of cost/constraints + # the DAE can be exported as SX -> detect GNSF in Matlab + # -> evaluated SX GNSF functions with MX. + u = model.u + + if isinstance(u, casadi.MX): + symbol = MX.sym + else: + symbol = SX.sym + + y = symbol("y", gnsf_ny, 1) + uhat = symbol("uhat", gnsf_nuhat, 1) + p = model.p + x1 = symbol("gnsf_x1", gnsf_nx1, 1) + x1dot = symbol("gnsf_x1dot", gnsf_nx1, 1) + z1 = symbol("gnsf_z1", gnsf_nz1, 1) + dummy = symbol("gnsf_dummy", 1, 1) + empty_var = symbol("gnsf_empty_var", 0, 0) + + ## generate C code + fun_name = model_name + '_gnsf_phi_fun' + phi_fun_ = Function(fun_name, [y, uhat, p], [phi_fun(y, uhat, p)]) + phi_fun_.generate(fun_name, casadi_opts) + + fun_name = model_name + '_gnsf_phi_fun_jac_y' + phi_fun_jac_y = model.phi_fun_jac_y + phi_fun_jac_y_ = Function(fun_name, [y, uhat, p], phi_fun_jac_y(y, uhat, p)) + phi_fun_jac_y_.generate(fun_name, casadi_opts) + + fun_name = model_name + '_gnsf_phi_jac_y_uhat' + phi_jac_y_uhat = model.phi_jac_y_uhat + phi_jac_y_uhat_ = Function(fun_name, [y, uhat, p], phi_jac_y_uhat(y, uhat, p)) + phi_jac_y_uhat_.generate(fun_name, casadi_opts) + + fun_name = model_name + '_gnsf_f_lo_fun_jac_x1k1uz' + f_lo_fun_jac_x1k1uz = model.f_lo_fun_jac_x1k1uz + f_lo_fun_jac_x1k1uz_eval = f_lo_fun_jac_x1k1uz(x1, x1dot, z1, u, p) + + # avoid codegeneration issue + if not isinstance(f_lo_fun_jac_x1k1uz_eval, tuple) and is_empty(f_lo_fun_jac_x1k1uz_eval): + f_lo_fun_jac_x1k1uz_eval = [empty_var] + + f_lo_fun_jac_x1k1uz_ = Function(fun_name, [x1, x1dot, z1, u, p], + f_lo_fun_jac_x1k1uz_eval) + f_lo_fun_jac_x1k1uz_.generate(fun_name, casadi_opts) + + fun_name = model_name + '_gnsf_get_matrices_fun' + get_matrices_fun_ = Function(fun_name, [dummy], get_matrices_fun(1)) + get_matrices_fun_.generate(fun_name, casadi_opts) + + # remove fields for json dump + del model.phi_fun + del model.phi_fun_jac_y + del model.phi_jac_y_uhat + del model.f_lo_fun_jac_x1k1uz + del model.get_matrices_fun + + os.chdir(cwd) + + return diff --git a/pyextra/acados_template/generate_c_code_implicit_ode.py b/pyextra/acados_template/generate_c_code_implicit_ode.py new file mode 100644 index 0000000000..5ef417c414 --- /dev/null +++ b/pyextra/acados_template/generate_c_code_implicit_ode.py @@ -0,0 +1,136 @@ +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + +import os +from casadi import * +from .utils import ALLOWED_CASADI_VERSIONS, is_empty, casadi_length, casadi_version_warning + +def generate_c_code_implicit_ode( model, opts ): + + casadi_version = CasadiMeta.version() + casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double') + if casadi_version not in (ALLOWED_CASADI_VERSIONS): + casadi_version_warning(casadi_version) + + generate_hess = opts["generate_hess"] + code_export_dir = opts["code_export_directory"] + + ## load model + x = model.x + xdot = model.xdot + u = model.u + z = model.z + p = model.p + f_impl = model.f_impl_expr + model_name = model.name + + ## get model dimensions + nx = casadi_length(x) + nu = casadi_length(u) + nz = casadi_length(z) + + ## generate jacobians + jac_x = jacobian(f_impl, x) + jac_xdot = jacobian(f_impl, xdot) + jac_u = jacobian(f_impl, u) + jac_z = jacobian(f_impl, z) + + ## generate hessian + x_xdot_z_u = vertcat(x, xdot, z, u) + + if isinstance(x, casadi.MX): + symbol = MX.sym + else: + symbol = SX.sym + + multiplier = symbol('multiplier', nx + nz) + + ADJ = jtimes(f_impl, x_xdot_z_u, multiplier, True) + HESS = jacobian(ADJ, x_xdot_z_u) + + ## Set up functions + p = model.p + fun_name = model_name + '_impl_dae_fun' + impl_dae_fun = Function(fun_name, [x, xdot, u, z, p], [f_impl]) + + fun_name = model_name + '_impl_dae_fun_jac_x_xdot_z' + impl_dae_fun_jac_x_xdot_z = Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_z]) + + # fun_name = model_name + '_impl_dae_fun_jac_x_xdot_z' + # impl_dae_fun_jac_x_xdot = Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_z]) + + # fun_name = model_name + '_impl_dae_jac_x_xdot_u' + # impl_dae_jac_x_xdot_u = Function(fun_name, [x, xdot, u, z, p], [jac_x, jac_xdot, jac_u, jac_z]) + + fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u_z' + impl_dae_fun_jac_x_xdot_u_z = Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_u, jac_z]) + + fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u' + impl_dae_fun_jac_x_xdot_u = Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_u]) + + fun_name = model_name + '_impl_dae_jac_x_xdot_u_z' + impl_dae_jac_x_xdot_u_z = Function(fun_name, [x, xdot, u, z, p], [jac_x, jac_xdot, jac_u, jac_z]) + + + fun_name = model_name + '_impl_dae_hess' + impl_dae_hess = Function(fun_name, [x, xdot, u, z, multiplier, p], [HESS]) + + # generate C code + if not os.path.exists(code_export_dir): + os.makedirs(code_export_dir) + + cwd = os.getcwd() + os.chdir(code_export_dir) + model_dir = model_name + '_model' + if not os.path.exists(model_dir): + os.mkdir(model_dir) + model_dir_location = './' + model_dir + os.chdir(model_dir_location) + + fun_name = model_name + '_impl_dae_fun' + impl_dae_fun.generate(fun_name, casadi_opts) + + fun_name = model_name + '_impl_dae_fun_jac_x_xdot_z' + impl_dae_fun_jac_x_xdot_z.generate(fun_name, casadi_opts) + + fun_name = model_name + '_impl_dae_jac_x_xdot_u_z' + impl_dae_jac_x_xdot_u_z.generate(fun_name, casadi_opts) + + fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u_z' + impl_dae_fun_jac_x_xdot_u_z.generate(fun_name, casadi_opts) + + if generate_hess: + fun_name = model_name + '_impl_dae_hess' + impl_dae_hess.generate(fun_name, casadi_opts) + + os.chdir(cwd) diff --git a/pyextra/acados_template/generate_c_code_nls_cost.py b/pyextra/acados_template/generate_c_code_nls_cost.py new file mode 100644 index 0000000000..cdb0bf0019 --- /dev/null +++ b/pyextra/acados_template/generate_c_code_nls_cost.py @@ -0,0 +1,113 @@ +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + +import os +from casadi import * +from .utils import ALLOWED_CASADI_VERSIONS, casadi_length, casadi_version_warning + +def generate_c_code_nls_cost( model, cost_name, stage_type, opts ): + + casadi_version = CasadiMeta.version() + casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double') + + if casadi_version not in (ALLOWED_CASADI_VERSIONS): + casadi_version_warning(casadi_version) + + x = model.x + p = model.p + + if isinstance(x, casadi.MX): + symbol = MX.sym + else: + symbol = SX.sym + + if stage_type == 'terminal': + middle_name = '_cost_y_e' + u = symbol('u', 0, 0) + cost_expr = model.cost_y_expr_e + + elif stage_type == 'initial': + middle_name = '_cost_y_0' + u = model.u + cost_expr = model.cost_y_expr_0 + + elif stage_type == 'path': + middle_name = '_cost_y' + u = model.u + cost_expr = model.cost_y_expr + + # set up directory + code_export_dir = opts["code_export_directory"] + if not os.path.exists(code_export_dir): + os.makedirs(code_export_dir) + + cwd = os.getcwd() + os.chdir(code_export_dir) + gen_dir = cost_name + '_cost' + if not os.path.exists(gen_dir): + os.mkdir(gen_dir) + gen_dir_location = './' + gen_dir + os.chdir(gen_dir_location) + + # set up expressions + cost_jac_expr = transpose(jacobian(cost_expr, vertcat(u, x))) + + ny = casadi_length(cost_expr) + + y = symbol('y', ny, 1) + + y_adj = jtimes(cost_expr, vertcat(u, x), y, True) + y_hess = jacobian(y_adj, vertcat(u, x)) + + ## generate C code + suffix_name = '_fun' + fun_name = cost_name + middle_name + suffix_name + y_fun = Function( fun_name, [x, u, p], \ + [ cost_expr ]) + y_fun.generate( fun_name, casadi_opts ) + + suffix_name = '_fun_jac_ut_xt' + fun_name = cost_name + middle_name + suffix_name + y_fun_jac_ut_xt = Function(fun_name, [x, u, p], \ + [ cost_expr, cost_jac_expr ]) + y_fun_jac_ut_xt.generate( fun_name, casadi_opts ) + + suffix_name = '_hess' + fun_name = cost_name + middle_name + suffix_name + y_hess = Function(fun_name, [x, u, y, p], [ y_hess ]) + y_hess.generate( fun_name, casadi_opts ) + + os.chdir(cwd) + + return + diff --git a/pyextra/acados_template/simulink_default_opts.json b/pyextra/acados_template/simulink_default_opts.json new file mode 100644 index 0000000000..16074a4027 --- /dev/null +++ b/pyextra/acados_template/simulink_default_opts.json @@ -0,0 +1,36 @@ +{ + "outputs": { + "u0": 1, + "utraj": 0, + "xtraj": 0, + "solver_status": 1, + "KKT_residual": 1, + "x1": 1, + "CPU_time": 1, + "sqp_iter": 1 + }, + "inputs": { + "lbx_0": 1, + "ubx_0": 1, + "parameter_traj": 1, + "y_ref_0": 1, + "y_ref": 1, + "y_ref_e": 1, + "lbx": 1, + "ubx": 1, + "lbx_e": 1, + "ubx_e": 1, + "lbu": 1, + "ubu": 1, + "lg": 1, + "ug": 1, + "lh": 1, + "uh": 1, + "cost_W_0": 0, + "cost_W": 0, + "cost_W_e": 0, + "x_init": 0, + "u_init": 0 + }, + "samplingtime": "t0" +} diff --git a/pyextra/acados_template/utils.py b/pyextra/acados_template/utils.py new file mode 100644 index 0000000000..b749909aeb --- /dev/null +++ b/pyextra/acados_template/utils.py @@ -0,0 +1,438 @@ +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + +import os, sys, json +import urllib.request +import shutil +import numpy as np +from casadi import SX, MX, DM, Function, CasadiMeta + +ALLOWED_CASADI_VERSIONS = ('3.5.5', '3.5.4', '3.5.3', '3.5.2', '3.5.1', '3.4.5', '3.4.0') + +TERA_VERSION = "0.0.34" + +def get_acados_path(): + ACADOS_PATH = os.environ.get('ACADOS_SOURCE_DIR') + if not ACADOS_PATH: + acados_template_path = os.path.dirname(os.path.abspath(__file__)) + acados_path = os.path.join(acados_template_path, '../../../') + ACADOS_PATH = os.path.realpath(acados_path) + msg = 'Warning: Did not find environment variable ACADOS_SOURCE_DIR, ' + msg += 'guessed ACADOS_PATH to be {}.\n'.format(ACADOS_PATH) + msg += 'Please export ACADOS_SOURCE_DIR to not avoid this warning.' + print(msg) + return ACADOS_PATH + + +def get_tera_exec_path(): + ACADOS_PATH = get_acados_path() + return os.path.join(ACADOS_PATH, 'bin/t_renderer') + + +platform2tera = { + "linux": "linux", + "darwin": "osx", + "win32": "window.exe" +} + + +def casadi_version_warning(casadi_version): + msg = 'Warning: Please note that the following versions of CasADi are ' + msg += 'officially supported: {}.\n '.format(" or ".join(ALLOWED_CASADI_VERSIONS)) + msg += 'If there is an incompatibility with the CasADi generated code, ' + msg += 'please consider changing your CasADi version.\n' + msg += 'Version {} currently in use.'.format(casadi_version) + print(msg) + + +def is_column(x): + if isinstance(x, np.ndarray): + if x.ndim == 1: + return True + elif x.ndim == 2 and x.shape[1] == 1: + return True + else: + return False + elif isinstance(x, (MX, SX, DM)): + if x.shape[1] == 1: + return True + elif x.shape[0] == 0 and x.shape[1] == 0: + return True + else: + return False + elif x == None or x == []: + return False + else: + raise Exception("is_column expects one of the following types: np.ndarray, casadi.MX, casadi.SX." + + " Got: " + str(type(x))) + + +def is_empty(x): + if isinstance(x, (MX, SX, DM)): + return x.is_empty() + elif isinstance(x, np.ndarray): + if np.prod(x.shape) == 0: + return True + else: + return False + elif x == None or x == []: + return True + else: + raise Exception("is_empty expects one of the following types: casadi.MX, casadi.SX, " + + "None, numpy array empty list. Got: " + str(type(x))) + + +def casadi_length(x): + if isinstance(x, (MX, SX, DM)): + return int(np.prod(x.shape)) + else: + raise Exception("casadi_length expects one of the following types: casadi.MX, casadi.SX." + + " Got: " + str(type(x))) + + +def make_model_consistent(model): + x = model.x + xdot = model.xdot + u = model.u + z = model.z + p = model.p + + if isinstance(x, MX): + symbol = MX.sym + elif isinstance(x, SX): + symbol = SX.sym + else: + raise Exception("model.x must be casadi.SX or casadi.MX, got {}".format(type(x))) + + if is_empty(p): + model.p = symbol('p', 0, 0) + + if is_empty(z): + model.z = symbol('z', 0, 0) + + return model + + +def get_tera(): + tera_path = get_tera_exec_path() + acados_path = get_acados_path() + + if os.path.exists(tera_path) and os.access(tera_path, os.X_OK): + return tera_path + + repo_url = "https://github.com/acados/tera_renderer/releases" + url = "{}/download/v{}/t_renderer-v{}-{}".format( + repo_url, TERA_VERSION, TERA_VERSION, platform2tera[sys.platform]) + + manual_install = 'For manual installation follow these instructions:\n' + manual_install += '1 Download binaries from {}\n'.format(url) + manual_install += '2 Copy them in {}/bin\n'.format(acados_path) + manual_install += '3 Strip the version and platform from the binaries: ' + manual_install += 'as t_renderer-v0.0.34-X -> t_renderer)\n' + manual_install += '4 Enable execution privilege on the file "t_renderer" with:\n' + manual_install += '"chmod +x {}"\n\n'.format(tera_path) + + msg = "\n" + msg += 'Tera template render executable not found, ' + msg += 'while looking in path:\n{}\n'.format(tera_path) + msg += 'In order to be able to render the templates, ' + msg += 'you need to download the tera renderer binaries from:\n' + msg += '{}\n\n'.format(repo_url) + msg += 'Do you wish to set up Tera renderer automatically?\n' + msg += 'y/N? (press y to download tera or any key for manual installation)\n' + + if input(msg) == 'y': + print("Dowloading {}".format(url)) + with urllib.request.urlopen(url) as response, open(tera_path, 'wb') as out_file: + shutil.copyfileobj(response, out_file) + print("Successfully downloaded t_renderer.") + os.chmod(tera_path, 0o755) + return tera_path + + msg_cancel = "\nYou cancelled automatic download.\n\n" + msg_cancel += manual_install + msg_cancel += "Once installed re-run your script.\n\n" + print(msg_cancel) + + sys.exit(1) + + +def render_template(in_file, out_file, template_dir, json_path): + cwd = os.getcwd() + if not os.path.exists(template_dir): + os.mkdir(template_dir) + os.chdir(template_dir) + + tera_path = get_tera() + + # setting up loader and environment + acados_path = os.path.dirname(os.path.abspath(__file__)) + + template_glob = acados_path + '/c_templates_tera/*' + acados_template_path = acados_path + '/c_templates_tera' + + # call tera as system cmd + os_cmd = "{tera_path} '{template_glob}' '{in_file}' '{json_path}' '{out_file}'".format( + tera_path=tera_path, + template_glob=template_glob, + json_path=json_path, + in_file=in_file, + out_file=out_file + ) + status = os.system(os_cmd) + if (status != 0): + raise Exception('Rendering of {} failed! Exiting.\n'.format(in_file)) + + os.chdir(cwd) + + +## Conversion functions +def np_array_to_list(np_array): + if isinstance(np_array, (np.ndarray)): + return np_array.tolist() + elif isinstance(np_array, (SX)): + return DM(np_array).full() + elif isinstance(np_array, (DM)): + return np_array.full() + else: + raise(Exception( + "Cannot convert to list type {}".format(type(np_array)) + )) + + +def format_class_dict(d): + """ + removes the __ artifact from class to dict conversion + """ + out = {} + for k, v in d.items(): + if isinstance(v, dict): + v = format_class_dict(v) + + out_key = k.split('__', 1)[-1] + out[k.replace(k, out_key)] = v + return out + + +def acados_class2dict(class_instance): + """ + removes the __ artifact from class to dict conversion + """ + + d = dict(class_instance.__dict__) + out = {} + for k, v in d.items(): + if isinstance(v, dict): + v = format_class_dict(v) + + out_key = k.split('__', 1)[-1] + out[k.replace(k, out_key)] = v + return out + + +def ocp_check_against_layout(ocp_nlp, ocp_dims): + """ + Check dimensions against layout + Parameters + --------- + ocp_nlp : dict + dictionary loaded from JSON to be post-processed. + + ocp_dims : instance of AcadosOcpDims + """ + + # load JSON layout + current_module = sys.modules[__name__] + acados_path = os.path.dirname(current_module.__file__) + with open(acados_path + '/acados_layout.json', 'r') as f: + ocp_nlp_layout = json.load(f) + + ocp_check_against_layout_recursion(ocp_nlp, ocp_dims, ocp_nlp_layout) + return + + +def ocp_check_against_layout_recursion(ocp_nlp, ocp_dims, layout): + + for key, item in ocp_nlp.items(): + + try: + layout_of_key = layout[key] + except KeyError: + raise Exception("ocp_check_against_layout_recursion: field" \ + " '{0}' is not in layout but in OCP description.".format(key)) + + if isinstance(item, dict): + ocp_check_against_layout_recursion(item, ocp_dims, layout_of_key) + + if 'ndarray' in layout_of_key: + if isinstance(item, int) or isinstance(item, float): + item = np.array([item]) + if isinstance(item, (list, np.ndarray)) and (layout_of_key[0] != 'str'): + dim_layout = [] + dim_names = layout_of_key[1] + + for dim_name in dim_names: + dim_layout.append(ocp_dims[dim_name]) + + dims = tuple(dim_layout) + + item = np.array(item) + item_dims = item.shape + if len(item_dims) != len(dims): + raise Exception('Mismatching dimensions for field {0}. ' \ + 'Expected {1} dimensional array, got {2} dimensional array.' \ + .format(key, len(dims), len(item_dims))) + + if np.prod(item_dims) != 0 or np.prod(dims) != 0: + if dims != item_dims: + raise Exception('acados -- mismatching dimensions for field {0}. ' \ + 'Provided data has dimensions {1}, ' \ + 'while associated dimensions {2} are {3}' \ + .format(key, item_dims, dim_names, dims)) + return + + +def J_to_idx(J): + nrows = J.shape[0] + idx = np.zeros((nrows, )) + for i in range(nrows): + this_idx = np.nonzero(J[i,:])[0] + if len(this_idx) != 1: + raise Exception('Invalid J matrix structure detected, ' \ + 'must contain one nonzero element per row. Exiting.') + if this_idx.size > 0 and J[i,this_idx[0]] != 1: + raise Exception('J matrices can only contain 1s. Exiting.') + idx[i] = this_idx[0] + return idx + + +def J_to_idx_slack(J): + nrows = J.shape[0] + ncol = J.shape[1] + idx = np.zeros((ncol, )) + i_idx = 0 + for i in range(nrows): + this_idx = np.nonzero(J[i,:])[0] + if len(this_idx) == 1: + idx[i_idx] = i + i_idx = i_idx + 1 + elif len(this_idx) > 1: + raise Exception('J_to_idx_slack: Invalid J matrix. Exiting. ' \ + 'Found more than one nonzero in row ' + str(i)) + if this_idx.size > 0 and J[i,this_idx[0]] != 1: + raise Exception('J_to_idx_slack: J matrices can only contain 1s, ' \ + 'got J(' + str(i) + ', ' + str(this_idx[0]) + ') = ' + str(J[i,this_idx[0]]) ) + if not i_idx == ncol: + raise Exception('J_to_idx_slack: J must contain a 1 in every column!') + return idx + + +def acados_dae_model_json_dump(model): + + # load model + x = model.x + xdot = model.xdot + u = model.u + z = model.z + p = model.p + + f_impl = model.f_impl_expr + model_name = model.name + + # create struct with impl_dae_fun, casadi_version + fun_name = model_name + '_impl_dae_fun' + impl_dae_fun = Function(fun_name, [x, xdot, u, z, p], [f_impl]) + + casadi_version = CasadiMeta.version() + str_impl_dae_fun = impl_dae_fun.serialize() + + dae_dict = {"str_impl_dae_fun": str_impl_dae_fun, "casadi_version": casadi_version} + + # dump + json_file = model_name + '_acados_dae.json' + with open(json_file, 'w') as f: + json.dump(dae_dict, f, default=np_array_to_list, indent=4, sort_keys=True) + print("dumped ", model_name, " dae to file:", json_file, "\n") + + +def set_up_imported_gnsf_model(acados_formulation): + + gnsf = acados_formulation.gnsf_model + + # check CasADi version + # dump_casadi_version = gnsf['casadi_version'] + # casadi_version = CasadiMeta.version() + + # if not casadi_version == dump_casadi_version: + # print("WARNING: GNSF model was dumped with another CasADi version.\n" + # + "This might yield errors. Please use the same version for compatibility, serialize version: " + # + dump_casadi_version + " current Python CasADi verison: " + casadi_version) + # input("Press any key to attempt to continue...") + + # load model + phi_fun = Function.deserialize(gnsf['phi_fun']) + phi_fun_jac_y = Function.deserialize(gnsf['phi_fun_jac_y']) + phi_jac_y_uhat = Function.deserialize(gnsf['phi_jac_y_uhat']) + get_matrices_fun = Function.deserialize(gnsf['get_matrices_fun']) + + # obtain gnsf dimensions + size_gnsf_A = get_matrices_fun.size_out(0) + acados_formulation.dims.gnsf_nx1 = size_gnsf_A[1] + acados_formulation.dims.gnsf_nz1 = size_gnsf_A[0] - size_gnsf_A[1] + acados_formulation.dims.gnsf_nuhat = max(phi_fun.size_in(1)) + acados_formulation.dims.gnsf_ny = max(phi_fun.size_in(0)) + acados_formulation.dims.gnsf_nout = max(phi_fun.size_out(0)) + + # save gnsf functions in model + acados_formulation.model.phi_fun = phi_fun + acados_formulation.model.phi_fun_jac_y = phi_fun_jac_y + acados_formulation.model.phi_jac_y_uhat = phi_jac_y_uhat + acados_formulation.model.get_matrices_fun = get_matrices_fun + + if "f_lo_fun_jac_x1k1uz" in gnsf: + f_lo_fun_jac_x1k1uz = Function.deserialize(gnsf['f_lo_fun_jac_x1k1uz']) + acados_formulation.model.f_lo_fun_jac_x1k1uz = f_lo_fun_jac_x1k1uz + else: + dummy_var_x1 = SX.sym('dummy_var_x1', acados_formulation.dims.gnsf_nx1) + dummy_var_x1dot = SX.sym('dummy_var_x1dot', acados_formulation.dims.gnsf_nx1) + dummy_var_z1 = SX.sym('dummy_var_z1', acados_formulation.dims.gnsf_nz1) + dummy_var_u = SX.sym('dummy_var_z1', acados_formulation.dims.nu) + dummy_var_p = SX.sym('dummy_var_z1', acados_formulation.dims.np) + empty_var = SX.sym('empty_var', 0, 0) + + empty_fun = Function('empty_fun', \ + [dummy_var_x1, dummy_var_x1dot, dummy_var_z1, dummy_var_u, dummy_var_p], + [empty_var]) + acados_formulation.model.f_lo_fun_jac_x1k1uz = empty_fun + + del acados_formulation.gnsf_model diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index e97792f7c8..5aceea2170 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -22,6 +22,8 @@ from selfdrive.version import dirty, get_git_commit, version, origin, branch, co terms_version, training_version, comma_remote, \ get_git_branch, get_git_remote +sys.path.append(os.path.join(BASEDIR, "pyextra")) + def manager_init(): # update system time from panda