[
{
"path": ".gitignore",
"content": "# CLion stuff\n.idea/\ncmake-build-debug/\n"
},
{
"path": "CMakeLists.txt",
"content": "# rpg_quadrotor_mpc\n# A model predictive control implementation for quadrotors.\n# Copyright (C) 2017-2018 Philipp Foehn, \n# Robotics and Perception Group, University of Zurich\n# \n# Intended to be used with rpg_quadrotor_control and rpg_quadrotor_common.\n# https://github.com/uzh-rpg/rpg_quadrotor_control\n# \n# This program is free software: you can redistribute it and/or modify\n# it under the terms of the GNU General Public License as published by\n# the Free Software Foundation, either version 3 of the License, or\n# (at your option) any later version.\n# \n# This program is distributed in the hope that it will be useful,\n# but WITHOUT ANY WARRANTY; without even the implied warranty of\n# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n# GNU General Public License for more details.\n# \n# You should have received a copy of the GNU General Public License\n# along with this program. If not, see .\n\n\ncmake_minimum_required(VERSION 2.8.3)\nproject(rpg_mpc)\n\nfind_package(catkin_simple REQUIRED)\n\ncatkin_simple()\n\n# activate c++ 11\nIF(CMAKE_COMPILER_IS_GNUCC)\n SET(CMAKE_CXX_FLAGS \"${CMAKE_CXX_FLAGS} -std=c++0x\")\nELSE()\n SET(CMAKE_CXX_FLAGS \"${CMAKE_CXX_FLAGS} -std=c++11\")\nENDIF()\n\n# ARM NEON flags\nif(\"${CMAKE_HOST_SYSTEM_PROCESSOR}\" STREQUAL \"armv7l\")\n set(CMAKE_C_FLAGS \"${CMAKE_C_FLAGS} -march=armv7-a -mfpu=neon -mfloat-abi=hard -funsafe-math-optimizations\")\n set(CMAKE_CXX_FLAGS \"${CMAKE_CXX_FLAGS} -march=armv7-a -mfpu=neon -mfloat-abi=hard -funsafe-math-optimizations\")\n message(\"enabling ARM neon optimizations\")\nendif()\n\n# flags for speed (should already be enabled by default)\nset(CMAKE_CXX_FLAGS \"${CMAKE_CXX_FLAGS} -Wall -Wextra\")\nset(CMAKE_CXX_FLAGS_DEBUG \"${CMAKE_CXX_FLAGS_DEBUG} -g\")\nset(CMAKE_CXX_FLAGS_RELEASE \"${CMAKE_CXX_FLAGS_RELEASE} -fopenmp -O3\")\n\ncs_add_library(mpc_solver\n externals/qpoases/SRC/Bounds.cpp\n externals/qpoases/SRC/Constraints.cpp\n externals/qpoases/SRC/CyclingManager.cpp\n externals/qpoases/SRC/Indexlist.cpp\n externals/qpoases/SRC/MessageHandling.cpp\n externals/qpoases/SRC/QProblem.cpp\n externals/qpoases/SRC/QProblemB.cpp\n externals/qpoases/SRC/SubjectTo.cpp\n externals/qpoases/SRC/Utils.cpp\n externals/qpoases/SRC/EXTRAS/SolutionAnalysis.cpp\n model/quadrotor_mpc_codegen/acado_qpoases_interface.cpp\n model/quadrotor_mpc_codegen/acado_integrator.c\n model/quadrotor_mpc_codegen/acado_solver.c\n model/quadrotor_mpc_codegen/acado_auxiliary_functions.c)\n\ntarget_include_directories(mpc_solver PUBLIC\n model/quadrotor_mpc_codegen/\n externals/qpoases\n externals/qpoases/INCLUDE\n externals/qpoases/SRC)\n\ncs_add_library(mpc_wrapper\n src/mpc_wrapper.cpp)\n\ntarget_link_libraries(mpc_wrapper\n mpc_solver)\n\ncs_add_library(mpc_controller\n src/mpc_controller.cpp)\n\ntarget_link_libraries(mpc_controller \n mpc_wrapper)\n\n# make an executable\ncs_install()\ncs_export()\n\ncs_add_executable(mpc_controller_node\n test/mpc_node.cpp)\ntarget_link_libraries(mpc_controller_node\n mpc_controller\n mpc_wrapper\n mpc_solver)\n\nif(catkin_LIBRARIES MATCHES \"autopilot\")\n\n message(\"Building with MPC Autopilot\")\n cs_add_executable(autopilot_mpc_instance\n src/autopilot_mpc_instance.cpp)\n target_link_libraries(autopilot_mpc_instance\n mpc_controller\n mpc_wrapper\n mpc_solver)\n\nendif(catkin_LIBRARIES MATCHES \"autopilot\")\n\ncs_install()\n\ncs_export()\n"
},
{
"path": "COPYING",
"content": " GNU GENERAL PUBLIC LICENSE\n Version 3, 29 June 2007\n\n Copyright (C) 2007 Free Software Foundation, Inc. \n Everyone is permitted to copy and distribute verbatim copies\n of this license document, but changing it is not allowed.\n\n Preamble\n\n The GNU General Public License is a free, copyleft license for\nsoftware and other kinds of works.\n\n The licenses for most software and other practical works are designed\nto take away your freedom to share and change the works. By contrast,\nthe GNU General Public License is intended to guarantee your freedom to\nshare and change all versions of a program--to make sure it remains free\nsoftware for all its users. We, the Free Software Foundation, use the\nGNU General Public License for most of our software; it applies also to\nany other work released this way by its authors. You can apply it to\nyour programs, too.\n\n When we speak of free software, we are referring to freedom, not\nprice. Our General Public Licenses are designed to make sure that you\nhave the freedom to distribute copies of free software (and charge for\nthem if you wish), that you receive source code or can get it if you\nwant it, that you can change the software or use pieces of it in new\nfree programs, and that you know you can do these things.\n\n To protect your rights, we need to prevent others from denying you\nthese rights or asking you to surrender the rights. Therefore, you have\ncertain responsibilities if you distribute copies of the software, or if\nyou modify it: responsibilities to respect the freedom of others.\n\n For example, if you distribute copies of such a program, whether\ngratis or for a fee, you must pass on to the recipients the same\nfreedoms that you received. You must make sure that they, too, receive\nor can get the source code. And you must show them these terms so they\nknow their rights.\n\n Developers that use the GNU GPL protect your rights with two steps:\n(1) assert copyright on the software, and (2) offer you this License\ngiving you legal permission to copy, distribute and/or modify it.\n\n For the developers' and authors' protection, the GPL clearly explains\nthat there is no warranty for this free software. For both users' and\nauthors' sake, the GPL requires that modified versions be marked as\nchanged, so that their problems will not be attributed erroneously to\nauthors of previous versions.\n\n Some devices are designed to deny users access to install or run\nmodified versions of the software inside them, although the manufacturer\ncan do so. This is fundamentally incompatible with the aim of\nprotecting users' freedom to change the software. The systematic\npattern of such abuse occurs in the area of products for individuals to\nuse, which is precisely where it is most unacceptable. Therefore, we\nhave designed this version of the GPL to prohibit the practice for those\nproducts. If such problems arise substantially in other domains, we\nstand ready to extend this provision to those domains in future versions\nof the GPL, as needed to protect the freedom of users.\n\n Finally, every program is threatened constantly by software patents.\nStates should not allow patents to restrict development and use of\nsoftware on general-purpose computers, but in those that do, we wish to\navoid the special danger that patents applied to a free program could\nmake it effectively proprietary. To prevent this, the GPL assures that\npatents cannot be used to render the program non-free.\n\n The precise terms and conditions for copying, distribution and\nmodification follow.\n\n TERMS AND CONDITIONS\n\n 0. Definitions.\n\n \"This License\" refers to version 3 of the GNU General Public License.\n\n \"Copyright\" also means copyright-like laws that apply to other kinds of\nworks, such as semiconductor masks.\n\n \"The Program\" refers to any copyrightable work licensed under this\nLicense. Each licensee is addressed as \"you\". \"Licensees\" and\n\"recipients\" may be individuals or organizations.\n\n To \"modify\" a work means to copy from or adapt all or part of the work\nin a fashion requiring copyright permission, other than the making of an\nexact copy. The resulting work is called a \"modified version\" of the\nearlier work or a work \"based on\" the earlier work.\n\n A \"covered work\" means either the unmodified Program or a work based\non the Program.\n\n To \"propagate\" a work means to do anything with it that, without\npermission, would make you directly or secondarily liable for\ninfringement under applicable copyright law, except executing it on a\ncomputer or modifying a private copy. Propagation includes copying,\ndistribution (with or without modification), making available to the\npublic, and in some countries other activities as well.\n\n To \"convey\" a work means any kind of propagation that enables other\nparties to make or receive copies. Mere interaction with a user through\na computer network, with no transfer of a copy, is not conveying.\n\n An interactive user interface displays \"Appropriate Legal Notices\"\nto the extent that it includes a convenient and prominently visible\nfeature that (1) displays an appropriate copyright notice, and (2)\ntells the user that there is no warranty for the work (except to the\nextent that warranties are provided), that licensees may convey the\nwork under this License, and how to view a copy of this License. If\nthe interface presents a list of user commands or options, such as a\nmenu, a prominent item in the list meets this criterion.\n\n 1. Source Code.\n\n The \"source code\" for a work means the preferred form of the work\nfor making modifications to it. \"Object code\" means any non-source\nform of a work.\n\n A \"Standard Interface\" means an interface that either is an official\nstandard defined by a recognized standards body, or, in the case of\ninterfaces specified for a particular programming language, one that\nis widely used among developers working in that language.\n\n The \"System Libraries\" of an executable work include anything, other\nthan the work as a whole, that (a) is included in the normal form of\npackaging a Major Component, but which is not part of that Major\nComponent, and (b) serves only to enable use of the work with that\nMajor Component, or to implement a Standard Interface for which an\nimplementation is available to the public in source code form. A\n\"Major Component\", in this context, means a major essential component\n(kernel, window system, and so on) of the specific operating system\n(if any) on which the executable work runs, or a compiler used to\nproduce the work, or an object code interpreter used to run it.\n\n The \"Corresponding Source\" for a work in object code form means all\nthe source code needed to generate, install, and (for an executable\nwork) run the object code and to modify the work, including scripts to\ncontrol those activities. However, it does not include the work's\nSystem Libraries, or general-purpose tools or generally available free\nprograms which are used unmodified in performing those activities but\nwhich are not part of the work. For example, Corresponding Source\nincludes interface definition files associated with source files for\nthe work, and the source code for shared libraries and dynamically\nlinked subprograms that the work is specifically designed to require,\nsuch as by intimate data communication or control flow between those\nsubprograms and other parts of the work.\n\n The Corresponding Source need not include anything that users\ncan regenerate automatically from other parts of the Corresponding\nSource.\n\n The Corresponding Source for a work in source code form is that\nsame work.\n\n 2. Basic Permissions.\n\n All rights granted under this License are granted for the term of\ncopyright on the Program, and are irrevocable provided the stated\nconditions are met. This License explicitly affirms your unlimited\npermission to run the unmodified Program. The output from running a\ncovered work is covered by this License only if the output, given its\ncontent, constitutes a covered work. This License acknowledges your\nrights of fair use or other equivalent, as provided by copyright law.\n\n You may make, run and propagate covered works that you do not\nconvey, without conditions so long as your license otherwise remains\nin force. You may convey covered works to others for the sole purpose\nof having them make modifications exclusively for you, or provide you\nwith facilities for running those works, provided that you comply with\nthe terms of this License in conveying all material for which you do\nnot control copyright. Those thus making or running the covered works\nfor you must do so exclusively on your behalf, under your direction\nand control, on terms that prohibit them from making any copies of\nyour copyrighted material outside their relationship with you.\n\n Conveying under any other circumstances is permitted solely under\nthe conditions stated below. Sublicensing is not allowed; section 10\nmakes it unnecessary.\n\n 3. Protecting Users' Legal Rights From Anti-Circumvention Law.\n\n No covered work shall be deemed part of an effective technological\nmeasure under any applicable law fulfilling obligations under article\n11 of the WIPO copyright treaty adopted on 20 December 1996, or\nsimilar laws prohibiting or restricting circumvention of such\nmeasures.\n\n When you convey a covered work, you waive any legal power to forbid\ncircumvention of technological measures to the extent such circumvention\nis effected by exercising rights under this License with respect to\nthe covered work, and you disclaim any intention to limit operation or\nmodification of the work as a means of enforcing, against the work's\nusers, your or third parties' legal rights to forbid circumvention of\ntechnological measures.\n\n 4. Conveying Verbatim Copies.\n\n You may convey verbatim copies of the Program's source code as you\nreceive it, in any medium, provided that you conspicuously and\nappropriately publish on each copy an appropriate copyright notice;\nkeep intact all notices stating that this License and any\nnon-permissive terms added in accord with section 7 apply to the code;\nkeep intact all notices of the absence of any warranty; and give all\nrecipients a copy of this License along with the Program.\n\n You may charge any price or no price for each copy that you convey,\nand you may offer support or warranty protection for a fee.\n\n 5. Conveying Modified Source Versions.\n\n You may convey a work based on the Program, or the modifications to\nproduce it from the Program, in the form of source code under the\nterms of section 4, provided that you also meet all of these conditions:\n\n a) The work must carry prominent notices stating that you modified\n it, and giving a relevant date.\n\n b) The work must carry prominent notices stating that it is\n released under this License and any conditions added under section\n 7. This requirement modifies the requirement in section 4 to\n \"keep intact all notices\".\n\n c) You must license the entire work, as a whole, under this\n License to anyone who comes into possession of a copy. This\n License will therefore apply, along with any applicable section 7\n additional terms, to the whole of the work, and all its parts,\n regardless of how they are packaged. This License gives no\n permission to license the work in any other way, but it does not\n invalidate such permission if you have separately received it.\n\n d) If the work has interactive user interfaces, each must display\n Appropriate Legal Notices; however, if the Program has interactive\n interfaces that do not display Appropriate Legal Notices, your\n work need not make them do so.\n\n A compilation of a covered work with other separate and independent\nworks, which are not by their nature extensions of the covered work,\nand which are not combined with it such as to form a larger program,\nin or on a volume of a storage or distribution medium, is called an\n\"aggregate\" if the compilation and its resulting copyright are not\nused to limit the access or legal rights of the compilation's users\nbeyond what the individual works permit. Inclusion of a covered work\nin an aggregate does not cause this License to apply to the other\nparts of the aggregate.\n\n 6. Conveying Non-Source Forms.\n\n You may convey a covered work in object code form under the terms\nof sections 4 and 5, provided that you also convey the\nmachine-readable Corresponding Source under the terms of this License,\nin one of these ways:\n\n a) Convey the object code in, or embodied in, a physical product\n (including a physical distribution medium), accompanied by the\n Corresponding Source fixed on a durable physical medium\n customarily used for software interchange.\n\n b) Convey the object code in, or embodied in, a physical product\n (including a physical distribution medium), accompanied by a\n written offer, valid for at least three years and valid for as\n long as you offer spare parts or customer support for that product\n model, to give anyone who possesses the object code either (1) a\n copy of the Corresponding Source for all the software in the\n product that is covered by this License, on a durable physical\n medium customarily used for software interchange, for a price no\n more than your reasonable cost of physically performing this\n conveying of source, or (2) access to copy the\n Corresponding Source from a network server at no charge.\n\n c) Convey individual copies of the object code with a copy of the\n written offer to provide the Corresponding Source. This\n alternative is allowed only occasionally and noncommercially, and\n only if you received the object code with such an offer, in accord\n with subsection 6b.\n\n d) Convey the object code by offering access from a designated\n place (gratis or for a charge), and offer equivalent access to the\n Corresponding Source in the same way through the same place at no\n further charge. You need not require recipients to copy the\n Corresponding Source along with the object code. If the place to\n copy the object code is a network server, the Corresponding Source\n may be on a different server (operated by you or a third party)\n that supports equivalent copying facilities, provided you maintain\n clear directions next to the object code saying where to find the\n Corresponding Source. Regardless of what server hosts the\n Corresponding Source, you remain obligated to ensure that it is\n available for as long as needed to satisfy these requirements.\n\n e) Convey the object code using peer-to-peer transmission, provided\n you inform other peers where the object code and Corresponding\n Source of the work are being offered to the general public at no\n charge under subsection 6d.\n\n A separable portion of the object code, whose source code is excluded\nfrom the Corresponding Source as a System Library, need not be\nincluded in conveying the object code work.\n\n A \"User Product\" is either (1) a \"consumer product\", which means any\ntangible personal property which is normally used for personal, family,\nor household purposes, or (2) anything designed or sold for incorporation\ninto a dwelling. In determining whether a product is a consumer product,\ndoubtful cases shall be resolved in favor of coverage. For a particular\nproduct received by a particular user, \"normally used\" refers to a\ntypical or common use of that class of product, regardless of the status\nof the particular user or of the way in which the particular user\nactually uses, or expects or is expected to use, the product. A product\nis a consumer product regardless of whether the product has substantial\ncommercial, industrial or non-consumer uses, unless such uses represent\nthe only significant mode of use of the product.\n\n \"Installation Information\" for a User Product means any methods,\nprocedures, authorization keys, or other information required to install\nand execute modified versions of a covered work in that User Product from\na modified version of its Corresponding Source. The information must\nsuffice to ensure that the continued functioning of the modified object\ncode is in no case prevented or interfered with solely because\nmodification has been made.\n\n If you convey an object code work under this section in, or with, or\nspecifically for use in, a User Product, and the conveying occurs as\npart of a transaction in which the right of possession and use of the\nUser Product is transferred to the recipient in perpetuity or for a\nfixed term (regardless of how the transaction is characterized), the\nCorresponding Source conveyed under this section must be accompanied\nby the Installation Information. But this requirement does not apply\nif neither you nor any third party retains the ability to install\nmodified object code on the User Product (for example, the work has\nbeen installed in ROM).\n\n The requirement to provide Installation Information does not include a\nrequirement to continue to provide support service, warranty, or updates\nfor a work that has been modified or installed by the recipient, or for\nthe User Product in which it has been modified or installed. Access to a\nnetwork may be denied when the modification itself materially and\nadversely affects the operation of the network or violates the rules and\nprotocols for communication across the network.\n\n Corresponding Source conveyed, and Installation Information provided,\nin accord with this section must be in a format that is publicly\ndocumented (and with an implementation available to the public in\nsource code form), and must require no special password or key for\nunpacking, reading or copying.\n\n 7. Additional Terms.\n\n \"Additional permissions\" are terms that supplement the terms of this\nLicense by making exceptions from one or more of its conditions.\nAdditional permissions that are applicable to the entire Program shall\nbe treated as though they were included in this License, to the extent\nthat they are valid under applicable law. If additional permissions\napply only to part of the Program, that part may be used separately\nunder those permissions, but the entire Program remains governed by\nthis License without regard to the additional permissions.\n\n When you convey a copy of a covered work, you may at your option\nremove any additional permissions from that copy, or from any part of\nit. (Additional permissions may be written to require their own\nremoval in certain cases when you modify the work.) You may place\nadditional permissions on material, added by you to a covered work,\nfor which you have or can give appropriate copyright permission.\n\n Notwithstanding any other provision of this License, for material you\nadd to a covered work, you may (if authorized by the copyright holders of\nthat material) supplement the terms of this License with terms:\n\n a) Disclaiming warranty or limiting liability differently from the\n terms of sections 15 and 16 of this License; or\n\n b) Requiring preservation of specified reasonable legal notices or\n author attributions in that material or in the Appropriate Legal\n Notices displayed by works containing it; or\n\n c) Prohibiting misrepresentation of the origin of that material, or\n requiring that modified versions of such material be marked in\n reasonable ways as different from the original version; or\n\n d) Limiting the use for publicity purposes of names of licensors or\n authors of the material; or\n\n e) Declining to grant rights under trademark law for use of some\n trade names, trademarks, or service marks; or\n\n f) Requiring indemnification of licensors and authors of that\n material by anyone who conveys the material (or modified versions of\n it) with contractual assumptions of liability to the recipient, for\n any liability that these contractual assumptions directly impose on\n those licensors and authors.\n\n All other non-permissive additional terms are considered \"further\nrestrictions\" within the meaning of section 10. If the Program as you\nreceived it, or any part of it, contains a notice stating that it is\ngoverned by this License along with a term that is a further\nrestriction, you may remove that term. If a license document contains\na further restriction but permits relicensing or conveying under this\nLicense, you may add to a covered work material governed by the terms\nof that license document, provided that the further restriction does\nnot survive such relicensing or conveying.\n\n If you add terms to a covered work in accord with this section, you\nmust place, in the relevant source files, a statement of the\nadditional terms that apply to those files, or a notice indicating\nwhere to find the applicable terms.\n\n Additional terms, permissive or non-permissive, may be stated in the\nform of a separately written license, or stated as exceptions;\nthe above requirements apply either way.\n\n 8. Termination.\n\n You may not propagate or modify a covered work except as expressly\nprovided under this License. Any attempt otherwise to propagate or\nmodify it is void, and will automatically terminate your rights under\nthis License (including any patent licenses granted under the third\nparagraph of section 11).\n\n However, if you cease all violation of this License, then your\nlicense from a particular copyright holder is reinstated (a)\nprovisionally, unless and until the copyright holder explicitly and\nfinally terminates your license, and (b) permanently, if the copyright\nholder fails to notify you of the violation by some reasonable means\nprior to 60 days after the cessation.\n\n Moreover, your license from a particular copyright holder is\nreinstated permanently if the copyright holder notifies you of the\nviolation by some reasonable means, this is the first time you have\nreceived notice of violation of this License (for any work) from that\ncopyright holder, and you cure the violation prior to 30 days after\nyour receipt of the notice.\n\n Termination of your rights under this section does not terminate the\nlicenses of parties who have received copies or rights from you under\nthis License. If your rights have been terminated and not permanently\nreinstated, you do not qualify to receive new licenses for the same\nmaterial under section 10.\n\n 9. Acceptance Not Required for Having Copies.\n\n You are not required to accept this License in order to receive or\nrun a copy of the Program. Ancillary propagation of a covered work\noccurring solely as a consequence of using peer-to-peer transmission\nto receive a copy likewise does not require acceptance. However,\nnothing other than this License grants you permission to propagate or\nmodify any covered work. These actions infringe copyright if you do\nnot accept this License. Therefore, by modifying or propagating a\ncovered work, you indicate your acceptance of this License to do so.\n\n 10. Automatic Licensing of Downstream Recipients.\n\n Each time you convey a covered work, the recipient automatically\nreceives a license from the original licensors, to run, modify and\npropagate that work, subject to this License. You are not responsible\nfor enforcing compliance by third parties with this License.\n\n An \"entity transaction\" is a transaction transferring control of an\norganization, or substantially all assets of one, or subdividing an\norganization, or merging organizations. If propagation of a covered\nwork results from an entity transaction, each party to that\ntransaction who receives a copy of the work also receives whatever\nlicenses to the work the party's predecessor in interest had or could\ngive under the previous paragraph, plus a right to possession of the\nCorresponding Source of the work from the predecessor in interest, if\nthe predecessor has it or can get it with reasonable efforts.\n\n You may not impose any further restrictions on the exercise of the\nrights granted or affirmed under this License. For example, you may\nnot impose a license fee, royalty, or other charge for exercise of\nrights granted under this License, and you may not initiate litigation\n(including a cross-claim or counterclaim in a lawsuit) alleging that\nany patent claim is infringed by making, using, selling, offering for\nsale, or importing the Program or any portion of it.\n\n 11. Patents.\n\n A \"contributor\" is a copyright holder who authorizes use under this\nLicense of the Program or a work on which the Program is based. The\nwork thus licensed is called the contributor's \"contributor version\".\n\n A contributor's \"essential patent claims\" are all patent claims\nowned or controlled by the contributor, whether already acquired or\nhereafter acquired, that would be infringed by some manner, permitted\nby this License, of making, using, or selling its contributor version,\nbut do not include claims that would be infringed only as a\nconsequence of further modification of the contributor version. For\npurposes of this definition, \"control\" includes the right to grant\npatent sublicenses in a manner consistent with the requirements of\nthis License.\n\n Each contributor grants you a non-exclusive, worldwide, royalty-free\npatent license under the contributor's essential patent claims, to\nmake, use, sell, offer for sale, import and otherwise run, modify and\npropagate the contents of its contributor version.\n\n In the following three paragraphs, a \"patent license\" is any express\nagreement or commitment, however denominated, not to enforce a patent\n(such as an express permission to practice a patent or covenant not to\nsue for patent infringement). To \"grant\" such a patent license to a\nparty means to make such an agreement or commitment not to enforce a\npatent against the party.\n\n If you convey a covered work, knowingly relying on a patent license,\nand the Corresponding Source of the work is not available for anyone\nto copy, free of charge and under the terms of this License, through a\npublicly available network server or other readily accessible means,\nthen you must either (1) cause the Corresponding Source to be so\navailable, or (2) arrange to deprive yourself of the benefit of the\npatent license for this particular work, or (3) arrange, in a manner\nconsistent with the requirements of this License, to extend the patent\nlicense to downstream recipients. \"Knowingly relying\" means you have\nactual knowledge that, but for the patent license, your conveying the\ncovered work in a country, or your recipient's use of the covered work\nin a country, would infringe one or more identifiable patents in that\ncountry that you have reason to believe are valid.\n\n If, pursuant to or in connection with a single transaction or\narrangement, you convey, or propagate by procuring conveyance of, a\ncovered work, and grant a patent license to some of the parties\nreceiving the covered work authorizing them to use, propagate, modify\nor convey a specific copy of the covered work, then the patent license\nyou grant is automatically extended to all recipients of the covered\nwork and works based on it.\n\n A patent license is \"discriminatory\" if it does not include within\nthe scope of its coverage, prohibits the exercise of, or is\nconditioned on the non-exercise of one or more of the rights that are\nspecifically granted under this License. You may not convey a covered\nwork if you are a party to an arrangement with a third party that is\nin the business of distributing software, under which you make payment\nto the third party based on the extent of your activity of conveying\nthe work, and under which the third party grants, to any of the\nparties who would receive the covered work from you, a discriminatory\npatent license (a) in connection with copies of the covered work\nconveyed by you (or copies made from those copies), or (b) primarily\nfor and in connection with specific products or compilations that\ncontain the covered work, unless you entered into that arrangement,\nor that patent license was granted, prior to 28 March 2007.\n\n Nothing in this License shall be construed as excluding or limiting\nany implied license or other defenses to infringement that may\notherwise be available to you under applicable patent law.\n\n 12. No Surrender of Others' Freedom.\n\n If conditions are imposed on you (whether by court order, agreement or\notherwise) that contradict the conditions of this License, they do not\nexcuse you from the conditions of this License. If you cannot convey a\ncovered work so as to satisfy simultaneously your obligations under this\nLicense and any other pertinent obligations, then as a consequence you may\nnot convey it at all. For example, if you agree to terms that obligate you\nto collect a royalty for further conveying from those to whom you convey\nthe Program, the only way you could satisfy both those terms and this\nLicense would be to refrain entirely from conveying the Program.\n\n 13. Use with the GNU Affero General Public License.\n\n Notwithstanding any other provision of this License, you have\npermission to link or combine any covered work with a work licensed\nunder version 3 of the GNU Affero General Public License into a single\ncombined work, and to convey the resulting work. The terms of this\nLicense will continue to apply to the part which is the covered work,\nbut the special requirements of the GNU Affero General Public License,\nsection 13, concerning interaction through a network will apply to the\ncombination as such.\n\n 14. Revised Versions of this License.\n\n The Free Software Foundation may publish revised and/or new versions of\nthe GNU General Public License from time to time. Such new versions will\nbe similar in spirit to the present version, but may differ in detail to\naddress new problems or concerns.\n\n Each version is given a distinguishing version number. If the\nProgram specifies that a certain numbered version of the GNU General\nPublic License \"or any later version\" applies to it, you have the\noption of following the terms and conditions either of that numbered\nversion or of any later version published by the Free Software\nFoundation. If the Program does not specify a version number of the\nGNU General Public License, you may choose any version ever published\nby the Free Software Foundation.\n\n If the Program specifies that a proxy can decide which future\nversions of the GNU General Public License can be used, that proxy's\npublic statement of acceptance of a version permanently authorizes you\nto choose that version for the Program.\n\n Later license versions may give you additional or different\npermissions. However, no additional obligations are imposed on any\nauthor or copyright holder as a result of your choosing to follow a\nlater version.\n\n 15. Disclaimer of Warranty.\n\n THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY\nAPPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT\nHOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM \"AS IS\" WITHOUT WARRANTY\nOF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO,\nTHE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR\nPURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM\nIS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF\nALL NECESSARY SERVICING, REPAIR OR CORRECTION.\n\n 16. Limitation of Liability.\n\n IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING\nWILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS\nTHE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY\nGENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE\nUSE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF\nDATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD\nPARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS),\nEVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF\nSUCH DAMAGES.\n\n 17. Interpretation of Sections 15 and 16.\n\n If the disclaimer of warranty and limitation of liability provided\nabove cannot be given local legal effect according to their terms,\nreviewing courts shall apply local law that most closely approximates\nan absolute waiver of all civil liability in connection with the\nProgram, unless a warranty or assumption of liability accompanies a\ncopy of the Program in return for a fee.\n\n END OF TERMS AND CONDITIONS\n\n How to Apply These Terms to Your New Programs\n\n If you develop a new program, and you want it to be of the greatest\npossible use to the public, the best way to achieve this is to make it\nfree software which everyone can redistribute and change under these terms.\n\n To do so, attach the following notices to the program. It is safest\nto attach them to the start of each source file to most effectively\nstate the exclusion of warranty; and each file should have at least\nthe \"copyright\" line and a pointer to where the full notice is found.\n\n \n Copyright (C) \n\n This program is free software: you can redistribute it and/or modify\n it under the terms of the GNU General Public License as published by\n the Free Software Foundation, either version 3 of the License, or\n (at your option) any later version.\n\n This program is distributed in the hope that it will be useful,\n but WITHOUT ANY WARRANTY; without even the implied warranty of\n MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n GNU General Public License for more details.\n\n You should have received a copy of the GNU General Public License\n along with this program. If not, see .\n\nAlso add information on how to contact you by electronic and paper mail.\n\n If the program does terminal interaction, make it output a short\nnotice like this when it starts in an interactive mode:\n\n Copyright (C) \n This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'.\n This is free software, and you are welcome to redistribute it\n under certain conditions; type `show c' for details.\n\nThe hypothetical commands `show w' and `show c' should show the appropriate\nparts of the General Public License. Of course, your program's commands\nmight be different; for a GUI interface, you would use an \"about box\".\n\n You should also get your employer (if you work as a programmer) or school,\nif any, to sign a \"copyright disclaimer\" for the program, if necessary.\nFor more information on this, and how to apply and follow the GNU GPL, see\n.\n\n The GNU General Public License does not permit incorporating your program\ninto proprietary programs. If your program is a subroutine library, you\nmay consider it more useful to permit linking proprietary applications with\nthe library. If this is what you want to do, use the GNU Lesser General\nPublic License instead of this License. But first, please read\n."
},
{
"path": "README.md",
"content": "# Model Predictive Control for Quadrotors with extension to Perception-Aware MPC\nModel Predictive Control for Quadrotors by \"Robotics and Perception Group\" at the Dep. of Informatics, \"University of Zurich\", and Dep. of Neuroinformatics, ETH and University of Zurich.\n\nThis MPC is intended to be used with https://github.com/uzh-rpg/rpg_quadrotor_control.\nIt is available with the extension to be used as a \"Perception Aware Model Predictive Controller\" (**PAMPC**).\n\n[**Check out our YouTube-Video, showing PAMPC in Action**](https://www.youtube.com/watch?v=9vaj829vE18)\n[](https://www.youtube.com/watch?v=9vaj829vE18)\n\n## Publication\nIf you use this code in an academic context, please cite the following [IROS 2018 paper](http://rpg.ifi.uzh.ch/docs/IROS18_Falanga.pdf).\n\nDavide Falanga, Philipp Foehn, Peng Lu, Davide Scaramuzza: **PAMPC: Perception-Aware Model Predictive Control for Quadrotors**, IEEE/RSJ Int. Conf. Intell. Robot. Syst. (IROS), 2018.\n\n```\n@InProceedings{Falanga2018\n author = {Falanga, Davide and Foehn, Philipp and Lu, Peng and Scaramuzza, Davide},\n title = {{PAMPC}: Perception-Aware Model Predictive Control for Quadrotors},\n booktitle = {IEEE/RSJ Int. Conf. Intell. Robot. Syst. (IROS)},\n year = {2018}\n}\n```\n\n\n## License\n\nCopyright (C) 2017-2018 Philipp Foehn, Robotics and Perception Group, University of Zurich\n\nThe RPG MPC repository provides packages that are intended to be used with [RPG Quadrotor Control](https://github.com/uzh-rpg/rpg_quadrotor_control) and [ROS](http://www.ros.org/). \nThis code has been tested with ROS kinetic on Ubuntu 16.04.\nThis is research code, expect that it changes often and any fitness for a particular purpose is disclaimed.\nFor a commercial license, please contact [Davide Scaramuzza](http://rpg.ifi.uzh.ch/people_scaramuzza.html).\n\n```\nThis program is free software: you can redistribute it and/or modify\nit under the terms of the GNU General Public License as published by\nthe Free Software Foundation, either version 3 of the License, or\n(at your option) any later version.\n\nThis program is distributed in the hope that it will be useful,\nbut WITHOUT ANY WARRANTY; without even the implied warranty of\nMERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\nGNU General Public License for more details.\n\nYou should have received a copy of the GNU General Public License\nalong with this program. If not, see .\n```\n\nThis work depends on the ACADO Toolkit, developed by the Optimization in Engineering Center (OPTEC) under supervision of Moritz Diehl. Licensing detail can be found on the [ACADO licensing page](http://acado.github.io/licensing.html). It is released under GNU Lesser General Public License as published by the Free Software Foundation, version 3.\nACADO uses qpOASES by Hans Joachim Ferreau et al., released under GPL v2.1.\n\n## Installation, Usage, and Documentation\nFor detailed instructions on installation, usage, and documentation, please head to our [Wiki](../../wiki).\n\nQuick Links:\n- [Installation](../../wiki/Installation)\n- [Basic Usage](../../wiki/Basic-Usage)\n- [Code Structure](../../wiki/Code-Structure)\n- [License](../../wiki/License)\n\n## Code Structure Overview\nThe whole MPC is based on ACADO (http://acado.github.io/).\nACADO's C++ interface is used to describe the quadrotor model and parameters for transcription into a quadratic program, which is then solved with qpOASES (https://projects.coin-or.org/qpOASES). To compile and run, none of these dependencies are needed, since the generated code is already included in this repository. To modify the model and solver options, please install ACADO from http://acado.github.io/install_linux.html.\n\nThe code is organized as follows:\n\n### Solver `mpc_solver`\n\nThe auto-generated model, transcription, and solver is built as a library called `mpc_solver`.\nThis library consist of purely auto-generated code with nomenclature, code-style and structure as used in ACADO.\n\n### Wrapper `mpc_wrapper`\n\nTo wrap this into a standard interface, the library `mpc_wrapper` is used.\nACADO uses arrays with column-major format to store matrices, since this is rather inconvenient, `mpc_wrapper` provides interfaces using Eigen objects by mapping the arrays.\n* It is written to be compatible even with changing model descriptions in the `mpc_solver`.\n* It should prevent the most common runtime errors caused by the user by doing some initialization and checks with hardcoded parameters, which are overwritten in normal usage.\n\n### Controller `mpc_controller`\n\nTo provide not only a solver, but a full controller, `mpc_controller` is a library based on the previous `mpc_solver` and `mpc_wrapper`, providing all funcitonality to implement a controller with minimal effort. It provides two main execution modes:\n\n* **Embedded**: The mpc_controller can be included in any oder controller class or copilot by generating an object with the default constructor \"MPC::MpcController controller();\". It still registers node-handles to publish the predicted trajectory after each control cycle, but does nothing more. It only provides the interfaces of `ControllerBase` as in the `LargeAngleController` but without a specific class for parameters.\n\n* **Standalone (not yet provided)**: The `mpc_controller` object can be passed node-handles or creates its own, registers multiple subscribers and publishers to get a state estimate as well as the control goals (pose or trajectory) and registers a timer to run a control loop. It works as a full standalone controller which can be used with the oneliner: `MPC::MpcController controller(ros::NodeHandle(), ros::NodeHandle(\"~\"));` as in `test/control_node.cpp` and `launch/mpc_controller.launch`.\n"
},
{
"path": "externals/qpoases/EXAMPLES/example1.cpp",
"content": "/*\n *\tThis file is part of qpOASES.\n *\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\n *\tCopyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.\n *\n *\tqpOASES is free software; you can redistribute it and/or\n *\tmodify it under the terms of the GNU Lesser General Public\n *\tLicense as published by the Free Software Foundation; either\n *\tversion 2.1 of the License, or (at your option) any later version.\n *\n *\tqpOASES is distributed in the hope that it will be useful,\n *\tbut WITHOUT ANY WARRANTY; without even the implied warranty of\n *\tMERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU\n *\tLesser General Public License for more details.\n *\n *\tYou should have received a copy of the GNU Lesser General Public\n *\tLicense along with qpOASES; if not, write to the Free Software\n *\tFoundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA\n *\n */\n\n\n/**\n *\t\\file EXAMPLES/example1.cpp\n *\t\\author Hans Joachim Ferreau\n *\t\\version 1.3embedded\n *\t\\date 2007-2008\n *\n *\tVery simple example for testing qpOASES (using QProblem class).\n */\n\n\n#include \n\n\n/** Example for qpOASES main function using the QProblem class. */\nint main( )\n{\n\t/* Setup data of first QP. */\n\treal_t H[2*2] = { 1.0, 0.0, 0.0, 0.5 };\n\treal_t A[1*2] = { 1.0, 1.0 };\n\treal_t g[2] = { 1.5, 1.0 };\n\treal_t lb[2] = { 0.5, -2.0 };\n\treal_t ub[2] = { 5.0, 2.0 };\n\treal_t lbA[1] = { -1.0 };\n\treal_t ubA[1] = { 2.0 };\n\n\t/* Setup data of second QP. */\n\treal_t g_new[2] = { 1.0, 1.5 };\n\treal_t lb_new[2] = { 0.0, -1.0 };\n\treal_t ub_new[2] = { 5.0, -0.5 };\n\treal_t lbA_new[1] = { -2.0 };\n\treal_t ubA_new[1] = { 1.0 };\n\n\n\t/* Setting up QProblem object. */\n\tQProblem example( 2,1 );\n\n\t/* Solve first QP. */\n\tint nWSR = 10;\n\texample.init( H,g,A,lb,ub,lbA,ubA, nWSR,0 );\n\n\t/* Solve second QP. */\n\tnWSR = 10;\n\texample.hotstart( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,0 );\n\n\treturn 0;\n}\n\n\n/*\n *\tend of file\n */\n"
},
{
"path": "externals/qpoases/EXAMPLES/example1b.cpp",
"content": "/*\n *\tThis file is part of qpOASES.\n *\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\n *\tCopyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.\n *\n *\tqpOASES is free software; you can redistribute it and/or\n *\tmodify it under the terms of the GNU Lesser General Public\n *\tLicense as published by the Free Software Foundation; either\n *\tversion 2.1 of the License, or (at your option) any later version.\n *\n *\tqpOASES is distributed in the hope that it will be useful,\n *\tbut WITHOUT ANY WARRANTY; without even the implied warranty of\n *\tMERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU\n *\tLesser General Public License for more details.\n *\n *\tYou should have received a copy of the GNU Lesser General Public\n *\tLicense along with qpOASES; if not, write to the Free Software\n *\tFoundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA\n *\n */\n\n\n/**\n *\t\\file EXAMPLES/example1b.cpp\n *\t\\author Hans Joachim Ferreau\n *\t\\version 1.3\n *\t\\date 2007-2008\n *\n *\tVery simple example for testing qpOASES using the QProblemB class.\n */\n\n\n#include \n\n\n/** Example for qpOASES main function using the QProblemB class. */\nint main( )\n{\n\t/* Setup data of first QP. */\n\treal_t H[2*2] = { 1.0, 0.0, 0.0, 0.5 };\n\treal_t g[2] = { 1.5, 1.0 };\n\treal_t lb[2] = { 0.5, -2.0 };\n\treal_t ub[2] = { 5.0, 2.0 };\n\n\t/* Setup data of second QP. */\n\treal_t g_new[2] = { 1.0, 1.5 };\n\treal_t lb_new[2] = { 0.0, -1.0 };\n\treal_t ub_new[2] = { 5.0, -0.5 };\n\n\n\t/* Setting up QProblemB object. */\n\tQProblemB example( 2 );\n\n\t/* Solve first QP. */\n\tint nWSR = 10;\n\texample.init( H,g,lb,ub, nWSR,0 );\n\n\t/* Solve second QP. */\n\tnWSR = 10;\n\texample.hotstart( g_new,lb_new,ub_new, nWSR,0 );\n\n\treturn 0;\n}\n\n\n/*\n *\tend of file\n */\n"
},
{
"path": "externals/qpoases/INCLUDE/Bounds.hpp",
"content": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.\r\n *\r\n *\tqpOASES is free software; you can redistribute it and/or\r\n *\tmodify it under the terms of the GNU Lesser General Public\r\n *\tLicense as published by the Free Software Foundation; either\r\n *\tversion 2.1 of the License, or (at your option) any later version.\r\n *\r\n *\tqpOASES is distributed in the hope that it will be useful,\r\n *\tbut WITHOUT ANY WARRANTY; without even the implied warranty of\r\n *\tMERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU\r\n *\tLesser General Public License for more details.\r\n *\r\n *\tYou should have received a copy of the GNU Lesser General Public\r\n *\tLicense along with qpOASES; if not, write to the Free Software\r\n *\tFoundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA\r\n *\r\n */\r\n\r\n\r\n/**\r\n *\t\\file INCLUDE/Bounds.hpp\r\n *\t\\author Hans Joachim Ferreau\r\n *\t\\version 1.3embedded\r\n *\t\\date 2007-2008\r\n *\r\n *\tDeclaration of the Bounds class designed to manage working sets of\r\n *\tbounds within a QProblem.\r\n */\r\n\r\n\r\n#ifndef QPOASES_BOUNDS_HPP\r\n#define QPOASES_BOUNDS_HPP\r\n\r\n\r\n#include \r\n\r\n\r\n\r\n/** This class manages working sets of bounds by storing\r\n *\tindex sets and other status information.\r\n *\r\n *\t\\author Hans Joachim Ferreau\r\n *\t\\version 1.3embedded\r\n *\t\\date 2007-2008\r\n */\r\nclass Bounds : public SubjectTo\r\n{\r\n\t/*\r\n\t *\tPUBLIC MEMBER FUNCTIONS\r\n\t */\r\n\tpublic:\r\n\t\t/** Default constructor. */\r\n\t\tBounds( );\r\n\r\n\t\t/** Copy constructor (deep copy). */\r\n\t\tBounds(\tconst Bounds& rhs\t/**< Rhs object. */\r\n\t\t\t\t);\r\n\r\n\t\t/** Destructor. */\r\n\t\t~Bounds( );\r\n\r\n\t\t/** Assignment operator (deep copy). */\r\n\t\tBounds& operator=(\tconst Bounds& rhs\t/**< Rhs object. */\r\n\t\t\t\t\t\t\t);\r\n\r\n\r\n\t\t/** Pseudo-constructor takes the number of bounds.\r\n\t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\treturnValue init(\tint n\t/**< Number of bounds. */\r\n\t\t\t\t\t\t\t);\r\n\r\n\r\n\t\t/** Initially adds number of a new (i.e. not yet in the list) bound to\r\n\t\t * given index set.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t \t\t\tRET_SETUP_BOUND_FAILED \\n\r\n\t\t\t\t\tRET_INDEX_OUT_OF_BOUNDS \\n\r\n\t\t\t\t\tRET_INVALID_ARGUMENTS */\r\n\t\treturnValue setupBound(\tint _number,\t\t\t\t\t/**< Number of new bound. */\r\n\t\t\t\t\t\t\t\tSubjectToStatus _status\t\t\t/**< Status of new bound. */\r\n\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Initially adds all numbers of new (i.e. not yet in the list) bounds to\r\n\t\t * to the index set of free bounds; the order depends on the SujectToType\r\n\t\t * of each index.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t \t\t\tRET_SETUP_BOUND_FAILED */\r\n\t\treturnValue setupAllFree( );\r\n\r\n\r\n\t\t/** Moves index of a bound from index list of fixed to that of free bounds.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t \t\t\tRET_MOVING_BOUND_FAILED \\n\r\n\t\t\t\t\tRET_INDEX_OUT_OF_BOUNDS */\r\n\t\treturnValue moveFixedToFree(\tint _number\t\t\t\t/**< Number of bound to be freed. */\r\n\t\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Moves index of a bound from index list of free to that of fixed bounds.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t \t\t\tRET_MOVING_BOUND_FAILED \\n\r\n\t\t\t\t\tRET_INDEX_OUT_OF_BOUNDS */\r\n\t\treturnValue moveFreeToFixed(\tint _number,\t\t\t/**< Number of bound to be fixed. */\r\n\t\t\t\t\t\t\t\t\t\tSubjectToStatus _status\t/**< Status of bound to be fixed. */\r\n\t\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Swaps the indices of two free bounds within the index set.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t \t\t\tRET_SWAPINDEX_FAILED */\r\n\t\treturnValue swapFree(\tint number1,\t\t\t\t\t/**< Number of first constraint or bound. */\r\n\t\t\t\t\t\t\t\tint number2\t\t\t\t\t\t/**< Number of second constraint or bound. */\r\n\t\t\t\t\t\t\t\t);\r\n\r\n\r\n\t\t/** Returns number of variables.\r\n\t\t *\t\\return Number of variables. */\r\n\t\tinline int getNV( ) const;\r\n\r\n\t\t/** Returns number of implicitly fixed variables.\r\n\t\t *\t\\return Number of implicitly fixed variables. */\r\n\t\tinline int getNFV( ) const;\r\n\r\n\t\t/** Returns number of bounded (but possibly free) variables.\r\n\t\t *\t\\return Number of bounded (but possibly free) variables. */\r\n\t\tinline int getNBV( ) const;\r\n\r\n\t\t/** Returns number of unbounded variables.\r\n\t\t *\t\\return Number of unbounded variables. */\r\n\t\tinline int getNUV( ) const;\r\n\r\n\r\n\t\t/** Sets number of implicitly fixed variables.\r\n\t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\tinline returnValue setNFV(\tint n\t/**< Number of implicitly fixed variables. */\r\n\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Sets number of bounded (but possibly free) variables.\r\n\t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\tinline returnValue setNBV(\tint n\t/**< Number of bounded (but possibly free) variables. */\r\n\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Sets number of unbounded variables.\r\n\t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\tinline returnValue setNUV(\tint n\t/**< Number of unbounded variables */\r\n\t\t\t\t\t\t\t\t\t);\r\n\r\n\r\n\t\t/** Returns number of free variables.\r\n\t\t *\t\\return Number of free variables. */\r\n\t\tinline int getNFR( );\r\n\r\n\t\t/** Returns number of fixed variables.\r\n\t\t *\t\\return Number of fixed variables. */\r\n\t\tinline int getNFX( );\r\n\r\n\r\n\t\t/** Returns a pointer to free variables index list.\r\n\t\t *\t\\return Pointer to free variables index list. */\r\n\t\tinline Indexlist* getFree( );\r\n\r\n\t\t/** Returns a pointer to fixed variables index list.\r\n\t\t *\t\\return Pointer to fixed variables index list. */\r\n\t\tinline Indexlist* getFixed( );\r\n\r\n\r\n\t/*\r\n\t *\tPROTECTED MEMBER VARIABLES\r\n\t */\r\n\tprotected:\r\n\t\tint nV;\t\t\t\t\t/**< Number of variables (nV = nFV + nBV + nUV). */\r\n\t\tint nFV;\t\t\t\t/**< Number of implicitly fixed variables. */\r\n\t\tint\tnBV;\t\t\t\t/**< Number of bounded (but possibly free) variables. */\r\n\t\tint nUV;\t\t\t\t/**< Number of unbounded variables. */\r\n\r\n\t\tIndexlist free;\t\t\t/**< Index list of free variables. */\r\n\t\tIndexlist fixed;\t\t/**< Index list of fixed variables. */\r\n};\r\n\r\n#include \r\n\r\n#endif\t/* QPOASES_BOUNDS_HPP */\r\n\r\n\r\n/*\r\n *\tend of file\r\n */\r\n"
},
{
"path": "externals/qpoases/INCLUDE/Constants.hpp",
"content": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.\r\n *\r\n *\tqpOASES is free software; you can redistribute it and/or\r\n *\tmodify it under the terms of the GNU Lesser General Public\r\n *\tLicense as published by the Free Software Foundation; either\r\n *\tversion 2.1 of the License, or (at your option) any later version.\r\n *\r\n *\tqpOASES is distributed in the hope that it will be useful,\r\n *\tbut WITHOUT ANY WARRANTY; without even the implied warranty of\r\n *\tMERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU\r\n *\tLesser General Public License for more details.\r\n *\r\n *\tYou should have received a copy of the GNU Lesser General Public\r\n *\tLicense along with qpOASES; if not, write to the Free Software\r\n *\tFoundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA\r\n *\r\n */\r\n\r\n\r\n/**\r\n *\t\\file INCLUDE/Constants.hpp\r\n *\t\\author Hans Joachim Ferreau\r\n *\t\\version 1.3embedded\r\n *\t\\date 2008\r\n *\r\n *\tDefinition of all global constants.\r\n */\r\n\r\n\r\n#ifndef QPOASES_CONSTANTS_HPP\r\n#define QPOASES_CONSTANTS_HPP\r\n\r\n#ifndef QPOASES_CUSTOM_INTERFACE\r\n#include \"acado_qpoases_interface.hpp\"\r\n#else\r\n #define XSTR(x) #x\r\n #define STR(x) XSTR(x)\r\n #include STR(QPOASES_CUSTOM_INTERFACE)\r\n#endif\r\n\r\n/** Maximum number of variables within a QP formulation.\r\n\tNote: this value has to be positive! */\r\nconst int NVMAX = QPOASES_NVMAX;\r\n\r\n/** Maximum number of constraints within a QP formulation.\r\n\tNote: this value has to be positive! */\r\nconst int NCMAX = QPOASES_NCMAX;\r\n\r\n/** Redefinition of NCMAX used for memory allocation, to avoid zero sized arrays\r\n and compiler errors. */\r\nconst int NCMAX_ALLOC = (NCMAX == 0) ? 1 : NCMAX;\r\n\r\n/**< Maximum number of working set recalculations.\r\n\tNote: this value has to be positive! */\r\nconst int NWSRMAX = QPOASES_NWSRMAX;\r\n\r\n/** Desired KKT tolerance of QP solution; a warning RET_INACCURATE_SOLUTION is\r\n * issued if this tolerance is not met.\r\n *\tNote: this value has to be positive! */\r\nconst real_t DESIREDACCURACY = (real_t) 1.0e-3;\r\n\r\n/** Critical KKT tolerance of QP solution; an error is issued if this\r\n * tolerance is not met.\r\n *\tNote: this value has to be positive! */\r\nconst real_t CRITICALACCURACY = (real_t) 1.0e-2;\r\n\r\n\r\n\r\n/** Numerical value of machine precision (min eps, s.t. 1+eps > 1).\r\n\tNote: this value has to be positive! */\r\nconst real_t EPS = (real_t) QPOASES_EPS;\r\n\r\n/** Numerical value of zero (for situations in which it would be\r\n *\tunreasonable to compare with 0.0).\r\n *\tNote: this value has to be positive! */\r\nconst real_t ZERO = (real_t) 1.0e-50;\r\n\r\n/** Numerical value of infinity (e.g. for non-existing bounds).\r\n *\tNote: this value has to be positive! */\r\nconst real_t INFTY = (real_t) 1.0e12;\r\n\r\n\r\n/** Lower/upper (constraints') bound tolerance (an inequality constraint\r\n *\twhose lower and upper bound differ by less than BOUNDTOL is regarded\r\n *\tto be an equality constraint).\r\n *\tNote: this value has to be positive! */\r\nconst real_t BOUNDTOL = (real_t) 1.0e-10;\r\n\r\n/** Offset for relaxing (constraints') bounds at beginning of an initial homotopy.\r\n *\tNote: this value has to be positive! */\r\nconst real_t BOUNDRELAXATION = (real_t) 1.0e3;\r\n\r\n\r\n/** Factor that determines physical lengths of index lists.\r\n *\tNote: this value has to be greater than 1! */\r\nconst int INDEXLISTFACTOR = 5;\r\n\r\n\r\n#endif\t/* QPOASES_CONSTANTS_HPP */\r\n\r\n\r\n/*\r\n *\tend of file\r\n */\r\n"
},
{
"path": "externals/qpoases/INCLUDE/Constraints.hpp",
"content": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.\r\n *\r\n *\tqpOASES is free software; you can redistribute it and/or\r\n *\tmodify it under the terms of the GNU Lesser General Public\r\n *\tLicense as published by the Free Software Foundation; either\r\n *\tversion 2.1 of the License, or (at your option) any later version.\r\n *\r\n *\tqpOASES is distributed in the hope that it will be useful,\r\n *\tbut WITHOUT ANY WARRANTY; without even the implied warranty of\r\n *\tMERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU\r\n *\tLesser General Public License for more details.\r\n *\r\n *\tYou should have received a copy of the GNU Lesser General Public\r\n *\tLicense along with qpOASES; if not, write to the Free Software\r\n *\tFoundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA\r\n *\r\n */\r\n\r\n\r\n/**\r\n *\t\\file INCLUDE/Constraints.hpp\r\n *\t\\author Hans Joachim Ferreau\r\n *\t\\version 1.3embedded\r\n *\t\\date 2007-2008\r\n *\r\n *\tDeclaration of the Constraints class designed to manage working sets of\r\n *\tconstraints within a QProblem.\r\n */\r\n\r\n\r\n#ifndef QPOASES_CONSTRAINTS_HPP\r\n#define QPOASES_CONSTRAINTS_HPP\r\n\r\n\r\n#include \r\n\r\n\r\n\r\n/** This class manages working sets of constraints by storing\r\n *\tindex sets and other status information.\r\n *\r\n *\t\\author Hans Joachim Ferreau\r\n *\t\\version 1.3embedded\r\n *\t\\date 2007-2008\r\n */\r\nclass Constraints : public SubjectTo\r\n{\r\n\t/*\r\n\t *\tPUBLIC MEMBER FUNCTIONS\r\n\t */\r\n\tpublic:\r\n\t\t/** Default constructor. */\r\n\t\tConstraints( );\r\n\r\n\t\t/** Copy constructor (deep copy). */\r\n\t\tConstraints(\tconst Constraints& rhs\t/**< Rhs object. */\r\n\t\t\t\t\t\t);\r\n\r\n\t\t/** Destructor. */\r\n\t\t~Constraints( );\r\n\r\n\t\t/** Assignment operator (deep copy). */\r\n\t\tConstraints& operator=(\tconst Constraints& rhs\t/**< Rhs object. */\r\n\t\t\t\t\t\t\t\t);\r\n\r\n\r\n\t\t/** Pseudo-constructor takes the number of constraints.\r\n\t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\treturnValue init(\tint n\t/**< Number of constraints. */\r\n\t\t\t\t\t\t\t);\r\n\r\n\r\n\t\t/** Initially adds number of a new (i.e. not yet in the list) constraint to\r\n\t\t * a given index set.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t \t\t\tRET_SETUP_CONSTRAINT_FAILED \\n\r\n\t\t\t\t\tRET_INDEX_OUT_OF_BOUNDS \\n\r\n\t\t\t\t\tRET_INVALID_ARGUMENTS */\r\n\t\treturnValue setupConstraint(\tint _number,\t\t\t\t/**< Number of new constraint. */\r\n\t\t\t\t\t\t\t\t\t\tSubjectToStatus _status\t\t/**< Status of new constraint. */\r\n\t\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Initially adds all enabled numbers of new (i.e. not yet in the list) constraints to\r\n\t\t * to the index set of inactive constraints; the order depends on the SujectToType\r\n\t\t * of each index. Only disabled constraints are added to index set of disabled constraints!\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t \t\t\tRET_SETUP_CONSTRAINT_FAILED */\r\n\t\treturnValue setupAllInactive( );\r\n\r\n\r\n\t\t/** Moves index of a constraint from index list of active to that of inactive constraints.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t \t\t\tRET_MOVING_CONSTRAINT_FAILED */\r\n\t\treturnValue moveActiveToInactive(\tint _number\t\t\t\t/**< Number of constraint to become inactive. */\r\n\t\t\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Moves index of a constraint from index list of inactive to that of active constraints.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t \t\t\tRET_MOVING_CONSTRAINT_FAILED */\r\n\t\treturnValue moveInactiveToActive(\tint _number,\t\t\t/**< Number of constraint to become active. */\r\n\t\t\t\t\t\t\t\t\t\t\tSubjectToStatus _status\t/**< Status of constraint to become active. */\r\n\t\t\t\t\t\t\t\t\t\t\t);\r\n\r\n\r\n\t\t/** Returns the number of constraints.\r\n\t\t *\t\\return Number of constraints. */\r\n\t\tinline int getNC( ) const;\r\n\r\n\t\t/** Returns the number of implicit equality constraints.\r\n\t\t *\t\\return Number of implicit equality constraints. */\r\n\t\tinline int getNEC( ) const;\r\n\r\n\t\t/** Returns the number of \"real\" inequality constraints.\r\n\t\t *\t\\return Number of \"real\" inequality constraints. */\r\n\t\tinline int getNIC( ) const;\r\n\r\n\t\t/** Returns the number of unbounded constraints (i.e. without any bounds).\r\n\t\t *\t\\return Number of unbounded constraints (i.e. without any bounds). */\r\n\t\tinline int getNUC( ) const;\r\n\r\n\r\n\t\t/** Sets number of implicit equality constraints.\r\n\t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\tinline returnValue setNEC(\tint n\t/**< Number of implicit equality constraints. */\r\n\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Sets number of \"real\" inequality constraints.\r\n\t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\tinline returnValue setNIC(\tint n\t/**< Number of \"real\" inequality constraints. */\r\n\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Sets number of unbounded constraints (i.e. without any bounds).\r\n\t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\tinline returnValue setNUC(\tint n\t/**< Number of unbounded constraints (i.e. without any bounds). */\r\n\t\t\t\t\t\t\t\t\t);\r\n\r\n\r\n\t\t/** Returns the number of active constraints.\r\n\t\t *\t\\return Number of constraints. */\r\n\t\tinline int getNAC( );\r\n\r\n\t\t/** Returns the number of inactive constraints.\r\n\t\t *\t\\return Number of constraints. */\r\n\t\tinline int getNIAC( );\r\n\r\n\r\n\t\t/** Returns a pointer to active constraints index list.\r\n\t\t *\t\\return Pointer to active constraints index list. */\r\n\t\tinline Indexlist* getActive( );\r\n\r\n\t\t/** Returns a pointer to inactive constraints index list.\r\n\t\t *\t\\return Pointer to inactive constraints index list. */\r\n\t\tinline Indexlist* getInactive( );\r\n\r\n\r\n\t/*\r\n\t *\tPROTECTED MEMBER VARIABLES\r\n\t */\r\n\tprotected:\r\n\t\tint nC;\t\t\t\t\t/**< Number of constraints (nC = nEC + nIC + nUC). */\r\n\t\tint nEC;\t\t\t\t/**< Number of implicit equality constraints. */\r\n\t\tint\tnIC;\t\t\t\t/**< Number of \"real\" inequality constraints. */\r\n\t\tint nUC;\t\t\t\t/**< Number of unbounded constraints (i.e. without any bounds). */\r\n\r\n\t\tIndexlist active;\t\t/**< Index list of active constraints. */\r\n\t\tIndexlist inactive;\t\t/**< Index list of inactive constraints. */\r\n};\r\n\r\n\r\n#include \r\n\r\n#endif\t/* QPOASES_CONSTRAINTS_HPP */\r\n\r\n\r\n/*\r\n *\tend of file\r\n */\r\n"
},
{
"path": "externals/qpoases/INCLUDE/CyclingManager.hpp",
"content": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.\r\n *\r\n *\tqpOASES is free software; you can redistribute it and/or\r\n *\tmodify it under the terms of the GNU Lesser General Public\r\n *\tLicense as published by the Free Software Foundation; either\r\n *\tversion 2.1 of the License, or (at your option) any later version.\r\n *\r\n *\tqpOASES is distributed in the hope that it will be useful,\r\n *\tbut WITHOUT ANY WARRANTY; without even the implied warranty of\r\n *\tMERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU\r\n *\tLesser General Public License for more details.\r\n *\r\n *\tYou should have received a copy of the GNU Lesser General Public\r\n *\tLicense along with qpOASES; if not, write to the Free Software\r\n *\tFoundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA\r\n *\r\n */\r\n\r\n\r\n/**\r\n *\t\\file INCLUDE/CyclingManager.hpp\r\n *\t\\author Hans Joachim Ferreau\r\n *\t\\version 1.3embedded\r\n *\t\\date 2007-2008\r\n *\r\n *\tDeclaration of the CyclingManager class designed to detect\r\n *\tand handle possible cycling during QP iterations.\r\n */\r\n\r\n\r\n#ifndef QPOASES_CYCLINGMANAGER_HPP\r\n#define QPOASES_CYCLINGMANAGER_HPP\r\n\r\n\r\n#include \r\n\r\n\r\n\r\n/** This class is intended to detect and handle possible cycling during QP iterations.\r\n *\tAs cycling seems to occur quite rarely, this class is NOT FULLY IMPLEMENTED YET!\r\n *\r\n *\t\\author Hans Joachim Ferreau\r\n *\t\\version 1.3embedded\r\n *\t\\date 2007-2008\r\n */\r\nclass CyclingManager\r\n{\r\n\t/*\r\n\t *\tPUBLIC MEMBER FUNCTIONS\r\n\t */\r\n\tpublic:\r\n\t\t/** Default constructor. */\r\n\t\tCyclingManager( );\r\n\r\n\t\t/** Copy constructor (deep copy). */\r\n\t\tCyclingManager(\tconst CyclingManager& rhs\t/**< Rhs object. */\r\n\t\t\t\t\t\t);\r\n\r\n\t\t/** Destructor. */\r\n\t\t~CyclingManager( );\r\n\r\n\t\t/** Copy asingment operator (deep copy). */\r\n\t\tCyclingManager& operator=(\tconst CyclingManager& rhs\t/**< Rhs object. */\r\n\t\t\t\t\t\t\t\t\t);\r\n\r\n\r\n\t\t/** Pseudo-constructor which takes the number of bounds/constraints.\r\n\t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\treturnValue init(\tint _nV,\t/**< Number of bounds to be managed. */\r\n\t\t\t\t\t\t\tint _nC\t\t/**< Number of constraints to be managed. */\r\n\t\t\t\t\t\t\t);\r\n\r\n\r\n\t\t/** Stores index of a bound/constraint that might cause cycling.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t\t\t\tRET_INDEX_OUT_OF_BOUNDS */\r\n\t\treturnValue setCyclingStatus(\tint number,\t\t\t\t/**< Number of bound/constraint. */\r\n\t\t\t\t\t\t\t\t\t\tBooleanType isBound,\t/**< Flag that indicates if given number corresponds to a\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t * bound (BT_TRUE) or a constraint (BT_FALSE). */\r\n\t\t\t\t\t\t\t\t\t\tCyclingStatus _status\t/**< Cycling status of bound/constraint. */\r\n\t\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Returns if bound/constraint might cause cycling.\r\n\t\t *\t\\return BT_TRUE: bound/constraint might cause cycling \\n\r\n\t\t \t\t\tBT_FALSE: otherwise */\r\n\t\tCyclingStatus getCyclingStatus(\tint number,\t\t\t/**< Number of bound/constraint. */\r\n\t\t\t\t\t\t\t\t\t\tBooleanType isBound\t/**< Flag that indicates if given number corresponds to\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t * a bound (BT_TRUE) or a constraint (BT_FALSE). */\r\n\t\t\t\t\t\t\t\t\t\t) const;\r\n\r\n\r\n\t\t/** Clears all previous cycling information.\r\n\t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\treturnValue clearCyclingData( );\r\n\r\n\r\n\t\t/** Returns if cycling was detected.\r\n\t\t *\t\\return BT_TRUE iff cycling was detected. */\r\n\t\tinline BooleanType isCyclingDetected( ) const;\r\n\r\n\r\n\t/*\r\n\t *\tPROTECTED MEMBER VARIABLES\r\n\t */\r\n\tprotected:\r\n\t\tint\tnV;\t\t\t\t\t\t\t\t\t/**< Number of managed bounds. */\r\n\t\tint\tnC;\t\t\t\t\t\t\t\t\t/**< Number of managed constraints. */\r\n\r\n\t\tCyclingStatus status[NVMAX+NCMAX];\t\t/**< Array to store cycling status of all bounds/constraints. */\r\n\r\n\t\tBooleanType cyclingDetected;\t\t\t/**< Flag if cycling was detected. */\r\n};\r\n\r\n\r\n#include \r\n\r\n#endif\t/* QPOASES_CYCLINGMANAGER_HPP */\r\n\r\n\r\n/*\r\n *\tend of file\r\n */\r\n"
},
{
"path": "externals/qpoases/INCLUDE/EXTRAS/SolutionAnalysis.hpp",
"content": "/*\n *\tThis file is part of qpOASES.\n *\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\n *\tCopyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.\n *\n *\tqpOASES is free software; you can redistribute it and/or\n *\tmodify it under the terms of the GNU Lesser General Public\n *\tLicense as published by the Free Software Foundation; either\n *\tversion 2.1 of the License, or (at your option) any later version.\n *\n *\tqpOASES is distributed in the hope that it will be useful,\n *\tbut WITHOUT ANY WARRANTY; without even the implied warranty of\n *\tMERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU\n *\tLesser General Public License for more details.\n *\n *\tYou should have received a copy of the GNU Lesser General Public\n *\tLicense along with qpOASES; if not, write to the Free Software\n *\tFoundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA\n *\n */\n\n\n/**\n *\t\\file INCLUDE/EXTRAS/SolutionAnalysis.hpp\n *\t\\author Milan Vukov, Boris Houska, Hans Joachim Ferreau\n *\t\\version 1.3embedded\n *\t\\date 2012\n *\n *\tSolution analysis class, based on a class in the standard version of the qpOASES\n */\n\n\n//\n\n#ifndef QPOASES_SOLUTIONANALYSIS_HPP\n#define QPOASES_SOLUTIONANALYSIS_HPP\n\n#include \n\n/** Enables the computation of variance as is in the standard version of qpOASES */\n#define QPOASES_USE_OLD_VERSION 0\n\n#if QPOASES_USE_OLD_VERSION\n#define KKT_DIM (2 * NVMAX + NCMAX)\n#endif\n\nclass SolutionAnalysis\n{\npublic:\n\t\n\t/** Default constructor. */\n\tSolutionAnalysis( );\n\t\n\t/** Copy constructor (deep copy). */\n\tSolutionAnalysis( \tconst SolutionAnalysis& rhs\t/**< Rhs object. */\n\t\t\t\t\t\t);\n\t\n\t/** Destructor. */\n\t~SolutionAnalysis( );\n\t\n\t/** Copy asingment operator (deep copy). */\n\tSolutionAnalysis& operator=(\tconst SolutionAnalysis& rhs\t/**< Rhs object. */\n\t\t\t\t\t\t\t\t\t);\n\t\n\t/** A routine for computation of inverse of the Hessian matrix. */\n\treturnValue getHessianInverse(\n\t\t\t\t\t\t\t\t\tQProblem* qp,\t\t\t/** QP */\n\t\t\t\t\t\t\t\t\treal_t* hessianInverse\t/** Inverse of the Hessian matrix*/\n\t\t\t\t\t\t\t\t\t);\n\t\n\t/** A routine for computation of inverse of the Hessian matrix. */\n\treturnValue getHessianInverse(\tQProblemB* qp,\t\t\t/** QP */\n\t\t\t\t\t\t\t\t\treal_t* hessianInverse\t/** Inverse of the Hessian matrix*/\n\t\t\t\t\t\t\t\t\t);\n\n#if QPOASES_USE_OLD_VERSION\n\treturnValue getVarianceCovariance(\n\t\t\t\t\t\t\t\t\t\tQProblem* qp,\n\t\t\t\t\t\t\t\t\t\treal_t* g_b_bA_VAR,\n\t\t\t\t\t\t\t\t\t\treal_t* Primal_Dual_VAR\n\t\t\t\t\t\t\t\t\t\t);\n#endif\n\t\nprivate:\n\t\n\treal_t delta_g_cov[ NVMAX ];\t\t/** A covariance-vector of g */\n\treal_t delta_lb_cov[ NVMAX ];\t\t/** A covariance-vector of lb */\n\treal_t delta_ub_cov[ NVMAX ];\t\t/** A covariance-vector of ub */\n\treal_t delta_lbA_cov[ NCMAX_ALLOC ];\t\t/** A covariance-vector of lbA */\n\treal_t delta_ubA_cov[ NCMAX_ALLOC ];\t\t/** A covariance-vector of ubA */\n\t\n#if QPOASES_USE_OLD_VERSION\n\treal_t K[KKT_DIM * KKT_DIM];\t\t/** A matrix to store an intermediate result */\n#endif\n\t\n\tint FR_idx[ NVMAX ];\t\t\t\t/** Index array for free variables */\n\tint FX_idx[ NVMAX ];\t\t\t\t/** Index array for fixed variables */\n\tint AC_idx[ NCMAX_ALLOC ];\t\t\t\t/** Index array for active constraints */\n\t\n\treal_t delta_xFR[ NVMAX ];\t\t\t/** QP reaction, primal, w.r.t. free */\n\treal_t delta_xFX[ NVMAX ];\t\t\t/** QP reaction, primal, w.r.t. fixed */\n\treal_t delta_yAC[ NVMAX ];\t\t\t/** QP reaction, dual, w.r.t. active */\n\treal_t delta_yFX[ NVMAX ];\t\t\t/** QP reaction, dual, w.r.t. fixed*/\n};\n\n#endif // QPOASES_SOLUTIONANALYSIS_HPP\n"
},
{
"path": "externals/qpoases/INCLUDE/Indexlist.hpp",
"content": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.\r\n *\r\n *\tqpOASES is free software; you can redistribute it and/or\r\n *\tmodify it under the terms of the GNU Lesser General Public\r\n *\tLicense as published by the Free Software Foundation; either\r\n *\tversion 2.1 of the License, or (at your option) any later version.\r\n *\r\n *\tqpOASES is distributed in the hope that it will be useful,\r\n *\tbut WITHOUT ANY WARRANTY; without even the implied warranty of\r\n *\tMERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU\r\n *\tLesser General Public License for more details.\r\n *\r\n *\tYou should have received a copy of the GNU Lesser General Public\r\n *\tLicense along with qpOASES; if not, write to the Free Software\r\n *\tFoundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA\r\n *\r\n */\r\n\r\n\r\n/**\r\n *\t\\file INCLUDE/Indexlist.hpp\r\n *\t\\author Hans Joachim Ferreau\r\n *\t\\version 1.3embedded\r\n *\t\\date 2007-2008\r\n *\r\n *\tDeclaration of the Indexlist class designed to manage index lists of\r\n *\tconstraints and bounds within a SubjectTo object.\r\n */\r\n\r\n\r\n#ifndef QPOASES_INDEXLIST_HPP\r\n#define QPOASES_INDEXLIST_HPP\r\n\r\n\r\n#include \r\n\r\n\r\n/** This class manages index lists.\r\n *\r\n *\t\\author Hans Joachim Ferreau\r\n *\t\\version 1.3embedded\r\n *\t\\date 2007-2008\r\n */\r\nclass Indexlist\r\n{\r\n\t/*\r\n\t *\tPUBLIC MEMBER FUNCTIONS\r\n\t */\r\n\tpublic:\r\n\t\t/** Default constructor. */\r\n\t\tIndexlist( );\r\n\r\n\t\t/** Copy constructor (deep copy). */\r\n\t\tIndexlist(\tconst Indexlist& rhs\t/**< Rhs object. */\r\n\t\t\t\t\t);\r\n\r\n\t\t/** Destructor. */\r\n\t\t~Indexlist( );\r\n\r\n\t\t/** Assingment operator (deep copy). */\r\n\t\tIndexlist& operator=(\tconst Indexlist& rhs\t/**< Rhs object. */\r\n\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Pseudo-constructor.\r\n\t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\treturnValue init( );\r\n\r\n\r\n\t\t/** Creates an array of all numbers within the index set in correct order.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t \t\t\tRET_INDEXLIST_CORRUPTED */\r\n\t\treturnValue\tgetNumberArray(\tint* const numberarray\t/**< Output: Array of numbers (NULL on error). */\r\n\t\t\t\t\t\t\t\t\t) const;\r\n\r\n\r\n\t\t/** Determines the index within the index list at with a given number is stored.\r\n\t\t *\t\\return >= 0: Index of given number. \\n\r\n\t\t \t\t\t-1: Number not found. */\r\n\t\tint\tgetIndex(\tint givennumber\t/**< Number whose index shall be determined. */\r\n\t\t\t\t\t\t) const;\r\n\r\n\t\t/** Determines the physical index within the index list at with a given number is stored.\r\n\t\t *\t\\return >= 0: Index of given number. \\n\r\n\t\t \t\t\t-1: Number not found. */\r\n\t\tint\tgetPhysicalIndex(\tint givennumber\t/**< Number whose physical index shall be determined. */\r\n\t\t\t\t\t\t\t\t) const;\r\n\r\n\t\t/** Returns the number stored at a given physical index.\r\n\t\t *\t\\return >= 0: Number stored at given physical index. \\n\r\n\t\t \t\t\t-RET_INDEXLIST_OUTOFBOUNDS */\r\n\t\tint\tgetNumber(\tint physicalindex\t/**< Physical index of the number to be returned. */\r\n\t\t\t\t\t\t) const;\r\n\r\n\r\n\t\t/** Returns the current length of the index list.\r\n\t\t *\t\\return Current length of the index list. */\r\n\t\tinline int getLength( );\r\n\r\n\t\t/** Returns last number within the index list.\r\n\t\t *\t\\return Last number within the index list. */\r\n\t\tinline int getLastNumber( ) const;\r\n\r\n\r\n\t\t/** Adds number to index list.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t \t\t\tRET_INDEXLIST_MUST_BE_REORDERD \\n\r\n\t\t \t\t\tRET_INDEXLIST_EXCEEDS_MAX_LENGTH */\r\n\t\treturnValue addNumber(\tint addnumber\t/**< Number to be added. */\r\n\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Removes number from index list.\r\n\t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\treturnValue removeNumber(\tint removenumber\t/**< Number to be removed. */\r\n\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Swaps two numbers within index list.\r\n\t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\treturnValue swapNumbers(\tint number1,/**< First number for swapping. */\r\n\t\t\t\t\t\t\t\t\tint number2\t/**< Second number for swapping. */\r\n\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Determines if a given number is contained in the index set.\r\n\t\t *\t\\return BT_TRUE iff number is contain in the index set */\r\n\t\tinline BooleanType isMember(\tint _number\t/**< Number to be tested for membership. */\r\n\t\t\t\t\t\t\t\t\t\t) const;\r\n\r\n\r\n\t/*\r\n\t *\tPROTECTED MEMBER VARIABLES\r\n\t */\r\n\tprotected:\r\n\t\tint number[INDEXLISTFACTOR*(NVMAX+NCMAX)];\t\t/**< Array to store numbers of constraints or bounds. */\r\n\t\tint next[INDEXLISTFACTOR*(NVMAX+NCMAX)];\t\t/**< Array to store physical index of successor. */\r\n\t\tint previous[INDEXLISTFACTOR*(NVMAX+NCMAX)];\t/**< Array to store physical index of predecossor. */\r\n\t\tint\tlength;\t\t\t\t\t\t\t\t\t\t/**< Length of index list. */\r\n\t\tint\tfirst;\t\t\t\t\t\t\t\t\t\t/**< Physical index of first element. */\r\n\t\tint\tlast;\t\t\t\t\t\t\t\t\t\t/**< Physical index of last element. */\r\n\t\tint\tlastusedindex;\t\t\t\t\t\t\t\t/**< Physical index of last entry in index list. */\r\n\t\tint\tphysicallength;\t\t\t\t\t\t\t\t/**< Physical length of index list. */\r\n};\r\n\r\n\r\n#include \r\n\r\n#endif\t/* QPOASES_INDEXLIST_HPP */\r\n\r\n\r\n/*\r\n *\tend of file\r\n */\r\n"
},
{
"path": "externals/qpoases/INCLUDE/MessageHandling.hpp",
"content": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.\r\n *\r\n *\tqpOASES is free software; you can redistribute it and/or\r\n *\tmodify it under the terms of the GNU Lesser General Public\r\n *\tLicense as published by the Free Software Foundation; either\r\n *\tversion 2.1 of the License, or (at your option) any later version.\r\n *\r\n *\tqpOASES is distributed in the hope that it will be useful,\r\n *\tbut WITHOUT ANY WARRANTY; without even the implied warranty of\r\n *\tMERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU\r\n *\tLesser General Public License for more details.\r\n *\r\n *\tYou should have received a copy of the GNU Lesser General Public\r\n *\tLicense along with qpOASES; if not, write to the Free Software\r\n *\tFoundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA\r\n *\r\n */\r\n\r\n\r\n/**\r\n *\t\\file INCLUDE/MessageHandling.hpp\r\n *\t\\author Hans Joachim Ferreau\r\n *\t\\version 1.3embedded\r\n *\t\\date 2007-2008\r\n *\r\n *\tDeclaration of the MessageHandling class including global return values.\r\n */\r\n\r\n\r\n#ifndef QPOASES_MESSAGEHANDLING_HPP\r\n#define QPOASES_MESSAGEHANDLING_HPP\r\n\r\n// #define PC_DEBUG\r\n\r\n#ifdef PC_DEBUG\r\n #include \r\n\r\n /** Defines an alias for FILE from stdio.h. */\r\n #define myFILE FILE\r\n /** Defines an alias for stderr from stdio.h. */\r\n #define myStderr stderr\r\n /** Defines an alias for stdout from stdio.h. */\r\n #define myStdout stdout\r\n#else\r\n /** Defines an alias for FILE from stdio.h. */\r\n #define myFILE int\r\n /** Defines an alias for stderr from stdio.h. */\r\n #define myStderr 0\r\n /** Defines an alias for stdout from stdio.h. */\r\n #define myStdout 0\r\n#endif\r\n\r\n\r\n#include \r\n#include \r\n\r\n\r\n/** Defines symbols for global return values. \\n\r\n * Important: All return values are assumed to be nonnegative! */\r\nenum returnValue\r\n{\r\nTERMINAL_LIST_ELEMENT = -1,\t\t\t\t\t\t/**< Terminal list element, internal usage only! */\r\n/* miscellaneous */\r\nSUCCESSFUL_RETURN = 0,\t\t\t\t\t\t\t/**< Successful return. */\r\nRET_DIV_BY_ZERO,\t\t \t\t\t\t\t\t/**< Division by zero. */\r\nRET_INDEX_OUT_OF_BOUNDS,\t\t\t\t\t\t/**< Index out of bounds. */\r\nRET_INVALID_ARGUMENTS,\t\t\t\t\t\t\t/**< At least one of the arguments is invalid. */\r\nRET_ERROR_UNDEFINED,\t\t\t\t\t\t\t/**< Error number undefined. */\r\nRET_WARNING_UNDEFINED,\t\t\t\t\t\t\t/**< Warning number undefined. */\r\nRET_INFO_UNDEFINED,\t\t\t\t\t\t\t\t/**< Info number undefined. */\r\nRET_EWI_UNDEFINED,\t\t\t\t\t\t\t\t/**< Error/warning/info number undefined. */\r\nRET_AVAILABLE_WITH_LINUX_ONLY,\t\t\t\t\t/**< This function is available under Linux only. */\r\nRET_UNKNOWN_BUG,\t\t\t\t\t\t\t\t/**< The error occured is not yet known. */\r\nRET_PRINTLEVEL_CHANGED,\t\t\t\t\t\t\t/**< 10 Print level changed. */\r\nRET_NOT_YET_IMPLEMENTED,\t\t\t\t\t\t/**< Requested function is not yet implemented in this version of qpOASES. */\r\n/* Indexlist */\r\nRET_INDEXLIST_MUST_BE_REORDERD,\t\t\t\t\t/**< Index list has to be reordered. */\r\nRET_INDEXLIST_EXCEEDS_MAX_LENGTH,\t\t\t\t/**< Index list exceeds its maximal physical length. */\r\nRET_INDEXLIST_CORRUPTED,\t\t\t\t\t\t/**< Index list corrupted. */\r\nRET_INDEXLIST_OUTOFBOUNDS,\t\t\t\t\t\t/**< Physical index is out of bounds. */\r\nRET_INDEXLIST_ADD_FAILED,\t\t\t\t\t\t/**< Adding indices from another index set failed. */\r\nRET_INDEXLIST_INTERSECT_FAILED,\t\t\t\t\t/**< Intersection with another index set failed. */\r\n/* SubjectTo / Bounds / Constraints */\r\nRET_INDEX_ALREADY_OF_DESIRED_STATUS,\t\t\t/**< Index is already of desired status. */\r\nRET_ADDINDEX_FAILED,\t\t\t\t\t\t\t/**< Cannot swap between different indexsets. */\r\nRET_SWAPINDEX_FAILED,\t\t\t\t\t\t\t/**< 20 Adding index to index set failed. */\r\nRET_NOTHING_TO_DO,\t\t\t\t\t\t\t\t/**< Nothing to do. */\r\nRET_SETUP_BOUND_FAILED,\t\t\t\t\t\t\t/**< Setting up bound index failed. */\r\nRET_SETUP_CONSTRAINT_FAILED,\t\t\t\t\t/**< Setting up constraint index failed. */\r\nRET_MOVING_BOUND_FAILED,\t\t\t\t\t\t/**< Moving bound between index sets failed. */\r\nRET_MOVING_CONSTRAINT_FAILED,\t\t\t\t\t/**< Moving constraint between index sets failed. */\r\n/* QProblem */\r\nRET_QP_ALREADY_INITIALISED,\t\t\t\t\t\t/**< QProblem has already been initialised. */\r\nRET_NO_INIT_WITH_STANDARD_SOLVER,\t\t\t\t/**< Initialisation via extern QP solver is not yet implemented. */\r\nRET_RESET_FAILED,\t\t\t\t\t\t\t\t/**< Reset failed. */\r\nRET_INIT_FAILED,\t\t\t\t\t\t\t\t/**< Initialisation failed. */\r\nRET_INIT_FAILED_TQ,\t\t\t\t\t\t\t\t/**< 30 Initialisation failed due to TQ factorisation. */\r\nRET_INIT_FAILED_CHOLESKY,\t\t\t\t\t\t/**< Initialisation failed due to Cholesky decomposition. */\r\nRET_INIT_FAILED_HOTSTART,\t\t\t\t\t\t/**< Initialisation failed! QP could not be solved! */\r\nRET_INIT_FAILED_INFEASIBILITY,\t\t\t\t\t/**< Initial QP could not be solved due to infeasibility! */\r\nRET_INIT_FAILED_UNBOUNDEDNESS,\t\t\t\t\t/**< Initial QP could not be solved due to unboundedness! */\r\nRET_INIT_SUCCESSFUL,\t\t\t\t\t\t\t/**< Initialisation done. */\r\nRET_OBTAINING_WORKINGSET_FAILED,\t\t\t\t/**< Failed to obtain working set for auxiliary QP. */\r\nRET_SETUP_WORKINGSET_FAILED,\t\t\t\t\t/**< Failed to setup working set for auxiliary QP. */\r\nRET_SETUP_AUXILIARYQP_FAILED,\t\t\t\t\t/**< Failed to setup auxiliary QP for initialised homotopy. */\r\nRET_NO_EXTERN_SOLVER,\t\t\t\t\t\t\t/**< No extern QP solver available. */\r\nRET_QP_UNBOUNDED,\t\t\t\t\t\t\t\t/**< 40 QP is unbounded. */\r\nRET_QP_INFEASIBLE,\t\t\t\t\t\t\t\t/**< QP is infeasible. */\r\nRET_QP_NOT_SOLVED,\t\t\t\t\t\t\t\t/**< Problems occured while solving QP with standard solver. */\r\nRET_QP_SOLVED,\t\t\t\t\t\t\t\t\t/**< QP successfully solved. */\r\nRET_UNABLE_TO_SOLVE_QP,\t\t\t\t\t\t\t/**< Problems occured while solving QP. */\r\nRET_INITIALISATION_STARTED,\t\t\t\t\t\t/**< Starting problem initialisation. */\r\nRET_HOTSTART_FAILED,\t\t\t\t\t\t\t/**< Unable to perform homotopy due to internal error. */\r\nRET_HOTSTART_FAILED_TO_INIT,\t\t\t\t\t/**< Unable to initialise problem. */\r\nRET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED,\t\t/**< Unable to perform homotopy as previous QP is not solved. */\r\nRET_ITERATION_STARTED,\t\t\t\t\t\t\t/**< Iteration... */\r\nRET_SHIFT_DETERMINATION_FAILED,\t\t\t\t\t/**< 50 Determination of shift of the QP data failed. */\r\nRET_STEPDIRECTION_DETERMINATION_FAILED,\t\t\t/**< Determination of step direction failed. */\r\nRET_STEPLENGTH_DETERMINATION_FAILED,\t\t\t/**< Determination of step direction failed. */\r\nRET_OPTIMAL_SOLUTION_FOUND,\t\t\t\t\t\t/**< Optimal solution of neighbouring QP found. */\r\nRET_HOMOTOPY_STEP_FAILED,\t\t\t\t\t\t/**< Unable to perform homotopy step. */\r\nRET_HOTSTART_STOPPED_INFEASIBILITY,\t\t\t\t/**< Premature homotopy termination because QP is infeasible. */\r\nRET_HOTSTART_STOPPED_UNBOUNDEDNESS,\t\t\t\t/**< Premature homotopy termination because QP is unbounded. */\r\nRET_WORKINGSET_UPDATE_FAILED,\t\t\t\t\t/**< Unable to update working sets according to initial guesses. */\r\nRET_MAX_NWSR_REACHED,\t\t\t\t\t\t\t/**< Maximum number of working set recalculations performed. */\r\nRET_CONSTRAINTS_NOT_SPECIFIED,\t\t\t\t\t/**< Problem does comprise constraints! You also have to specify new constraints' bounds. */\r\nRET_INVALID_FACTORISATION_FLAG,\t\t\t\t\t/**< 60 Invalid factorisation flag. */\r\nRET_UNABLE_TO_SAVE_QPDATA,\t\t\t\t\t\t/**< Unable to save QP data. */\r\nRET_STEPDIRECTION_FAILED_TQ,\t\t\t\t\t/**< Abnormal termination due to TQ factorisation. */\r\nRET_STEPDIRECTION_FAILED_CHOLESKY,\t\t\t\t/**< Abnormal termination due to Cholesky factorisation. */\r\nRET_CYCLING_DETECTED,\t\t\t\t\t\t\t/**< Cycling detected. */\r\nRET_CYCLING_NOT_RESOLVED,\t\t\t\t\t\t/**< Cycling cannot be resolved, QP probably infeasible. */\r\nRET_CYCLING_RESOLVED,\t\t\t\t\t\t\t/**< Cycling probably resolved. */\r\nRET_STEPSIZE,\t\t\t\t\t\t\t\t\t/**< For displaying performed stepsize. */\r\nRET_STEPSIZE_NONPOSITIVE,\t\t\t\t\t\t/**< For displaying non-positive stepsize. */\r\nRET_SETUPSUBJECTTOTYPE_FAILED,\t\t\t\t\t/**< Setup of SubjectToTypes failed. */\r\nRET_ADDCONSTRAINT_FAILED,\t\t\t\t\t\t/**< 70 Addition of constraint to working set failed. */\r\nRET_ADDCONSTRAINT_FAILED_INFEASIBILITY,\t\t\t/**< Addition of constraint to working set failed (due to QP infeasibility). */\r\nRET_ADDBOUND_FAILED,\t\t\t\t\t\t\t/**< Addition of bound to working set failed. */\r\nRET_ADDBOUND_FAILED_INFEASIBILITY,\t\t\t\t/**< Addition of bound to working set failed (due to QP infeasibility). */\r\nRET_REMOVECONSTRAINT_FAILED,\t\t\t\t\t/**< Removal of constraint from working set failed. */\r\nRET_REMOVEBOUND_FAILED,\t\t\t\t\t\t\t/**< Removal of bound from working set failed. */\r\nRET_REMOVE_FROM_ACTIVESET,\t\t\t\t\t\t/**< Removing from active set... */\r\nRET_ADD_TO_ACTIVESET,\t\t\t\t\t\t\t/**< Adding to active set... */\r\nRET_REMOVE_FROM_ACTIVESET_FAILED,\t\t\t\t/**< Removing from active set failed. */\r\nRET_ADD_TO_ACTIVESET_FAILED,\t\t\t\t\t/**< Adding to active set failed. */\r\nRET_CONSTRAINT_ALREADY_ACTIVE,\t\t\t\t\t/**< 80 Constraint is already active. */\r\nRET_ALL_CONSTRAINTS_ACTIVE,\t\t\t\t\t\t/**< All constraints are active, no further constraint can be added. */\r\nRET_LINEARLY_DEPENDENT,\t\t\t\t\t\t\t/**< New bound/constraint is linearly dependent. */\r\nRET_LINEARLY_INDEPENDENT,\t\t\t\t\t\t/**< New bound/constraint is linearly independent. */\r\nRET_LI_RESOLVED,\t\t\t\t\t\t\t\t/**< Linear independence of active contraint matrix successfully resolved. */\r\nRET_ENSURELI_FAILED,\t\t\t\t\t\t\t/**< Failed to ensure linear indepence of active contraint matrix. */\r\nRET_ENSURELI_FAILED_TQ,\t\t\t\t\t\t\t/**< Abnormal termination due to TQ factorisation. */\r\nRET_ENSURELI_FAILED_NOINDEX,\t\t\t\t\t/**< No index found, QP probably infeasible. */\r\nRET_ENSURELI_FAILED_CYCLING,\t\t\t\t\t/**< Cycling detected, QP probably infeasible. */\r\nRET_BOUND_ALREADY_ACTIVE,\t\t\t\t\t\t/**< Bound is already active. */\r\nRET_ALL_BOUNDS_ACTIVE,\t\t\t\t\t\t\t/**< 90 All bounds are active, no further bound can be added. */\r\nRET_CONSTRAINT_NOT_ACTIVE,\t\t\t\t\t\t/**< Constraint is not active. */\r\nRET_BOUND_NOT_ACTIVE,\t\t\t\t\t\t\t/**< Bound is not active. */\r\nRET_HESSIAN_NOT_SPD,\t\t\t\t\t\t\t/**< Projected Hessian matrix not positive definite. */\r\nRET_MATRIX_SHIFT_FAILED,\t\t\t\t\t\t/**< Unable to update matrices or to transform vectors. */\r\nRET_MATRIX_FACTORISATION_FAILED,\t\t\t\t/**< Unable to calculate new matrix factorisations. */\r\nRET_PRINT_ITERATION_FAILED,\t\t\t\t\t\t/**< Unable to print information on current iteration. */\r\nRET_NO_GLOBAL_MESSAGE_OUTPUTFILE,\t\t\t\t/**< No global message output file initialised. */\r\n/* Utils */\r\nRET_UNABLE_TO_OPEN_FILE,\t\t\t\t\t\t/**< Unable to open file. */\r\nRET_UNABLE_TO_WRITE_FILE,\t\t\t\t\t\t/**< Unable to write into file. */\r\nRET_UNABLE_TO_READ_FILE,\t\t\t\t\t\t/**< 100 Unable to read from file. */\r\nRET_FILEDATA_INCONSISTENT,\t\t\t\t\t\t/**< File contains inconsistent data. */\r\n/* SolutionAnalysis */\r\nRET_NO_SOLUTION, \t\t\t\t\t\t\t\t/**< QP solution does not satisfy KKT optimality conditions. */\r\nRET_INACCURATE_SOLUTION\t\t\t\t\t\t\t/**< KKT optimality conditions not satisfied to sufficient accuracy. */\r\n};\r\n\r\n\r\n\r\n/** This class handles all kinds of messages (errors, warnings, infos) initiated\r\n * by qpOASES modules and stores the correspoding global preferences.\r\n *\r\n *\t\\author Hans Joachim Ferreau (special thanks to Leonard Wirsching)\r\n *\t\\version 1.3embedded\r\n *\t\\date 2007-2008\r\n */\r\nclass MessageHandling\r\n{\r\n\t/*\r\n\t *\tINTERNAL DATA STRUCTURES\r\n\t */\r\n\tpublic:\r\n\t\t/** Data structure for entries in global message list. */\r\n\t\ttypedef struct {\r\n\t\t\treturnValue key;\t\t\t\t\t\t\t/**< Global return value. */\r\n\t\t\tconst char* data;\t\t\t\t\t\t\t/**< Corresponding message. */\r\n\t\t\tVisibilityStatus globalVisibilityStatus; \t/**< Determines if message can be printed.\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t* \t If this value is set to VS_HIDDEN, no message is printed! */\r\n\t\t} ReturnValueList;\r\n\r\n\r\n\t/*\r\n\t *\tPUBLIC MEMBER FUNCTIONS\r\n\t */\r\n\tpublic:\r\n\t\t/** Default constructor. */\r\n\t\tMessageHandling( );\r\n\r\n\t\t/** Constructor which takes the desired output file. */\r\n\t\tMessageHandling( myFILE* _outputFile\t\t\t\t\t/**< Output file. */\r\n\t\t\t\t\t\t );\r\n\r\n\t\t/** Constructor which takes the desired visibility states. */\r\n\t\tMessageHandling(\tVisibilityStatus _errorVisibility,\t/**< Visibility status for error messages. */\r\n\t\t\t\t\t\t\tVisibilityStatus _warningVisibility,/**< Visibility status for warning messages. */\r\n\t\t\t\t\t\t\tVisibilityStatus _infoVisibility\t/**< Visibility status for info messages. */\r\n\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Constructor which takes the desired output file and desired visibility states. */\r\n\t\tMessageHandling(\tmyFILE* _outputFile,\t\t\t\t/**< Output file. */\r\n\t\t\t\t\t\t\tVisibilityStatus _errorVisibility,\t/**< Visibility status for error messages. */\r\n\t\t\t\t\t\t\tVisibilityStatus _warningVisibility,/**< Visibility status for warning messages. */\r\n\t\t\t\t\t\t\tVisibilityStatus _infoVisibility\t/**< Visibility status for info messages. */\r\n\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Copy constructor (deep copy). */\r\n\t\tMessageHandling(\tconst MessageHandling& rhs\t/**< Rhs object. */\r\n\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Destructor. */\r\n\t\t~MessageHandling( );\r\n\r\n\t\t/** Assignment operator (deep copy). */\r\n\t\tMessageHandling& operator=(\tconst MessageHandling& rhs\t/**< Rhs object. */\r\n\t\t\t\t\t\t\t\t\t);\r\n\r\n\r\n\t\t/** Prints an error message(a simplified macro THROWERROR is also provided). \\n\r\n\t\t * Errors are definied as abnormal events which cause an immediate termination of the current (sub) function.\r\n\t\t * Errors of a sub function should be commented by the calling function by means of a warning message\r\n\t\t * (if this error does not cause an error of the calling function, either)!\r\n\t\t * \\return Error number returned by sub function call\r\n\t\t */\r\n\t\treturnValue throwError(\r\n\t\t\treturnValue Enumber,\t\t\t\t\t/**< Error number returned by sub function call. */\r\n\t\t\tconst char* additionaltext,\t\t\t\t/**< Additional error text (0, if none). */\r\n\t\t\tconst char* functionname,\t\t\t\t/**< Name of function which caused the error. */\r\n\t\t\tconst char* filename,\t\t\t\t\t/**< Name of file which caused the error. */\r\n\t\t\tconst unsigned long linenumber,\t\t\t/**< Number of line which caused the error.incompatible binary file */\r\n\t\t\tVisibilityStatus localVisibilityStatus\t/**< Determines (locally) if error message can be printed to myStderr.\r\n\t\t\t \t\t\t\t\t\t\t\t\t * If GLOBAL visibility status of the message is set to VS_HIDDEN,\r\n\t \t\t\t \t\t\t\t\t\t\t\t * no message is printed, anyway! */\r\n\t\t);\r\n\r\n\t\t/** Prints a warning message (a simplified macro THROWWARNING is also provided).\r\n\t\t * Warnings are definied as abnormal events which does NOT cause an immediate termination of the current (sub) function.\r\n\t\t * \\return Warning number returned by sub function call\r\n\t\t */\r\n\t\treturnValue throwWarning(\r\n\t\t\treturnValue Wnumber,\t \t\t\t\t/**< Warning number returned by sub function call. */\r\n\t\t\tconst char* additionaltext,\t\t\t\t/**< Additional warning text (0, if none). */\r\n\t\t\tconst char* functionname,\t\t\t\t/**< Name of function which caused the warning. */\r\n\t\t\tconst char* filename, \t\t\t\t/**< Name of file which caused the warning. */\r\n\t\t\tconst unsigned long linenumber,\t \t\t/**< Number of line which caused the warning. */\r\n\t\t\tVisibilityStatus localVisibilityStatus\t/**< Determines (locally) if warning message can be printed to myStderr.\r\n\t\t \t\t \t\t\t\t\t\t\t\t\t * If GLOBAL visibility status of the message is set to VS_HIDDEN,\r\n\t\t\t\t\t \t\t\t\t\t\t\t\t * no message is printed, anyway! */\r\n\t\t\t);\r\n\r\n\t /** Prints a info message (a simplified macro THROWINFO is also provided).\r\n\t\t * \\return Info number returned by sub function call\r\n\t\t */\r\n\t\treturnValue throwInfo(\r\n\t\t\treturnValue Inumber,\t \t\t\t\t/**< Info number returned by sub function call. */\r\n\t\t\tconst char* additionaltext,\t \t\t\t/**< Additional warning text (0, if none). */\r\n\t\t\tconst char* functionname,\t\t\t\t/**< Name of function which submitted the info. */\r\n\t\t\tconst char* filename, \t\t\t\t/**< Name of file which submitted the info. */\r\n\t\t\tconst unsigned long linenumber,\t\t\t/**< Number of line which submitted the info. */\r\n\t\t\tVisibilityStatus localVisibilityStatus\t/**< Determines (locally) if info message can be printed to myStderr.\r\n\t\t\t\t \t\t\t\t\t\t\t\t\t * If GLOBAL visibility status of the message is set to VS_HIDDEN,\r\n\t\t\t\t \t\t\t\t\t\t\t\t\t * no message is printed, anyway! */\r\n\t\t\t);\r\n\r\n\r\n\t\t/** Resets all preferences to default values.\r\n\t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\treturnValue reset( );\r\n\r\n\r\n\t\t/** Prints a complete list of all messages to output file.\r\n\t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\treturnValue listAllMessages( );\r\n\r\n\r\n\t\t/** Returns visibility status for error messages.\r\n\t\t *\t\\return Visibility status for error messages. */\r\n\t\tinline VisibilityStatus getErrorVisibilityStatus( ) const;\r\n\r\n\t\t/** Returns visibility status for warning messages.\r\n\t\t *\t\\return Visibility status for warning messages. */\r\n\t\tinline VisibilityStatus getWarningVisibilityStatus( ) const;\r\n\r\n\t\t/** Returns visibility status for info messages.\r\n\t\t *\t\\return Visibility status for info messages. */\r\n\t\tinline VisibilityStatus getInfoVisibilityStatus( ) const;\r\n\r\n\t\t/** Returns pointer to output file.\r\n\t\t *\t\\return Pointer to output file. */\r\n\t\tinline myFILE* getOutputFile( ) const;\r\n\r\n\t\t/** Returns error count value.\r\n\t\t *\t\\return Error count value. */\r\n\t\tinline int getErrorCount( ) const;\r\n\r\n\r\n\t\t/** Changes visibility status for error messages. */\r\n\t\tinline void setErrorVisibilityStatus(\tVisibilityStatus _errorVisibility\t/**< New visibility status for error messages. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Changes visibility status for warning messages. */\r\n\t\tinline void setWarningVisibilityStatus(\tVisibilityStatus _warningVisibility\t/**< New visibility status for warning messages. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Changes visibility status for info messages. */\r\n\t\tinline void setInfoVisibilityStatus(\tVisibilityStatus _infoVisibility\t/**< New visibility status for info messages. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Changes output file for messages. */\r\n\t\tinline void setOutputFile(\tmyFILE* _outputFile\t/**< New output file for messages. */\r\n\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Changes error count.\r\n\t\t * \\return SUCCESSFUL_RETURN \\n\r\n\t\t *\t\t RET_INVALID_ARGUMENT */\r\n\t\tinline returnValue setErrorCount(\tint _errorCount\t/**< New error count value. */\r\n\t\t\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Return the error code string. */\r\n\t\tstatic const char* getErrorString(int error);\r\n\r\n\t/*\r\n\t *\tPROTECTED MEMBER FUNCTIONS\r\n\t */\r\n\tprotected:\r\n\t\t/** Prints a info message to myStderr (auxiliary function).\r\n\t\t * \\return Error/warning/info number returned by sub function call\r\n\t\t */\r\n\t\treturnValue throwMessage(\r\n\t\t\treturnValue RETnumber,\t \t\t\t\t/**< Error/warning/info number returned by sub function call. */\r\n\t\t\tconst char* additionaltext,\t\t\t\t/**< Additional warning text (0, if none). */\r\n\t\t\tconst char* functionname,\t\t\t\t/**< Name of function which caused the error/warning/info. */\r\n\t\t\tconst char* filename, \t\t\t\t/**< Name of file which caused the error/warning/info. */\r\n\t\t\tconst unsigned long linenumber,\t\t\t/**< Number of line which caused the error/warning/info. */\r\n\t\t\tVisibilityStatus localVisibilityStatus,\t/**< Determines (locally) if info message can be printed to myStderr.\r\n\t\t\t\t\t \t\t\t\t\t\t\t\t * If GLOBAL visibility status of the message is set to VS_HIDDEN,\r\n\t\t\t\t \t\t\t\t\t\t\t\t\t * no message is printed, anyway! */\r\n\t\t\tconst char* RETstring\t\t\t\t\t/**< Leading string of error/warning/info message. */\r\n\t\t\t);\r\n\r\n\r\n\t/*\r\n\t *\tPROTECTED MEMBER VARIABLES\r\n\t */\r\n\tprotected:\r\n\t\tVisibilityStatus errorVisibility;\t\t/**< Error messages visible? */\r\n\t\tVisibilityStatus warningVisibility;\t\t/**< Warning messages visible? */\r\n\t\tVisibilityStatus infoVisibility;\t\t/**< Info messages visible? */\r\n\r\n\t\tmyFILE* outputFile;\t\t\t\t\t\t/**< Output file for messages. */\r\n\r\n\t\tint errorCount; \t\t\t\t\t\t/**< Counts number of errors (for nicer output only). */\r\n};\r\n\r\n\r\n#ifndef __FUNCTION__\r\n /** Ensures that __FUNCTION__ macro is defined. */\r\n #define __FUNCTION__ 0\r\n#endif\r\n\r\n#ifndef __FILE__\r\n /** Ensures that __FILE__ macro is defined. */\r\n #define __FILE__ 0\r\n#endif\r\n\r\n#ifndef __LINE__\r\n /** Ensures that __LINE__ macro is defined. */\r\n #define __LINE__ 0\r\n#endif\r\n\r\n\r\n/** Short version of throwError with default values, only returnValue is needed */\r\n#define THROWERROR(retval) ( getGlobalMessageHandler( )->throwError((retval),0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE) )\r\n\r\n/** Short version of throwWarning with default values, only returnValue is needed */\r\n#define THROWWARNING(retval) ( getGlobalMessageHandler( )->throwWarning((retval),0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE) )\r\n\r\n/** Short version of throwInfo with default values, only returnValue is needed */\r\n#define THROWINFO(retval) ( getGlobalMessageHandler( )->throwInfo((retval),0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE) )\r\n\r\n\r\n/** Returns a pointer to global message handler.\r\n * \\return Pointer to global message handler.\r\n */\r\nMessageHandling* getGlobalMessageHandler( );\r\n\r\n\r\n#include \r\n\r\n#endif /* QPOASES_MESSAGEHANDLING_HPP */\r\n\r\n\r\n/*\r\n *\tend of file\r\n */\r\n"
},
{
"path": "externals/qpoases/INCLUDE/QProblem.hpp",
"content": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.\r\n *\r\n *\tqpOASES is free software; you can redistribute it and/or\r\n *\tmodify it under the terms of the GNU Lesser General Public\r\n *\tLicense as published by the Free Software Foundation; either\r\n *\tversion 2.1 of the License, or (at your option) any later version.\r\n *\r\n *\tqpOASES is distributed in the hope that it will be useful,\r\n *\tbut WITHOUT ANY WARRANTY; without even the implied warranty of\r\n *\tMERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU\r\n *\tLesser General Public License for more details.\r\n *\r\n *\tYou should have received a copy of the GNU Lesser General Public\r\n *\tLicense along with qpOASES; if not, write to the Free Software\r\n *\tFoundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA\r\n *\r\n */\r\n\r\n\r\n/**\r\n *\t\\file INCLUDE/QProblem.hpp\r\n *\t\\author Hans Joachim Ferreau\r\n *\t\\version 1.3embedded\r\n *\t\\date 2007-2008\r\n *\r\n *\tDeclaration of the QProblem class which is able to use the newly\r\n *\tdeveloped online active set strategy for parametric quadratic programming.\r\n */\r\n\r\n\r\n\r\n#ifndef QPOASES_QPROBLEM_HPP\r\n#define QPOASES_QPROBLEM_HPP\r\n\r\n\r\n#include \r\n#include \r\n#include \r\n\r\n\r\n/** A class for setting up and solving quadratic programs. The main feature is\r\n *\tthe possibily to use the newly developed online active set strategy for\r\n * \tparametric quadratic programming.\r\n *\r\n *\t\\author Hans Joachim Ferreau\r\n *\t\\version 1.3embedded\r\n *\t\\date 2007-2008\r\n */\r\nclass QProblem : public QProblemB\r\n{\r\n\t/* allow SolutionAnalysis class to access private members */\r\n\tfriend class SolutionAnalysis;\r\n\t\r\n\t/*\r\n\t *\tPUBLIC MEMBER FUNCTIONS\r\n\t */\r\n\tpublic:\r\n\t\t/** Default constructor. */\r\n\t\tQProblem( );\r\n\r\n\t\t/** Constructor which takes the QP dimensions only. */\r\n\t\tQProblem(\tint _nV,\t \t\t\t\t/**< Number of variables. */\r\n\t\t\t\t\tint _nC \t\t\t\t\t/**< Number of constraints. */\r\n\t\t\t\t\t);\r\n\r\n\t\t/** Copy constructor (deep copy). */\r\n\t\tQProblem(\tconst QProblem& rhs\t/**< Rhs object. */\r\n\t\t\t\t\t);\r\n\r\n\t\t/** Destructor. */\r\n\t\t~QProblem( );\r\n\r\n\t\t/** Assignment operator (deep copy). */\r\n\t\tQProblem& operator=(\tconst QProblem& rhs\t\t/**< Rhs object. */\r\n\t\t\t\t\t\t\t\t);\r\n\r\n\r\n\t\t/** Clears all data structures of QProblemB except for QP data.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t\t\t\tRET_RESET_FAILED */\r\n\t\treturnValue reset( );\r\n\r\n\r\n\t\t/** Initialises a QProblem with given QP data and solves it\r\n\t\t *\tusing an initial homotopy with empty working set (at most nWSR iterations).\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t\t\t\tRET_INIT_FAILED \\n\r\n\t\t\t\t\tRET_INIT_FAILED_CHOLESKY \\n\r\n\t\t\t\t\tRET_INIT_FAILED_TQ \\n\r\n\t\t\t\t\tRET_INIT_FAILED_HOTSTART \\n\r\n\t\t\t\t\tRET_INIT_FAILED_INFEASIBILITY \\n\r\n\t\t\t\t\tRET_INIT_FAILED_UNBOUNDEDNESS \\n\r\n\t\t\t\t\tRET_MAX_NWSR_REACHED \\n\r\n\t\t\t\t\tRET_INVALID_ARGUMENTS \\n\r\n\t\t\t\t\tRET_INACCURATE_SOLUTION \\n\r\n\t\t \t\t\tRET_NO_SOLUTION */\r\n\t\treturnValue init(\tconst real_t* const _H, \t\t/**< Hessian matrix. */\r\n\t\t\t\t\t\t\tconst real_t* const _g, \t\t/**< Gradient vector. */\r\n\t\t\t\t\t\t\tconst real_t* const _A, \t\t/**< Constraint matrix. */\r\n\t\t\t\t\t\t\tconst real_t* const _lb,\t\t/**< Lower bound vector (on variables). \\n\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\tIf no lower bounds exist, a NULL pointer can be passed. */\r\n\t\t\t\t\t\t\tconst real_t* const _ub,\t\t/**< Upper bound vector (on variables). \\n\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\tIf no upper bounds exist, a NULL pointer can be passed. */\r\n\t\t\t\t\t\t\tconst real_t* const _lbA,\t\t/**< Lower constraints' bound vector. \\n\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\tIf no lower constraints' bounds exist, a NULL pointer can be passed. */\r\n\t\t\t\t\t\t\tconst real_t* const _ubA,\t\t/**< Upper constraints' bound vector. \\n\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\tIf no lower constraints' bounds exist, a NULL pointer can be passed. */\r\n\t\t\t\t\t\t\tint& nWSR,\t\t\t\t\t\t/**< Input: Maximum number of working set recalculations when using initial homotopy.\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\tOutput: Number of performed working set recalculations. */\r\n\t\t\t\t\t\t\tconst real_t* const yOpt = 0,\t/**< Initial guess for dual solution vector. */\r\n\t\t\t\t\t\t\treal_t* const cputime = 0\t\t/**< Output: CPU time required to initialise QP. */\r\n\t\t\t\t\t\t\t);\r\n\r\n\r\n\t\t/** Initialises a QProblem with given QP data and solves it\r\n\t\t *\tusing an initial homotopy with empty working set (at most nWSR iterations).\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t\t\t\tRET_INIT_FAILED \\n\r\n\t\t\t\t\tRET_INIT_FAILED_CHOLESKY \\n\r\n\t\t\t\t\tRET_INIT_FAILED_TQ \\n\r\n\t\t\t\t\tRET_INIT_FAILED_HOTSTART \\n\r\n\t\t\t\t\tRET_INIT_FAILED_INFEASIBILITY \\n\r\n\t\t\t\t\tRET_INIT_FAILED_UNBOUNDEDNESS \\n\r\n\t\t\t\t\tRET_MAX_NWSR_REACHED \\n\r\n\t\t\t\t\tRET_INVALID_ARGUMENTS \\n\r\n\t\t\t\t\tRET_INACCURATE_SOLUTION \\n\r\n\t\t \t\t\tRET_NO_SOLUTION */\r\n\t\treturnValue init(\tconst real_t* const _H, \t\t/**< Hessian matrix. */\r\n\t\t\t\t\t\t\tconst real_t* const _R, \t\t/**< Cholesky factorization of the Hessian matrix. */\r\n\t\t\t\t\t\t\tconst real_t* const _g, \t\t/**< Gradient vector. */\r\n\t\t\t\t\t\t\tconst real_t* const _A, \t\t/**< Constraint matrix. */\r\n\t\t\t\t\t\t\tconst real_t* const _lb,\t\t/**< Lower bound vector (on variables). \\n\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\tIf no lower bounds exist, a NULL pointer can be passed. */\r\n\t\t\t\t\t\t\tconst real_t* const _ub,\t\t/**< Upper bound vector (on variables). \\n\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\tIf no upper bounds exist, a NULL pointer can be passed. */\r\n\t\t\t\t\t\t\tconst real_t* const _lbA,\t\t/**< Lower constraints' bound vector. \\n\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\tIf no lower constraints' bounds exist, a NULL pointer can be passed. */\r\n\t\t\t\t\t\t\tconst real_t* const _ubA,\t\t/**< Upper constraints' bound vector. \\n\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\tIf no lower constraints' bounds exist, a NULL pointer can be passed. */\r\n\t\t\t\t\t\t\tint& nWSR,\t\t\t\t\t\t/**< Input: Maximum number of working set recalculations when using initial homotopy.\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\tOutput: Number of performed working set recalculations. */\r\n\t\t\t\t\t\t\tconst real_t* const yOpt = 0,\t/**< Initial guess for dual solution vector. */\r\n\t\t\t\t\t\t\treal_t* const cputime = 0\t\t/**< Output: CPU time required to initialise QP. */\r\n\t\t\t\t\t\t\t);\r\n\r\n\r\n\t\t/** Solves QProblem using online active set strategy.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t \t\t\tRET_MAX_NWSR_REACHED \\n\r\n\t\t \t\t\tRET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \\n\r\n\t\t\t\t\tRET_HOTSTART_FAILED \\n\r\n\t\t\t\t\tRET_SHIFT_DETERMINATION_FAILED \\n\r\n\t\t\t\t\tRET_STEPDIRECTION_DETERMINATION_FAILED \\n\r\n\t\t\t\t\tRET_STEPLENGTH_DETERMINATION_FAILED \\n\r\n\t\t\t\t\tRET_HOMOTOPY_STEP_FAILED \\n\r\n\t\t\t\t\tRET_HOTSTART_STOPPED_INFEASIBILITY \\n\r\n\t\t\t\t\tRET_HOTSTART_STOPPED_UNBOUNDEDNESS \\n\r\n\t\t\t\t\tRET_INACCURATE_SOLUTION \\n\r\n\t\t \t\t\tRET_NO_SOLUTION */\r\n\t\treturnValue hotstart(\tconst real_t* const g_new,\t\t/**< Gradient of neighbouring QP to be solved. */\r\n\t\t\t\t\t\t\t\tconst real_t* const lb_new,\t\t/**< Lower bounds of neighbouring QP to be solved. \\n\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t \t\t\t \t If no lower bounds exist, a NULL pointer can be passed. */\r\n\t\t\t\t\t\t\t\tconst real_t* const ub_new,\t\t/**< Upper bounds of neighbouring QP to be solved. \\n\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t \t\t\t \t If no upper bounds exist, a NULL pointer can be passed. */\r\n\t\t\t\t\t\t\t\tconst real_t* const lbA_new,\t/**< Lower constraints' bounds of neighbouring QP to be solved. \\n\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t \t\t\t \t If no lower constraints' bounds exist, a NULL pointer can be passed. */\r\n\t\t\t\t\t\t\t\tconst real_t* const ubA_new,\t/**< Upper constraints' bounds of neighbouring QP to be solved. \\n\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t \t\t\t \t If no upper constraints' bounds exist, a NULL pointer can be passed. */\r\n\t\t\t\t\t\t\t\tint& nWSR,\t\t\t\t\t\t/**< Input: Maximum number of working set recalculations; \\n\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t \t\t Output: Number of performed working set recalculations. */\r\n\t\t\t\t\t\t\t\treal_t* const cputime\t\t\t/**< Output: CPU time required to solve QP (or to perform nWSR iterations). */\r\n\t\t\t\t\t\t\t\t);\r\n\r\n\r\n\t\t/** Returns constraint matrix of the QP (deep copy).\r\n\t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\tinline returnValue getA(\treal_t* const _A\t/**< Array of appropriate dimension for copying constraint matrix.*/\r\n\t\t\t\t\t\t\t\t\t) const;\r\n\r\n\t\t/** Returns a single row of constraint matrix of the QP (deep copy).\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t\t\t\tRET_INDEX_OUT_OF_BOUNDS */\r\n\t\tinline returnValue getA(\tint number,\t\t\t/**< Number of entry to be returned. */\r\n\t\t\t\t\t\t\t\t\treal_t* const row\t/**< Array of appropriate dimension for copying (number)th constraint. */\r\n\t\t\t\t\t\t\t\t\t) const;\r\n\r\n\t\t/** Returns lower constraints' bound vector of the QP (deep copy).\r\n\t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\tinline returnValue getLBA(\treal_t* const _lbA\t/**< Array of appropriate dimension for copying lower constraints' bound vector.*/\r\n\t\t\t\t\t\t\t\t\t) const;\r\n\r\n\t\t/** Returns single entry of lower constraints' bound vector of the QP.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t\t\t\tRET_INDEX_OUT_OF_BOUNDS */\r\n\t\tinline returnValue getLBA(\tint number,\t\t/**< Number of entry to be returned. */\r\n\t\t\t\t\t\t\t\t\treal_t& value\t/**< Output: lbA[number].*/\r\n\t\t\t\t\t\t\t\t\t) const;\r\n\r\n\t\t/** Returns upper constraints' bound vector of the QP (deep copy).\r\n\t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\tinline returnValue getUBA(\treal_t* const _ubA\t/**< Array of appropriate dimension for copying upper constraints' bound vector.*/\r\n\t\t\t\t\t\t\t\t\t) const;\r\n\r\n\t\t/** Returns single entry of upper constraints' bound vector of the QP.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t\t\t\tRET_INDEX_OUT_OF_BOUNDS */\r\n\t\tinline returnValue getUBA(\tint number,\t\t/**< Number of entry to be returned. */\r\n\t\t\t\t\t\t\t\t\treal_t& value\t/**< Output: ubA[number].*/\r\n\t\t\t\t\t\t\t\t\t) const;\r\n\r\n\r\n\t\t/** Returns current constraints object of the QP (deep copy).\r\n\t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\tinline returnValue getConstraints(\tConstraints* const _constraints\t/** Output: Constraints object. */\r\n\t\t\t\t\t\t\t\t\t\t\t) const;\r\n\r\n\r\n\t\t/** Returns the number of constraints.\r\n\t\t *\t\\return Number of constraints. */\r\n\t\tinline int getNC( ) const;\r\n\r\n\t\t/** Returns the number of (implicitly defined) equality constraints.\r\n\t\t *\t\\return Number of (implicitly defined) equality constraints. */\r\n\t\tinline int getNEC( ) const;\r\n\r\n\t\t/** Returns the number of active constraints.\r\n\t\t *\t\\return Number of active constraints. */\r\n\t\tinline int getNAC( );\r\n\r\n\t\t/** Returns the number of inactive constraints.\r\n\t\t *\t\\return Number of inactive constraints. */\r\n\t\tinline int getNIAC( );\r\n\r\n\t\t/** Returns the dimension of null space.\r\n\t\t *\t\\return Dimension of null space. */\r\n\t\tint getNZ( );\r\n\r\n\r\n\t\t/** Returns the dual solution vector (deep copy).\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t\t\t\tRET_QP_NOT_SOLVED */\r\n\t\treturnValue getDualSolution(\treal_t* const yOpt\t/**< Output: Dual solution vector (if QP has been solved). */\r\n\t\t\t\t\t\t\t\t\t\t) const;\r\n\r\n\r\n\t/*\r\n\t *\tPROTECTED MEMBER FUNCTIONS\r\n\t */\r\n\tprotected:\r\n\t\t/** Determines type of constraints and bounds (i.e. implicitly fixed, unbounded etc.).\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t\t\t\tRET_SETUPSUBJECTTOTYPE_FAILED */\r\n\t\treturnValue setupSubjectToType( );\r\n\r\n\t\t/** Computes the Cholesky decomposition R of the projected Hessian (i.e. R^T*R = Z^T*H*Z).\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t *\t\t\tRET_INDEXLIST_CORRUPTED */\r\n\t\treturnValue setupCholeskyDecompositionProjected( );\r\n\r\n\t\t/** Initialises TQ factorisation of A (i.e. A*Q = [0 T]) if NO constraint is active.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t \t\t\tRET_INDEXLIST_CORRUPTED */\r\n\t\treturnValue setupTQfactorisation( );\r\n\r\n\r\n\t\t/** Solves a QProblem whose QP data is assumed to be stored in the member variables.\r\n\t\t * A guess for its primal/dual optimal solution vectors and the corresponding\r\n\t\t * working sets of bounds and constraints can be provided.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t\t\t\tRET_INIT_FAILED \\n\r\n\t\t\t\t\tRET_INIT_FAILED_CHOLESKY \\n\r\n\t\t\t\t\tRET_INIT_FAILED_TQ \\n\r\n\t\t\t\t\tRET_INIT_FAILED_HOTSTART \\n\r\n\t\t\t\t\tRET_INIT_FAILED_INFEASIBILITY \\n\r\n\t\t\t\t\tRET_INIT_FAILED_UNBOUNDEDNESS \\n\r\n\t\t\t\t\tRET_MAX_NWSR_REACHED */\r\n\t\treturnValue solveInitialQP(\tconst real_t* const xOpt,\t\t\t\t\t\t/**< Optimal primal solution vector.\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t *\t A NULL pointer can be passed. */\r\n\t\t\t\t\t\t\t\t\tconst real_t* const yOpt,\t\t\t\t\t\t/**< Optimal dual solution vector.\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t *\t A NULL pointer can be passed. */\r\n\t\t\t\t\t\t\t\t\tconst Bounds* const guessedBounds,\t\t\t\t/**< Guessed working set of bounds for solution (xOpt,yOpt).\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t \t\t\t *\t A NULL pointer can be passed. */\r\n\t\t\t\t\t\t\t\t\tconst Constraints* const guessedConstraints,\t/**< Optimal working set of constraints for solution (xOpt,yOpt).\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t \t\t\t *\t A NULL pointer can be passed. */\r\n\t\t\t\t\t\t\t\t\tint& nWSR, \t\t\t\t\t\t\t\t\t\t/**< Input: Maximum number of working set recalculations; \\n\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t \t\t\t\t\t *\t Output: Number of performed working set recalculations. */\r\n\t\t\t\t\t\t\t\t\treal_t* const cputime\t\t\t\t\t\t\t/**< Output: CPU time required to solve QP (or to perform nWSR iterations). */\r\n\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Obtains the desired working set for the auxiliary initial QP in\r\n\t\t * accordance with the user specifications\r\n\t\t * (assumes that member AX has already been initialised!)\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t\t\t\tRET_OBTAINING_WORKINGSET_FAILED \\n\r\n\t\t\t\t\tRET_INVALID_ARGUMENTS */\r\n\t\treturnValue obtainAuxiliaryWorkingSet(\tconst real_t* const xOpt,\t\t\t\t\t\t/**< Optimal primal solution vector.\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t *\t If a NULL pointer is passed, all entries are assumed to be zero. */\r\n\t\t\t\t\t\t\t\t\t\t\t\tconst real_t* const yOpt,\t\t\t\t\t\t/**< Optimal dual solution vector.\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t *\t If a NULL pointer is passed, all entries are assumed to be zero. */\r\n\t\t\t\t\t\t\t\t\t\t\t\tconst Bounds* const guessedBounds,\t\t\t\t/**< Guessed working set of bounds for solution (xOpt,yOpt). */\r\n\t\t\t\t\t\t\t\t\t\t\t\tconst Constraints* const guessedConstraints,\t/**< Guessed working set for solution (xOpt,yOpt). */\r\n\t\t\t\t\t\t\t\t\t\t\t\tBounds* auxiliaryBounds,\t\t\t\t\t\t/**< Input: Allocated bound object. \\n\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t *\t Ouput: Working set of constraints for auxiliary QP. */\r\n\t\t\t\t\t\t\t\t\t\t\t\tConstraints* auxiliaryConstraints\t\t\t\t/**< Input: Allocated bound object. \\n\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t *\t Ouput: Working set for auxiliary QP. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t) const;\r\n\r\n\t\t/** Setups bound and constraints data structures according to auxiliaryBounds/Constraints.\r\n\t\t * (If the working set shall be setup afresh, make sure that\r\n\t\t * bounds and constraints data structure have been resetted\r\n\t\t * and the TQ factorisation has been initialised!)\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t\t\t\tRET_SETUP_WORKINGSET_FAILED \\n\r\n\t\t\t\t\tRET_INVALID_ARGUMENTS \\n\r\n\t\t\t\t\tRET_UNKNOWN BUG */\r\n\t\treturnValue setupAuxiliaryWorkingSet(\tconst Bounds* const auxiliaryBounds,\t\t\t/**< Working set of bounds for auxiliary QP. */\r\n\t\t\t\t\t\t\t\t\t\t\t\tconst Constraints* const auxiliaryConstraints,\t/**< Working set of constraints for auxiliary QP. */\r\n\t\t\t\t\t\t\t\t\t\t\t\tBooleanType setupAfresh\t\t\t\t\t\t\t/**< Flag indicating if given working set shall be\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t * setup afresh or by updating the current one. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Setups the optimal primal/dual solution of the auxiliary initial QP.\r\n\t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\treturnValue setupAuxiliaryQPsolution(\tconst real_t* const xOpt,\t\t\t/**< Optimal primal solution vector.\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t \t*\t If a NULL pointer is passed, all entries are set to zero. */\r\n\t\t\t\t\t\t\t\t\t\t\t\tconst real_t* const yOpt\t\t\t/**< Optimal dual solution vector.\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t *\t If a NULL pointer is passed, all entries are set to zero. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Setups gradient of the auxiliary initial QP for given\r\n\t\t * optimal primal/dual solution and given initial working set\r\n\t\t * (assumes that members X, Y and BOUNDS, CONSTRAINTS have already been initialised!).\r\n\t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\treturnValue setupAuxiliaryQPgradient( );\r\n\r\n\t\t/** Setups (constraints') bounds of the auxiliary initial QP for given\r\n\t\t * optimal primal/dual solution and given initial working set\r\n\t\t * (assumes that members X, Y and BOUNDS, CONSTRAINTS have already been initialised!).\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t\t\t\tRET_UNKNOWN BUG */\r\n\t\treturnValue setupAuxiliaryQPbounds(\tconst Bounds* const auxiliaryBounds,\t\t\t/**< Working set of bounds for auxiliary QP. */\r\n\t\t\t\t\t\t\t\t\t\t\tconst Constraints* const auxiliaryConstraints,\t/**< Working set of constraints for auxiliary QP. */\r\n\t\t\t\t\t\t\t\t\t\t\tBooleanType useRelaxation\t\t\t\t\t\t/**< Flag indicating if inactive (constraints') bounds shall be relaxed. */\r\n\t\t\t\t\t\t\t\t\t\t\t);\r\n\r\n\r\n\t\t/** Adds a constraint to active set.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t \t\t\tRET_ADDCONSTRAINT_FAILED \\n\r\n\t\t\t\t\tRET_ADDCONSTRAINT_FAILED_INFEASIBILITY \\n\r\n\t\t\t\t\tRET_ENSURELI_FAILED */\r\n\t\treturnValue addConstraint(\tint number,\t\t\t\t\t/**< Number of constraint to be added to active set. */\r\n\t\t\t\t\t\t\t\t\tSubjectToStatus C_status,\t/**< Status of new active constraint. */\r\n\t\t\t\t\t\t\t\t\tBooleanType updateCholesky\t/**< Flag indicating if Cholesky decomposition shall be updated. */\r\n\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Checks if new active constraint to be added is linearly dependent from\r\n\t\t *\tfrom row of the active constraints matrix.\r\n\t\t *\t\\return\t RET_LINEARLY_DEPENDENT \\n\r\n\t\t \t\t\t RET_LINEARLY_INDEPENDENT \\n\r\n\t\t\t\t\t RET_INDEXLIST_CORRUPTED */\r\n\t\treturnValue addConstraint_checkLI(\tint number\t\t\t/**< Number of constraint to be added to active set. */\r\n\t\t\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Ensures linear independence of constraint matrix when a new constraint is added.\r\n\t\t * \tTo this end a bound or constraint is removed simultaneously if necessary.\r\n\t\t *\t\\return\t SUCCESSFUL_RETURN \\n\r\n\t\t \t\t\t RET_LI_RESOLVED \\n\r\n\t\t\t\t\t RET_ENSURELI_FAILED \\n\r\n\t\t\t\t\t RET_ENSURELI_FAILED_TQ \\n\r\n\t\t\t\t\t RET_ENSURELI_FAILED_NOINDEX \\n\r\n\t\t\t\t\t RET_REMOVE_FROM_ACTIVESET */\r\n\t\treturnValue addConstraint_ensureLI(\tint number,\t\t\t\t\t/**< Number of constraint to be added to active set. */\r\n\t\t\t\t\t\t\t\t\t\t\tSubjectToStatus C_status\t/**< Status of new active bound. */\r\n\t\t\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Adds a bound to active set.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t \t\t\tRET_ADDBOUND_FAILED \\n\r\n\t\t\t\t\tRET_ADDBOUND_FAILED_INFEASIBILITY \\n\r\n\t\t\t\t\tRET_ENSURELI_FAILED */\r\n\t\treturnValue addBound(\tint number,\t\t\t\t\t/**< Number of bound to be added to active set. */\r\n\t\t\t\t\t\t\t\tSubjectToStatus B_status,\t/**< Status of new active bound. */\r\n\t\t\t\t\t\t\t\tBooleanType updateCholesky\t/**< Flag indicating if Cholesky decomposition shall be updated. */\r\n\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Checks if new active bound to be added is linearly dependent from\r\n\t\t *\tfrom row of the active constraints matrix.\r\n\t\t *\t\\return\t RET_LINEARLY_DEPENDENT \\n\r\n\t\t \t\t\t RET_LINEARLY_INDEPENDENT */\r\n\t\treturnValue addBound_checkLI(\tint number\t\t\t/**< Number of bound to be added to active set. */\r\n\t\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Ensures linear independence of constraint matrix when a new bound is added.\r\n\t\t *\tTo this end a bound or constraint is removed simultaneously if necessary.\r\n\t\t *\t\\return\t SUCCESSFUL_RETURN \\n\r\n\t\t \t\t\t RET_LI_RESOLVED \\n\r\n\t\t\t\t\t RET_ENSURELI_FAILED \\n\r\n\t\t\t\t\t RET_ENSURELI_FAILED_TQ \\n\r\n\t\t\t\t\t RET_ENSURELI_FAILED_NOINDEX \\n\r\n\t\t\t\t\t RET_REMOVE_FROM_ACTIVESET */\r\n\t\treturnValue addBound_ensureLI(\tint number,\t\t\t\t\t/**< Number of bound to be added to active set. */\r\n\t\t\t\t\t\t\t\t\t\tSubjectToStatus B_status\t/**< Status of new active bound. */\r\n\t\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Removes a constraint from active set.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t \t\t\tRET_CONSTRAINT_NOT_ACTIVE \\n\r\n\t\t\t\t\tRET_REMOVECONSTRAINT_FAILED \\n\r\n\t\t\t\t\tRET_HESSIAN_NOT_SPD */\r\n\t\treturnValue removeConstraint(\tint number,\t\t\t\t\t/**< Number of constraint to be removed from active set. */\r\n\t\t\t\t\t\t\t\t\t\tBooleanType updateCholesky\t/**< Flag indicating if Cholesky decomposition shall be updated. */\r\n\t\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Removes a bounds from active set.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t \t\t\tRET_BOUND_NOT_ACTIVE \\n\r\n\t\t\t\t\tRET_HESSIAN_NOT_SPD \\n\r\n\t\t\t\t\tRET_REMOVEBOUND_FAILED */\r\n\t\treturnValue removeBound(\tint number,\t\t\t\t\t/**< Number of bound to be removed from active set. */\r\n\t\t\t\t\t\t\t\t\tBooleanType updateCholesky\t/**< Flag indicating if Cholesky decomposition shall be updated. */\r\n\t\t\t\t\t\t\t\t\t);\r\n\r\n\r\n\t\t/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t\t\t\tRET_DIV_BY_ZERO */\r\n\t\treturnValue backsolveR(\tconst real_t* const b,\t/**< Right hand side vector. */\r\n\t\t\t\t\t\t\t\tBooleanType transposed,\t/**< Indicates if the transposed system shall be solved. */\r\n\t\t\t\t\t\t\t\treal_t* const a \t\t/**< Output: Solution vector */\r\n\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix. \\n\r\n\t\t * Special variant for the case that this function is called from within \"removeBound()\".\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t\t\t\tRET_DIV_BY_ZERO */\r\n\t\treturnValue backsolveR(\tconst real_t* const b,\t\t/**< Right hand side vector. */\r\n\t\t\t\t\t\t\t\tBooleanType transposed,\t\t/**< Indicates if the transposed system shall be solved. */\r\n\t\t\t\t\t\t\t\tBooleanType removingBound,\t/**< Indicates if function is called from \"removeBound()\". */\r\n\t\t\t\t\t\t\t\treal_t* const a \t\t\t/**< Output: Solution vector */\r\n\t\t\t\t\t\t\t\t);\r\n\r\n\r\n\t\t/** Solves the system Ta = b or T^Ta = b where T is a reverse upper triangular matrix.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t \t\t\tRET_DIV_BY_ZERO */\r\n\t\treturnValue backsolveT(\tconst real_t* const b,\t/**< Right hand side vector. */\r\n\t\t\t\t\t\t\t\tBooleanType transposed,\t/**< Indicates if the transposed system shall be solved. */\r\n\t\t\t\t\t\t\t\treal_t* const a \t\t/**< Output: Solution vector */\r\n\t\t\t\t\t\t\t\t);\r\n\r\n\r\n\t\t/** Determines step direction of the shift of the QP data.\r\n\t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\treturnValue hotstart_determineDataShift(const int* const FX_idx, \t/**< Index array of fixed variables. */\r\n\t\t\t\t\t\t\t\t\t\t\t\tconst int* const AC_idx, \t/**< Index array of active constraints. */\r\n\t\t\t\t\t\t\t\t\t\t\t\tconst real_t* const g_new,\t/**< New gradient vector. */\r\n\t\t\t\t\t\t\t\t\t\t\t\tconst real_t* const lbA_new,/**< New lower constraints' bounds. */\r\n\t\t\t\t\t\t\t\t\t\t\t\tconst real_t* const ubA_new,/**< New upper constraints' bounds. */\r\n\t\t\t\t\t\t\t\t\t\t\t\tconst real_t* const lb_new,\t/**< New lower bounds. */\r\n\t\t\t\t\t\t\t\t\t\t\t\tconst real_t* const ub_new,\t/**< New upper bounds. */\r\n\t\t\t\t\t\t\t\t\t\t\t\treal_t* const delta_g,\t \t/**< Output: Step direction of gradient vector. */\r\n\t\t\t\t\t\t\t\t\t\t\t\treal_t* const delta_lbA,\t/**< Output: Step direction of lower constraints' bounds. */\r\n\t\t\t\t\t\t\t\t\t\t\t\treal_t* const delta_ubA,\t/**< Output: Step direction of upper constraints' bounds. */\r\n\t\t\t\t\t\t\t\t\t\t\t\treal_t* const delta_lb,\t \t/**< Output: Step direction of lower bounds. */\r\n\t\t\t\t\t\t\t\t\t\t\t\treal_t* const delta_ub,\t \t/**< Output: Step direction of upper bounds. */\r\n\t\t\t\t\t\t\t\t\t\t\t\tBooleanType& Delta_bC_isZero,/**< Output: Indicates if active constraints' bounds are to be shifted. */\r\n\t\t\t\t\t\t\t\t\t\t\t\tBooleanType& Delta_bB_isZero/**< Output: Indicates if active bounds are to be shifted. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Determines step direction of the homotopy path.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t \t\t\tRET_STEPDIRECTION_FAILED_TQ \\n\r\n\t\t\t\t\tRET_STEPDIRECTION_FAILED_CHOLESKY */\r\n\t\treturnValue hotstart_determineStepDirection(const int* const FR_idx, \t\t/**< Index array of free variables. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tconst int* const FX_idx, \t\t/**< Index array of fixed variables. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tconst int* const AC_idx, \t\t/**< Index array of active constraints. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_g,\t/**< Step direction of gradient vector. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_lbA,\t/**< Step direction of lower constraints' bounds. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_ubA,\t/**< Step direction of upper constraints' bounds. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_lb,\t/**< Step direction of lower bounds. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_ub,\t/**< Step direction of upper bounds. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tBooleanType Delta_bC_isZero, \t/**< Indicates if active constraints' bounds are to be shifted. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tBooleanType Delta_bB_isZero,\t/**< Indicates if active bounds are to be shifted. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\treal_t* const delta_xFX, \t\t/**< Output: Primal homotopy step direction of fixed variables. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\treal_t* const delta_xFR,\t \t/**< Output: Primal homotopy step direction of free variables. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\treal_t* const delta_yAC, \t\t/**< Output: Dual homotopy step direction of active constraints' multiplier. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\treal_t* const delta_yFX \t\t/**< Output: Dual homotopy step direction of fixed variables' multiplier. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Determines the maximum possible step length along the homotopy path.\r\n\t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\treturnValue hotstart_determineStepLength(\tconst int* const FR_idx, \t\t\t/**< Index array of free variables. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tconst int* const FX_idx, \t\t\t/**< Index array of fixed variables. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tconst int* const AC_idx, \t\t\t/**< Index array of active constraints. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tconst int* const IAC_idx, \t\t\t/**< Index array of inactive constraints. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_lbA,\t\t/**< Step direction of lower constraints' bounds. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_ubA,\t\t/**< Step direction of upper constraints' bounds. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_lb,\t \t/**< Step direction of lower bounds. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_ub,\t \t/**< Step direction of upper bounds. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_xFX, \t\t/**< Primal homotopy step direction of fixed variables. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_xFR,\t\t/**< Primal homotopy step direction of free variables. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_yAC,\t\t/**< Dual homotopy step direction of active constraints' multiplier. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_yFX,\t\t/**< Dual homotopy step direction of fixed variables' multiplier. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\treal_t* const delta_Ax,\t\t\t\t/**< Output: Step in vector Ax. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tint& BC_idx, \t\t\t\t\t\t/**< Output: Index of blocking constraint. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tSubjectToStatus& BC_status,\t\t\t/**< Output: Status of blocking constraint. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tBooleanType& BC_isBound \t\t\t/**< Output: Indicates if blocking constraint is a bound. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Performs a step along the homotopy path (and updates active set).\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t \t\t\t RET_OPTIMAL_SOLUTION_FOUND \\n\r\n\t\t \t\t\t RET_REMOVE_FROM_ACTIVESET_FAILED \\n\r\n\t\t\t\t\t RET_ADD_TO_ACTIVESET_FAILED \\n\r\n\t\t\t\t\t RET_QP_INFEASIBLE */\r\n\t\treturnValue hotstart_performStep(\tconst int* const FR_idx, \t\t\t/**< Index array of free variables. */\r\n\t\t\t\t\t\t\t\t\t\t\tconst int* const FX_idx, \t\t\t/**< Index array of fixed variables. */\r\n\t\t\t\t\t\t\t\t\t\t\tconst int* const AC_idx, \t\t\t/**< Index array of active constraints. */\r\n\t\t\t\t\t\t\t\t\t\t\tconst int* const IAC_idx, \t\t\t/**< Index array of inactive constraints. */\r\n\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_g,\t \t/**< Step direction of gradient vector. */\r\n\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_lbA,\t \t/**< Step direction of lower constraints' bounds. */\r\n\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_ubA,\t \t/**< Step direction of upper constraints' bounds. */\r\n\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_lb,\t \t/**< Step direction of lower bounds. */\r\n\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_ub,\t \t/**< Step direction of upper bounds. */\r\n\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_xFX, \t\t/**< Primal homotopy step direction of fixed variables. */\r\n\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_xFR,\t \t/**< Primal homotopy step direction of free variables. */\r\n\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_yAC,\t \t/**< Dual homotopy step direction of active constraints' multiplier. */\r\n\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_yFX, \t\t/**< Dual homotopy step direction of fixed variables' multiplier. */\r\n\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_Ax, \t\t/**< Step in vector Ax. */\r\n\t\t\t\t\t\t\t\t\t\t\tint BC_idx, \t\t\t\t\t\t/**< Index of blocking constraint. */\r\n\t\t\t\t\t\t\t\t\t\t\tSubjectToStatus BC_status,\t\t\t/**< Status of blocking constraint. */\r\n\t\t\t\t\t\t\t\t\t\t\tBooleanType BC_isBound \t\t\t\t/**< Indicates if blocking constraint is a bound. */\r\n\t\t\t\t\t\t\t\t\t\t\t);\r\n\r\n\r\n\t\t/** Checks if lower/upper (constraints') bounds remain consistent\r\n\t\t * (i.e. if lb <= ub and lbA <= ubA ) during the current step.\r\n\t\t *\t\\return BT_TRUE iff (constraints\") bounds remain consistent\r\n\t\t */\r\n\t\tBooleanType areBoundsConsistent(\tconst real_t* const delta_lb,\t/**< Step direction of lower bounds. */\r\n\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_ub,\t/**< Step direction of upper bounds. */\r\n\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_lbA,\t/**< Step direction of lower constraints' bounds. */\r\n\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_ubA\t/**< Step direction of upper constraints' bounds. */\r\n\t\t\t\t\t\t\t\t\t\t\t) const;\r\n\r\n\r\n\t\t/** Setups internal QP data.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t\t\t\tRET_INVALID_ARGUMENTS */\r\n\t\treturnValue setupQPdata(\tconst real_t* const _H, \t/**< Hessian matrix. */\r\n\t\t\t\t\t\t\t\t\tconst real_t* const _R, \t/**< Cholesky factorization of the Hessian matrix. */\r\n\t\t\t\t\t\t\t\t\tconst real_t* const _g, \t/**< Gradient vector. */\r\n\t\t\t\t\t\t\t\t\tconst real_t* const _A, \t/**< Constraint matrix. */\r\n\t\t\t\t\t\t\t\t\tconst real_t* const _lb,\t/**< Lower bound vector (on variables). \\n\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t If no lower bounds exist, a NULL pointer can be passed. */\r\n\t\t\t\t\t\t\t\t\tconst real_t* const _ub,\t/**< Upper bound vector (on variables). \\n\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t If no upper bounds exist, a NULL pointer can be passed. */\r\n\t\t\t\t\t\t\t\t\tconst real_t* const _lbA,\t/**< Lower constraints' bound vector. \\n\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t If no lower constraints' bounds exist, a NULL pointer can be passed. */\r\n\t\t\t\t\t\t\t\t\tconst real_t* const _ubA\t/**< Upper constraints' bound vector. \\n\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t If no lower constraints' bounds exist, a NULL pointer can be passed. */\r\n\t\t\t\t\t\t\t\t\t);\r\n\r\n\r\n\t\t#ifdef PC_DEBUG /* Define print functions only for debugging! */\r\n\r\n\t\t/** Prints concise information on the current iteration.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n */\r\n\t\treturnValue printIteration(\tint iteration,\t\t\t\t/**< Number of current iteration. */\r\n\t\t\t\t\t\t\t\t\tint BC_idx, \t\t\t\t/**< Index of blocking constraint. */\r\n\t\t\t\t\t\t\t\t\tSubjectToStatus BC_status,\t/**< Status of blocking constraint. */\r\n\t\t\t\t\t\t\t\t\tBooleanType BC_isBound \t\t/**< Indicates if blocking constraint is a bound. */\r\n\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Prints concise information on the current iteration.\r\n\t\t * NOTE: ONLY DEFINED FOR SUPPRESSING A COMPILER WARNING!!\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n */\r\n\t\treturnValue printIteration(\tint iteration,\t\t\t\t/**< Number of current iteration. */\r\n\t\t\t\t\t\t\t\t\tint BC_idx, \t\t\t\t/**< Index of blocking bound. */\r\n\t\t\t\t\t\t\t\t\tSubjectToStatus BC_status\t/**< Status of blocking bound. */\r\n\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t#endif /* PC_DEBUG */\r\n\r\n\r\n\t\t/** Determines the maximum violation of the KKT optimality conditions\r\n\t\t * of the current iterate within the QProblem object.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t * \t\t\tRET_INACCURATE_SOLUTION \\n\r\n\t\t * \t\t\tRET_NO_SOLUTION */\r\n\t\treturnValue checkKKTconditions( );\r\n\r\n\r\n\t\t/** Sets constraint matrix of the QP. \\n\r\n\t\t\t(Remark: Also internal vector Ax is recomputed!)\r\n\t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\tinline returnValue setA(\tconst real_t* const A_new\t/**< New constraint matrix (with correct dimension!). */\r\n\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Changes single row of constraint matrix of the QP. \\n\r\n\t\t\t(Remark: Also correponding component of internal vector Ax is recomputed!)\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t\t\t\tRET_INDEX_OUT_OF_BOUNDS */\r\n\t\tinline returnValue setA(\tint number,\t\t\t\t\t/**< Number of row to be changed. */\r\n\t\t\t\t\t\t\t\t\tconst real_t* const value\t/**< New (number)th constraint (with correct dimension!). */\r\n\t\t\t\t\t\t\t\t\t);\r\n\r\n\r\n\t\t/** Sets constraints' lower bound vector of the QP.\r\n\t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\tinline returnValue setLBA(\tconst real_t* const lbA_new\t/**< New constraints' lower bound vector (with correct dimension!). */\r\n\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Changes single entry of lower constraints' bound vector of the QP.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t\t\t\tRET_INDEX_OUT_OF_BOUNDS */\r\n\t\tinline returnValue setLBA(\tint number,\t\t/**< Number of entry to be changed. */\r\n\t\t\t\t\t\t\t\t\treal_t value\t/**< New value for entry of lower constraints' bound vector (with correct dimension!). */\r\n\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Sets constraints' upper bound vector of the QP.\r\n\t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\tinline returnValue setUBA(\tconst real_t* const ubA_new\t/**< New constraints' upper bound vector (with correct dimension!). */\r\n\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Changes single entry of upper constraints' bound vector of the QP.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t\t\t\tRET_INDEX_OUT_OF_BOUNDS */\r\n\t\tinline returnValue setUBA(\tint number,\t\t/**< Number of entry to be changed. */\r\n\t\t\t\t\t\t\t\t\treal_t value\t/**< New value for entry of upper constraints' bound vector (with correct dimension!). */\r\n\t\t\t\t\t\t\t\t\t);\r\n\r\n\r\n\t/*\r\n\t *\tPROTECTED MEMBER VARIABLES\r\n\t */\r\n\tprotected:\r\n\t\treal_t A[NCMAX_ALLOC*NVMAX];\t\t/**< Constraint matrix. */\r\n\t\treal_t lbA[NCMAX_ALLOC];\t\t\t/**< Lower constraints' bound vector. */\r\n\t\treal_t ubA[NCMAX_ALLOC];\t\t\t/**< Upper constraints' bound vector. */\r\n\r\n\t\tConstraints constraints;\t\t\t/**< Data structure for problem's constraints. */\r\n\r\n\t\treal_t T[NVMAX*NVMAX];\t\t\t\t/**< Reverse triangular matrix, A = [0 T]*Q'. */\r\n\t\treal_t Q[NVMAX*NVMAX];\t\t\t\t/**< Orthonormal quadratic matrix, A = [0 T]*Q'. */\r\n\t\tint sizeT;\t\t\t\t\t\t\t/**< Matrix T is stored in a (sizeT x sizeT) array. */\r\n\r\n\t\treal_t Ax[NCMAX_ALLOC];\t\t\t\t/**< Stores the current product A*x (for increased efficiency only). */\r\n\r\n\t\tCyclingManager cyclingManager;\t\t/**< Data structure for storing (possible) cycling information (NOT YET IMPLEMENTED!). */\r\n};\r\n\r\n\r\n#include \r\n\r\n#endif\t/* QPOASES_QPROBLEM_HPP */\r\n\r\n\r\n/*\r\n *\tend of file\r\n */\r\n"
},
{
"path": "externals/qpoases/INCLUDE/QProblemB.hpp",
"content": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.\r\n *\r\n *\tqpOASES is free software; you can redistribute it and/or\r\n *\tmodify it under the terms of the GNU Lesser General Public\r\n *\tLicense as published by the Free Software Foundation; either\r\n *\tversion 2.1 of the License, or (at your option) any later version.\r\n *\r\n *\tqpOASES is distributed in the hope that it will be useful,\r\n *\tbut WITHOUT ANY WARRANTY; without even the implied warranty of\r\n *\tMERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU\r\n *\tLesser General Public License for more details.\r\n *\r\n *\tYou should have received a copy of the GNU Lesser General Public\r\n *\tLicense along with qpOASES; if not, write to the Free Software\r\n *\tFoundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA\r\n *\r\n */\r\n\r\n\r\n/**\r\n *\t\\file INCLUDE/QProblemB.hpp\r\n *\t\\author Hans Joachim Ferreau\r\n *\t\\version 1.3embedded\r\n *\t\\date 2007-2008\r\n *\r\n *\tDeclaration of the QProblemB class which is able to use the newly\r\n *\tdeveloped online active set strategy for parametric quadratic programming\r\n *\tfor problems with (simple) bounds only.\r\n */\r\n\r\n\r\n\r\n#ifndef QPOASES_QPROBLEMB_HPP\r\n#define QPOASES_QPROBLEMB_HPP\r\n\r\n\r\n#include \r\n\r\n\r\n\r\nclass SolutionAnalysis;\r\n\r\n/** Class for setting up and solving quadratic programs with (simple) bounds only.\r\n *\tThe main feature is the possibily to use the newly developed online active set strategy\r\n *\tfor parametric quadratic programming.\r\n *\r\n *\t\\author Hans Joachim Ferreau\r\n *\t\\version 1.3embedded\r\n *\t\\date 2007-2008\r\n */\r\nclass QProblemB\r\n{\r\n\t/* allow SolutionAnalysis class to access private members */\r\n\tfriend class SolutionAnalysis;\r\n\t\r\n\t/*\r\n\t *\tPUBLIC MEMBER FUNCTIONS\r\n\t */\r\n\tpublic:\r\n\t\t/** Default constructor. */\r\n\t\tQProblemB( );\r\n\r\n\t\t/** Constructor which takes the QP dimension only. */\r\n\t\tQProblemB(\tint _nV\t\t\t\t\t\t/**< Number of variables. */\r\n\t\t\t\t\t);\r\n\r\n\t\t/** Copy constructor (deep copy). */\r\n\t\tQProblemB(\tconst QProblemB& rhs\t/**< Rhs object. */\r\n\t\t\t\t\t);\r\n\r\n\t\t/** Destructor. */\r\n\t\t~QProblemB( );\r\n\r\n\t\t/** Assignment operator (deep copy). */\r\n\t\tQProblemB& operator=(\tconst QProblemB& rhs\t/**< Rhs object. */\r\n\t\t\t\t\t\t\t\t);\r\n\r\n\r\n\t\t/** Clears all data structures of QProblemB except for QP data.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t\t\t\tRET_RESET_FAILED */\r\n\t\treturnValue reset( );\r\n\r\n\r\n\t\t/** Initialises a QProblemB with given QP data and solves it\r\n\t\t *\tusing an initial homotopy with empty working set (at most nWSR iterations).\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t\t\t\tRET_INIT_FAILED \\n\r\n\t\t\t\t\tRET_INIT_FAILED_CHOLESKY \\n\r\n\t\t\t\t\tRET_INIT_FAILED_HOTSTART \\n\r\n\t\t\t\t\tRET_INIT_FAILED_INFEASIBILITY \\n\r\n\t\t\t\t\tRET_INIT_FAILED_UNBOUNDEDNESS \\n\r\n\t\t\t\t\tRET_MAX_NWSR_REACHED \\n\r\n\t\t\t\t\tRET_INVALID_ARGUMENTS \\n\r\n\t\t\t\t\tRET_INACCURATE_SOLUTION \\n\r\n\t\t \t\t\tRET_NO_SOLUTION */\r\n\t\treturnValue init(\tconst real_t* const _H, \t\t/**< Hessian matrix. */\r\n\t\t\t\t\t\t\tconst real_t* const _g,\t\t\t/**< Gradient vector. */\r\n\t\t\t\t\t\t\tconst real_t* const _lb,\t\t/**< Lower bounds (on variables). \\n\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\tIf no lower bounds exist, a NULL pointer can be passed. */\r\n\t\t\t\t\t\t\tconst real_t* const _ub,\t\t/**< Upper bounds (on variables). \\n\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\tIf no upper bounds exist, a NULL pointer can be passed. */\r\n\t\t\t\t\t\t\tint& nWSR, \t\t\t\t\t\t/**< Input: Maximum number of working set recalculations when using initial homotopy. \\n\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\tOutput: Number of performed working set recalculations. */\r\n\t\t\t\t\t\t\tconst real_t* const yOpt = 0,\t/**< Initial guess for dual solution vector. */\r\n\t\t\t\t \t\t\treal_t* const cputime = 0\t\t/**< Output: CPU time required to initialise QP. */\r\n\t\t\t\t\t\t\t);\r\n\r\n\r\n\t\t/** Initialises a QProblemB with given QP data and solves it\r\n\t\t *\tusing an initial homotopy with empty working set (at most nWSR iterations).\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t\t\t\tRET_INIT_FAILED \\n\r\n\t\t\t\t\tRET_INIT_FAILED_CHOLESKY \\n\r\n\t\t\t\t\tRET_INIT_FAILED_HOTSTART \\n\r\n\t\t\t\t\tRET_INIT_FAILED_INFEASIBILITY \\n\r\n\t\t\t\t\tRET_INIT_FAILED_UNBOUNDEDNESS \\n\r\n\t\t\t\t\tRET_MAX_NWSR_REACHED \\n\r\n\t\t\t\t\tRET_INVALID_ARGUMENTS \\n\r\n\t\t\t\t\tRET_INACCURATE_SOLUTION \\n\r\n\t\t \t\t\tRET_NO_SOLUTION */\r\n\t\treturnValue init(\tconst real_t* const _H, \t\t/**< Hessian matrix. */\r\n\t\t\t\t\t\t\tconst real_t* const _R, \t\t/**< Cholesky factorization of the Hessian matrix. */\r\n\t\t\t\t\t\t\tconst real_t* const _g,\t\t\t/**< Gradient vector. */\r\n\t\t\t\t\t\t\tconst real_t* const _lb,\t\t/**< Lower bounds (on variables). \\n\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\tIf no lower bounds exist, a NULL pointer can be passed. */\r\n\t\t\t\t\t\t\tconst real_t* const _ub,\t\t/**< Upper bounds (on variables). \\n\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\tIf no upper bounds exist, a NULL pointer can be passed. */\r\n\t\t\t\t\t\t\tint& nWSR, \t\t\t\t\t\t/**< Input: Maximum number of working set recalculations when using initial homotopy. \\n\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\tOutput: Number of performed working set recalculations. */\r\n\t\t\t\t\t\t\tconst real_t* const yOpt = 0,\t/**< Initial guess for dual solution vector. */\r\n\t\t\t\t \t\t\treal_t* const cputime = 0\t\t/**< Output: CPU time required to initialise QP. */\r\n\t\t\t\t\t\t\t);\r\n\r\n\r\n\t\t/** Solves an initialised QProblemB using online active set strategy.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t\t\t\tRET_MAX_NWSR_REACHED \\n\r\n\t\t\t\t\tRET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \\n\r\n\t\t\t\t\tRET_HOTSTART_FAILED \\n\r\n\t\t\t\t\tRET_SHIFT_DETERMINATION_FAILED \\n\r\n\t\t\t\t\tRET_STEPDIRECTION_DETERMINATION_FAILED \\n\r\n\t\t\t\t\tRET_STEPLENGTH_DETERMINATION_FAILED \\n\r\n\t\t\t\t\tRET_HOMOTOPY_STEP_FAILED \\n\r\n\t\t\t\t\tRET_HOTSTART_STOPPED_INFEASIBILITY \\n\r\n\t\t\t\t\tRET_HOTSTART_STOPPED_UNBOUNDEDNESS \\n\r\n\t\t\t\t\tRET_INACCURATE_SOLUTION \\n\r\n\t\t \t\t\tRET_NO_SOLUTION */\r\n\t\treturnValue hotstart(\tconst real_t* const g_new,\t/**< Gradient of neighbouring QP to be solved. */\r\n\t\t\t\t\t\t\t\tconst real_t* const lb_new,\t/**< Lower bounds of neighbouring QP to be solved. \\n\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t \t\t\t If no lower bounds exist, a NULL pointer can be passed. */\r\n\t\t\t\t\t\t\t\tconst real_t* const ub_new,\t/**< Upper bounds of neighbouring QP to be solved. \\n\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t \t\t\t If no upper bounds exist, a NULL pointer can be passed. */\r\n\t\t\t\t\t\t\t\tint& nWSR,\t\t\t\t\t/**< Input: Maximum number of working set recalculations; \\n\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t Output: Number of performed working set recalculations. */\r\n\t\t\t\t\t\t\t\treal_t* const cputime\t\t/**< Output: CPU time required to solve QP (or to perform nWSR iterations). */\r\n\t\t\t\t\t\t\t\t);\r\n\r\n\r\n\t\t/** Returns Hessian matrix of the QP (deep copy).\r\n\t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\tinline returnValue getH(\treal_t* const _H\t/**< Array of appropriate dimension for copying Hessian matrix.*/\r\n\t\t\t\t\t\t\t\t\t) const;\r\n\r\n\t\t/** Returns gradient vector of the QP (deep copy).\r\n\t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\tinline returnValue getG(\treal_t* const _g\t/**< Array of appropriate dimension for copying gradient vector.*/\r\n\t\t\t\t\t\t\t\t\t) const;\r\n\r\n\t\t/** Returns lower bound vector of the QP (deep copy).\r\n\t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\tinline returnValue getLB(\treal_t* const _lb\t/**< Array of appropriate dimension for copying lower bound vector.*/\r\n\t\t\t\t\t\t\t\t\t) const;\r\n\r\n\t\t/** Returns single entry of lower bound vector of the QP.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t\t\t\tRET_INDEX_OUT_OF_BOUNDS */\r\n\t\tinline returnValue getLB(\tint number,\t\t/**< Number of entry to be returned. */\r\n\t\t\t\t\t\t\t\t\treal_t& value\t/**< Output: lb[number].*/\r\n\t\t\t\t\t\t\t\t\t) const;\r\n\r\n\t\t/** Returns upper bound vector of the QP (deep copy).\r\n\t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\tinline returnValue getUB(\treal_t* const _ub\t/**< Array of appropriate dimension for copying upper bound vector.*/\r\n\t\t\t\t\t\t\t\t\t) const;\r\n\r\n\t\t/** Returns single entry of upper bound vector of the QP.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t\t\t\tRET_INDEX_OUT_OF_BOUNDS */\r\n\t\tinline returnValue getUB(\tint number,\t\t/**< Number of entry to be returned. */\r\n\t\t\t\t\t\t\t\t\treal_t& value\t/**< Output: ub[number].*/\r\n\t\t\t\t\t\t\t\t\t) const;\r\n\r\n\r\n\t\t/** Returns current bounds object of the QP (deep copy).\r\n\t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\tinline returnValue getBounds(\tBounds* const _bounds\t/** Output: Bounds object. */\r\n\t\t\t\t\t\t\t\t\t\t) const;\r\n\r\n\r\n\t\t/** Returns the number of variables.\r\n\t\t *\t\\return Number of variables. */\r\n\t\tinline int getNV( ) const;\r\n\r\n\t\t/** Returns the number of free variables.\r\n\t\t *\t\\return Number of free variables. */\r\n\t\tinline int getNFR( );\r\n\r\n\t\t/** Returns the number of fixed variables.\r\n\t\t *\t\\return Number of fixed variables. */\r\n\t\tinline int getNFX( );\r\n\r\n\t\t/** Returns the number of implicitly fixed variables.\r\n\t\t *\t\\return Number of implicitly fixed variables. */\r\n\t\tinline int getNFV( ) const;\r\n\r\n\t\t/** Returns the dimension of null space.\r\n\t\t *\t\\return Dimension of null space. */\r\n\t\tint getNZ( );\r\n\r\n\r\n\t\t/** Returns the optimal objective function value.\r\n\t\t *\t\\return finite value: Optimal objective function value (QP was solved) \\n\r\n\t\t \t\t\t+infinity:\t QP was not yet solved */\r\n\t\treal_t getObjVal( ) const;\r\n\r\n\t\t/** Returns the objective function value at an arbitrary point x.\r\n\t\t *\t\\return Objective function value at point x */\r\n\t\treal_t getObjVal(\tconst real_t* const _x\t/**< Point at which the objective function shall be evaluated. */\r\n\t\t\t\t\t\t\t) const;\r\n\r\n\t\t/** Returns the primal solution vector.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t\t\t\tRET_QP_NOT_SOLVED */\r\n\t\treturnValue getPrimalSolution(\treal_t* const xOpt\t\t\t/**< Output: Primal solution vector (if QP has been solved). */\r\n\t\t\t\t\t\t\t\t\t\t) const;\r\n\r\n\t\t/** Returns the dual solution vector.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t\t\t\tRET_QP_NOT_SOLVED */\r\n\t\treturnValue getDualSolution(\treal_t* const yOpt\t/**< Output: Dual solution vector (if QP has been solved). */\r\n\t\t\t\t\t\t\t\t\t\t) const;\r\n\r\n\r\n\t\t/** Returns status of the solution process.\r\n\t\t *\t\\return Status of solution process. */\r\n\t\tinline QProblemStatus getStatus( ) const;\r\n\r\n\r\n\t\t/** Returns if the QProblem object is initialised.\r\n\t\t *\t\\return BT_TRUE: QProblemB initialised \\n\r\n\t\t \t\t\tBT_FALSE: QProblemB not initialised */\r\n\t\tinline BooleanType isInitialised( ) const;\r\n\r\n\t\t/** Returns if the QP has been solved.\r\n\t\t *\t\\return BT_TRUE: QProblemB solved \\n\r\n\t\t \t\t\tBT_FALSE: QProblemB not solved */\r\n\t\tinline BooleanType isSolved( ) const;\r\n\r\n\t\t/** Returns if the QP is infeasible.\r\n\t\t *\t\\return BT_TRUE: QP infeasible \\n\r\n\t\t \t\t\tBT_FALSE: QP feasible (or not known to be infeasible!) */\r\n\t\tinline BooleanType isInfeasible( ) const;\r\n\r\n\t\t/** Returns if the QP is unbounded.\r\n\t\t *\t\\return BT_TRUE: QP unbounded \\n\r\n\t\t \t\t\tBT_FALSE: QP unbounded (or not known to be unbounded!) */\r\n\t\tinline BooleanType isUnbounded( ) const;\r\n\r\n\r\n\t\t/** Returns the print level.\r\n\t\t *\t\\return Print level. */\r\n\t\tinline PrintLevel getPrintLevel( ) const;\r\n\r\n\t\t/** Changes the print level.\r\n \t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\treturnValue setPrintLevel(\tPrintLevel _printlevel\t/**< New print level. */\r\n\t\t\t\t\t\t\t\t\t);\r\n\r\n\r\n\t\t/** Returns Hessian type flag (type is not determined due to this call!).\r\n\t\t *\t\\return Hessian type. */\r\n\t\tinline HessianType getHessianType( ) const;\r\n\r\n\t\t/** Changes the print level.\r\n \t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\tinline returnValue setHessianType(\tHessianType _hessianType /**< New Hessian type. */\r\n\t\t\t\t\t\t\t\t\t\t\t);\r\n\r\n\r\n\t/*\r\n\t *\tPROTECTED MEMBER FUNCTIONS\r\n\t */\r\n\tprotected:\r\n\t\t/** Checks if Hessian happens to be the identity matrix,\r\n\t\t * and sets corresponding status flag (otherwise the flag remains unaltered!).\r\n\t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\treturnValue checkForIdentityHessian( );\r\n\r\n\t\t/** Determines type of constraints and bounds (i.e. implicitly fixed, unbounded etc.).\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t\t\t\tRET_SETUPSUBJECTTOTYPE_FAILED */\r\n\t\treturnValue setupSubjectToType( );\r\n\r\n\t\t/** Computes the Cholesky decomposition R of the (simply projected) Hessian (i.e. R^T*R = Z^T*H*Z).\r\n\t\t * It only works in the case where Z is a simple projection matrix!\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t *\t\t\tRET_INDEXLIST_CORRUPTED */\r\n\t\treturnValue setupCholeskyDecomposition( );\r\n\r\n\r\n\t\t/** Solves a QProblemB whose QP data is assumed to be stored in the member variables.\r\n\t\t * A guess for its primal/dual optimal solution vectors and the corresponding\r\n\t\t * optimal working set can be provided.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t\t\t\tRET_INIT_FAILED \\n\r\n\t\t\t\t\tRET_INIT_FAILED_CHOLESKY \\n\r\n\t\t\t\t\tRET_INIT_FAILED_HOTSTART \\n\r\n\t\t\t\t\tRET_INIT_FAILED_INFEASIBILITY \\n\r\n\t\t\t\t\tRET_INIT_FAILED_UNBOUNDEDNESS \\n\r\n\t\t\t\t\tRET_MAX_NWSR_REACHED */\r\n\t\treturnValue solveInitialQP(\tconst real_t* const xOpt,\t\t\t/**< Optimal primal solution vector.\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t *\t A NULL pointer can be passed. */\r\n\t\t\t\t\t\t\t\t\tconst real_t* const yOpt,\t\t\t/**< Optimal dual solution vector.\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t *\t A NULL pointer can be passed. */\r\n\t\t\t\t\t\t\t\t\tconst Bounds* const guessedBounds,\t/**< Guessed working set for solution (xOpt,yOpt).\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t *\t A NULL pointer can be passed. */\r\n\t\t\t\t\t\t\t\t\tint& nWSR, \t\t\t\t\t\t\t/**< Input: Maximum number of working set recalculations; \\n\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t \t\t *\t Output: Number of performed working set recalculations. */\r\n\t\t\t\t\t\t\t\t\treal_t* const cputime\t\t\t\t/**< Output: CPU time required to solve QP (or to perform nWSR iterations). */\r\n\t\t\t\t\t\t\t\t\t);\r\n\r\n\r\n\t\t/** Obtains the desired working set for the auxiliary initial QP in\r\n\t\t * accordance with the user specifications\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t\t\t\tRET_OBTAINING_WORKINGSET_FAILED \\n\r\n\t\t\t\t\tRET_INVALID_ARGUMENTS */\r\n\t\treturnValue obtainAuxiliaryWorkingSet(\tconst real_t* const xOpt,\t\t\t/**< Optimal primal solution vector.\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t *\t If a NULL pointer is passed, all entries are assumed to be zero. */\r\n\t\t\t\t\t\t\t\t\t\t\t\tconst real_t* const yOpt,\t\t\t/**< Optimal dual solution vector.\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t *\t If a NULL pointer is passed, all entries are assumed to be zero. */\r\n\t\t\t\t\t\t\t\t\t\t\t\tconst Bounds* const guessedBounds,\t/**< Guessed working set for solution (xOpt,yOpt). */\r\n\t\t\t\t\t\t\t\t\t\t\t\tBounds* auxiliaryBounds\t\t\t\t/**< Input: Allocated bound object. \\n\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t *\t Ouput: Working set for auxiliary QP. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t) const;\r\n\r\n\t\t/** Setups bound data structure according to auxiliaryBounds.\r\n\t\t * (If the working set shall be setup afresh, make sure that\r\n\t\t * bounds data structure has been resetted!)\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t\t\t\tRET_SETUP_WORKINGSET_FAILED \\n\r\n\t\t\t\t\tRET_INVALID_ARGUMENTS \\n\r\n\t\t\t\t\tRET_UNKNOWN BUG */\r\n\t\treturnValue setupAuxiliaryWorkingSet(\tconst Bounds* const auxiliaryBounds,\t/**< Working set for auxiliary QP. */\r\n\t\t\t\t\t\t\t\t\t\t\t\tBooleanType setupAfresh\t\t\t\t\t/**< Flag indicating if given working set shall be\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t * setup afresh or by updating the current one. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Setups the optimal primal/dual solution of the auxiliary initial QP.\r\n\t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\treturnValue setupAuxiliaryQPsolution(\tconst real_t* const xOpt,\t\t\t/**< Optimal primal solution vector.\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t \t*\t If a NULL pointer is passed, all entries are set to zero. */\r\n\t\t\t\t\t\t\t\t\t\t\t\tconst real_t* const yOpt\t\t\t/**< Optimal dual solution vector.\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t *\t If a NULL pointer is passed, all entries are set to zero. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Setups gradient of the auxiliary initial QP for given\r\n\t\t * optimal primal/dual solution and given initial working set\r\n\t\t * (assumes that members X, Y and BOUNDS have already been initialised!).\r\n\t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\treturnValue setupAuxiliaryQPgradient( );\r\n\r\n\t\t/** Setups bounds of the auxiliary initial QP for given\r\n\t\t * optimal primal/dual solution and given initial working set\r\n\t\t * (assumes that members X, Y and BOUNDS have already been initialised!).\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t\t\t\tRET_UNKNOWN BUG */\r\n\t\treturnValue setupAuxiliaryQPbounds( BooleanType useRelaxation\t/**< Flag indicating if inactive bounds shall be relaxed. */\r\n\t\t\t\t\t\t\t\t\t\t\t);\r\n\r\n\r\n\t\t/** Adds a bound to active set (specialised version for the case where no constraints exist).\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t \t\t\tRET_ADDBOUND_FAILED */\r\n\t\treturnValue addBound(\tint number,\t\t\t\t\t/**< Number of bound to be added to active set. */\r\n\t\t\t\t\t\t\t\tSubjectToStatus B_status,\t/**< Status of new active bound. */\r\n\t\t\t\t\t\t\t\tBooleanType updateCholesky\t/**< Flag indicating if Cholesky decomposition shall be updated. */\r\n\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Removes a bounds from active set (specialised version for the case where no constraints exist).\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t\t\t\tRET_HESSIAN_NOT_SPD \\n\r\n\t\t\t\t\tRET_REMOVEBOUND_FAILED */\r\n\t\treturnValue removeBound(\tint number,\t\t\t\t\t/**< Number of bound to be removed from active set. */\r\n\t\t\t\t\t\t\t\t\tBooleanType updateCholesky\t/**< Flag indicating if Cholesky decomposition shall be updated. */\r\n\t\t\t\t\t\t\t\t\t);\r\n\r\n\r\n\t\t/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t\t\t\tRET_DIV_BY_ZERO */\r\n\t\treturnValue backsolveR(\tconst real_t* const b,\t/**< Right hand side vector. */\r\n\t\t\t\t\t\t\t\tBooleanType transposed,\t/**< Indicates if the transposed system shall be solved. */\r\n\t\t\t\t\t\t\t\treal_t* const a \t\t/**< Output: Solution vector */\r\n\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix. \\n\r\n\t\t * Special variant for the case that this function is called from within \"removeBound()\".\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t\t\t\tRET_DIV_BY_ZERO */\r\n\t\treturnValue backsolveR(\tconst real_t* const b,\t\t/**< Right hand side vector. */\r\n\t\t\t\t\t\t\t\tBooleanType transposed,\t\t/**< Indicates if the transposed system shall be solved. */\r\n\t\t\t\t\t\t\t\tBooleanType removingBound,\t/**< Indicates if function is called from \"removeBound()\". */\r\n\t\t\t\t\t\t\t\treal_t* const a \t\t\t/**< Output: Solution vector */\r\n\t\t\t\t\t\t\t\t);\r\n\r\n\r\n\t\t/** Determines step direction of the shift of the QP data.\r\n\t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\treturnValue hotstart_determineDataShift(const int* const FX_idx, \t/**< Index array of fixed variables. */\r\n\t\t\t\t\t\t\t\t\t\t\t\tconst real_t* const g_new,\t/**< New gradient vector. */\r\n\t\t\t\t\t\t\t\t\t\t\t\tconst real_t* const lb_new,\t/**< New lower bounds. */\r\n\t\t\t\t\t\t\t\t\t\t\t\tconst real_t* const ub_new,\t/**< New upper bounds. */\r\n\t\t\t\t\t\t\t\t\t\t\t\treal_t* const delta_g,\t \t/**< Output: Step direction of gradient vector. */\r\n\t\t\t\t\t\t\t\t\t\t\t\treal_t* const delta_lb,\t \t/**< Output: Step direction of lower bounds. */\r\n\t\t\t\t\t\t\t\t\t\t\t\treal_t* const delta_ub,\t \t/**< Output: Step direction of upper bounds. */\r\n\t\t\t\t\t\t\t\t\t\t\t\tBooleanType& Delta_bB_isZero/**< Output: Indicates if active bounds are to be shifted. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t);\r\n\r\n\r\n\t\t/** Checks if lower/upper bounds remain consistent\r\n\t\t * (i.e. if lb <= ub) during the current step.\r\n\t\t *\t\\return BT_TRUE iff bounds remain consistent\r\n\t\t */\r\n\t\tBooleanType areBoundsConsistent(\tconst real_t* const delta_lb,\t\t/**< Step direction of lower bounds. */\r\n\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_ub\t\t/**< Step direction of upper bounds. */\r\n\t\t\t\t\t\t\t\t\t\t\t) const;\r\n\r\n\r\n\t\t/** Setups internal QP data.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t\t\t\tRET_INVALID_ARGUMENTS */\r\n\t\treturnValue setupQPdata(\tconst real_t* const _H, \t/**< Hessian matrix. */\r\n\t\t\t\t\t\t\t\t\tconst real_t* const _R, \t/**< Cholesky factorization of the Hessian matrix. */\r\n\t\t\t\t\t\t\t\t\tconst real_t* const _g,\t\t/**< Gradient vector. */\r\n\t\t\t\t\t\t\t\t\tconst real_t* const _lb,\t/**< Lower bounds (on variables). \\n\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t If no lower bounds exist, a NULL pointer can be passed. */\r\n\t\t\t\t\t\t\t\t\tconst real_t* const _ub\t\t/**< Upper bounds (on variables). \\n\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t If no upper bounds exist, a NULL pointer can be passed. */\r\n\t\t\t\t\t\t\t\t\t);\r\n\r\n\r\n\t\t/** Sets Hessian matrix of the QP.\r\n\t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\tinline returnValue setH(\tconst real_t* const H_new\t/**< New Hessian matrix (with correct dimension!). */\r\n\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Changes gradient vector of the QP.\r\n\t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\tinline returnValue setG(\tconst real_t* const g_new\t/**< New gradient vector (with correct dimension!). */\r\n\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Changes lower bound vector of the QP.\r\n\t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\tinline returnValue setLB(\tconst real_t* const lb_new\t/**< New lower bound vector (with correct dimension!). */\r\n\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Changes single entry of lower bound vector of the QP.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t\t\t\tRET_INDEX_OUT_OF_BOUNDS */\r\n\t\tinline returnValue setLB(\tint number,\t\t/**< Number of entry to be changed. */\r\n\t\t\t\t\t\t\t\t\treal_t value\t/**< New value for entry of lower bound vector. */\r\n\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Changes upper bound vector of the QP.\r\n\t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\tinline returnValue setUB(\tconst real_t* const ub_new\t/**< New upper bound vector (with correct dimension!). */\r\n\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Changes single entry of upper bound vector of the QP.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t\t\t\tRET_INDEX_OUT_OF_BOUNDS */\r\n\t\tinline returnValue setUB(\tint number,\t\t/**< Number of entry to be changed. */\r\n\t\t\t\t\t\t\t\t\treal_t value\t/**< New value for entry of upper bound vector. */\r\n\t\t\t\t\t\t\t\t\t);\r\n\r\n\r\n\t\t/** Computes parameters for the Givens matrix G for which [x,y]*G = [z,0]\r\n\t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\tinline void computeGivens(\treal_t xold,\t/**< Matrix entry to be normalised. */\r\n\t\t\t\t\t\t\t\t\treal_t yold,\t/**< Matrix entry to be annihilated. */\r\n\t\t\t\t\t\t\t\t\treal_t& xnew,\t/**< Output: Normalised matrix entry. */\r\n\t\t\t\t\t\t\t\t\treal_t& ynew,\t/**< Output: Annihilated matrix entry. */\r\n\t\t\t\t\t\t\t\t\treal_t& c,\t\t/**< Output: Cosine entry of Givens matrix. */\r\n\t\t\t\t\t\t\t\t\treal_t& s \t\t/**< Output: Sine entry of Givens matrix. */\r\n\t\t\t\t\t\t\t\t\t) const;\r\n\r\n\t\t/** Applies Givens matrix determined by c and s (cf. computeGivens).\r\n\t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\tinline void applyGivens(\treal_t c,\t\t/**< Cosine entry of Givens matrix. */\r\n\t\t\t\t\t\t\t\t\treal_t s,\t\t/**< Sine entry of Givens matrix. */\r\n\t\t\t\t\t\t\t\t\treal_t xold,\t/**< Matrix entry to be transformed corresponding to\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t *\t the normalised entry of the original matrix. */\r\n\t\t\t\t\t\t\t\t\treal_t yold, \t/**< Matrix entry to be transformed corresponding to\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t *\t the annihilated entry of the original matrix. */\r\n\t\t\t\t\t\t\t\t\treal_t& xnew,\t/**< Output: Transformed matrix entry corresponding to\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t *\t the normalised entry of the original matrix. */\r\n\t\t\t\t\t\t\t\t\treal_t& ynew\t/**< Output: Transformed matrix entry corresponding to\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t *\t the annihilated entry of the original matrix. */\r\n\t\t\t\t\t\t\t\t\t) const;\r\n\r\n\r\n\t/*\r\n\t *\tPRIVATE MEMBER FUNCTIONS\r\n\t */\r\n\tprivate:\r\n\t\t/** Determines step direction of the homotopy path.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t \t\t\tRET_STEPDIRECTION_FAILED_CHOLESKY */\r\n\t\treturnValue hotstart_determineStepDirection(const int* const FR_idx, \t\t/**< Index array of free variables. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tconst int* const FX_idx, \t\t/**< Index array of fixed variables. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_g,\t/**< Step direction of gradient vector. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_lb,\t/**< Step direction of lower bounds. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_ub,\t/**< Step direction of upper bounds. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tBooleanType Delta_bB_isZero,\t/**< Indicates if active bounds are to be shifted. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\treal_t* const delta_xFX, \t\t/**< Output: Primal homotopy step direction of fixed variables. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\treal_t* const delta_xFR,\t \t/**< Output: Primal homotopy step direction of free variables. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\treal_t* const delta_yFX \t\t/**< Output: Dual homotopy step direction of fixed variables' multiplier. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Determines the maximum possible step length along the homotopy path.\r\n\t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\treturnValue hotstart_determineStepLength(\tconst int* const FR_idx, \t\t/**< Index array of free variables. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tconst int* const FX_idx, \t\t/**< Index array of fixed variables. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_lb,\t/**< Step direction of lower bounds. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_ub,\t/**< Step direction of upper bounds. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_xFR,\t/**< Primal homotopy step direction of free variables. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_yFX,\t/**< Dual homotopy step direction of fixed variables' multiplier. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tint& BC_idx, \t\t\t\t\t/**< Output: Index of blocking constraint. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tSubjectToStatus& BC_status\t\t/**< Output: Status of blocking constraint. */\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Performs a step along the homotopy path (and updates active set).\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t \t\t\t RET_OPTIMAL_SOLUTION_FOUND \\n\r\n\t\t \t\t\t RET_REMOVE_FROM_ACTIVESET_FAILED \\n\r\n\t\t\t\t\t RET_ADD_TO_ACTIVESET_FAILED \\n\r\n\t\t\t\t\t RET_QP_INFEASIBLE */\r\n\t\treturnValue hotstart_performStep(\tconst int* const FR_idx, \t\t\t/**< Index array of free variables. */\r\n\t\t\t\t\t\t\t\t\t\t\tconst int* const FX_idx, \t\t\t/**< Index array of fixed variables. */\r\n\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_g,\t \t/**< Step direction of gradient vector. */\r\n\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_lb,\t \t/**< Step direction of lower bounds. */\r\n\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_ub,\t \t/**< Step direction of upper bounds. */\r\n\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_xFX, \t\t/**< Primal homotopy step direction of fixed variables. */\r\n\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_xFR,\t \t/**< Primal homotopy step direction of free variables. */\r\n\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_yFX, \t\t/**< Dual homotopy step direction of fixed variables' multiplier. */\r\n\t\t\t\t\t\t\t\t\t\t\tint BC_idx, \t\t\t\t\t\t/**< Index of blocking constraint. */\r\n\t\t\t\t\t\t\t\t\t\t\tSubjectToStatus BC_status \t\t\t/**< Status of blocking constraint. */\r\n\t\t\t\t\t\t\t\t\t\t\t);\r\n\r\n\r\n\t\t#ifdef PC_DEBUG /* Define print functions only for debugging! */\r\n\r\n\t\t/** Prints concise information on the current iteration.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n */\r\n\t\treturnValue printIteration(\tint iteration,\t\t\t\t/**< Number of current iteration. */\r\n\t\t\t\t\t\t\t\t\tint BC_idx, \t\t\t\t/**< Index of blocking bound. */\r\n\t\t\t\t\t\t\t\t\tSubjectToStatus BC_status\t/**< Status of blocking bound. */\r\n\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t#endif /* PC_DEBUG */\r\n\r\n\r\n\t\t/** Determines the maximum violation of the KKT optimality conditions\r\n\t\t * of the current iterate within the QProblemB object.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t * \t\t\tRET_INACCURATE_SOLUTION \\n\r\n\t\t * \t\t\tRET_NO_SOLUTION */\r\n\t\treturnValue checkKKTconditions( );\r\n\r\n\r\n\t/*\r\n\t *\tPROTECTED MEMBER VARIABLES\r\n\t */\r\n\tprotected:\r\n\t\treal_t H[NVMAX*NVMAX];\t\t/**< Hessian matrix. */\r\n\t\tBooleanType hasHessian;\t\t/**< Flag indicating whether H contains Hessian or corresponding Cholesky factor R; \\sa init. */\r\n\r\n\t\treal_t g[NVMAX];\t\t\t/**< Gradient. */\r\n\t\treal_t lb[NVMAX];\t\t\t/**< Lower bound vector (on variables). */\r\n\t\treal_t ub[NVMAX];\t\t\t/**< Upper bound vector (on variables). */\r\n\r\n\t\tBounds bounds;\t\t\t\t/**< Data structure for problem's bounds. */\r\n\r\n\t\treal_t R[NVMAX*NVMAX];\t\t/**< Cholesky decomposition of H (i.e. H = R^T*R). */\r\n\t\tBooleanType hasCholesky;\t/**< Flag indicating whether Cholesky decomposition has already been setup. */\r\n\r\n\t\treal_t x[NVMAX];\t\t\t/**< Primal solution vector. */\r\n\t\treal_t y[NVMAX+NCMAX];\t\t/**< Dual solution vector. */\r\n\r\n\t\treal_t tau;\t\t\t\t\t/**< Last homotopy step length. */\r\n\r\n\t\tQProblemStatus status;\t\t/**< Current status of the solution process. */\r\n\r\n\t\tBooleanType infeasible;\t\t/**< QP infeasible? */\r\n\t\tBooleanType unbounded;\t\t/**< QP unbounded? */\r\n\r\n\t\tHessianType hessianType;\t/**< Type of Hessian matrix. */\r\n\r\n\t\tPrintLevel printlevel;\t\t/**< Print level. */\r\n\r\n\t\tint count;\t\t\t\t\t/**< Counts the number of hotstart function calls (internal usage only!). */\r\n};\r\n\r\n\r\n#include \r\n\r\n#endif\t/* QPOASES_QPROBLEMB_HPP */\r\n\r\n\r\n/*\r\n *\tend of file\r\n */\r\n"
},
{
"path": "externals/qpoases/INCLUDE/SubjectTo.hpp",
"content": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.\r\n *\r\n *\tqpOASES is free software; you can redistribute it and/or\r\n *\tmodify it under the terms of the GNU Lesser General Public\r\n *\tLicense as published by the Free Software Foundation; either\r\n *\tversion 2.1 of the License, or (at your option) any later version.\r\n *\r\n *\tqpOASES is distributed in the hope that it will be useful,\r\n *\tbut WITHOUT ANY WARRANTY; without even the implied warranty of\r\n *\tMERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU\r\n *\tLesser General Public License for more details.\r\n *\r\n *\tYou should have received a copy of the GNU Lesser General Public\r\n *\tLicense along with qpOASES; if not, write to the Free Software\r\n *\tFoundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA\r\n *\r\n */\r\n\r\n\r\n/**\r\n *\t\\file INCLUDE/SubjectTo.hpp\r\n *\t\\author Hans Joachim Ferreau\r\n *\t\\version 1.3embedded\r\n *\t\\date 2007-2008\r\n *\r\n *\tDeclaration of the SubjectTo class designed to manage working sets of\r\n *\tconstraints and bounds within a QProblem.\r\n */\r\n\r\n\r\n#ifndef QPOASES_SUBJECTTO_HPP\r\n#define QPOASES_SUBJECTTO_HPP\r\n\r\n\r\n#include \r\n\r\n\r\n\r\n/** This class manages working sets of constraints and bounds by storing\r\n *\tindex sets and other status information.\r\n *\r\n *\t\\author Hans Joachim Ferreau\r\n *\t\\version 1.3embedded\r\n *\t\\date 2007-2008\r\n */\r\nclass SubjectTo\r\n{\r\n\t/*\r\n\t *\tPUBLIC MEMBER FUNCTIONS\r\n\t */\r\n\tpublic:\r\n\t\t/** Default constructor. */\r\n\t\tSubjectTo( );\r\n\r\n\t\t/** Copy constructor (deep copy). */\r\n\t\tSubjectTo(\tconst SubjectTo& rhs\t/**< Rhs object. */\r\n\t\t\t\t\t);\r\n\r\n\t\t/** Destructor. */\r\n\t\t~SubjectTo( );\r\n\r\n\t\t/** Assignment operator (deep copy). */\r\n\t\tSubjectTo& operator=(\tconst SubjectTo& rhs\t/**< Rhs object. */\r\n\t\t\t\t\t\t\t\t);\r\n\r\n\r\n\t\t/** Pseudo-constructor takes the number of constraints or bounds.\r\n\t\t *\t\\return SUCCESSFUL_RETURN */\r\n\t\treturnValue init(\tint n \t/**< Number of constraints or bounds. */\r\n\t\t\t\t\t\t\t);\r\n\r\n\r\n\t\t/** Returns type of (constraints') bound.\r\n\t\t *\t\\return Type of (constraints') bound \\n\r\n\t\t \t\t\tRET_INDEX_OUT_OF_BOUNDS */\r\n\t\tinline SubjectToType getType(\tint i\t\t/**< Number of (constraints') bound. */\r\n\t\t\t\t\t\t\t\t\t\t) const ;\r\n\r\n\t\t/** Returns status of (constraints') bound.\r\n\t\t *\t\\return Status of (constraints') bound \\n\r\n\t\t \t\t\tST_UNDEFINED */\r\n\t\tinline SubjectToStatus getStatus(\tint i\t\t/**< Number of (constraints') bound. */\r\n\t\t\t\t\t\t\t\t\t\t\t) const;\r\n\r\n\r\n\t\t/** Sets type of (constraints') bound.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t \t\t\tRET_INDEX_OUT_OF_BOUNDS */\r\n\t\tinline returnValue setType(\tint i,\t\t\t\t/**< Number of (constraints') bound. */\r\n\t\t\t\t\t\t\t\t\tSubjectToType value\t/**< Type of (constraints') bound. */\r\n\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Sets status of (constraints') bound.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t \t\t\tRET_INDEX_OUT_OF_BOUNDS */\r\n\t\tinline returnValue setStatus(\tint i,\t\t\t\t\t/**< Number of (constraints') bound. */\r\n\t\t\t\t\t\t\t\t\t\tSubjectToStatus value\t/**< Status of (constraints') bound. */\r\n\t\t\t\t\t\t\t\t\t\t);\r\n\r\n\r\n\t\t/** Sets status of lower (constraints') bounds. */\r\n\t\tinline void setNoLower(\tBooleanType _status\t\t/**< Status of lower (constraints') bounds. */\r\n\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Sets status of upper (constraints') bounds. */\r\n\t\tinline void setNoUpper(\tBooleanType _status\t\t/**< Status of upper (constraints') bounds. */\r\n\t\t\t\t\t\t\t\t);\r\n\r\n\r\n\t\t/** Returns status of lower (constraints') bounds.\r\n\t\t *\t\\return BT_TRUE if there is no lower (constraints') bound on any variable. */\r\n\t\tinline BooleanType isNoLower( ) const;\r\n\r\n\t\t/** Returns status of upper bounds.\r\n\t\t *\t\\return BT_TRUE if there is no upper (constraints') bound on any variable. */\r\n\t\tinline BooleanType isNoUpper( ) const;\r\n\r\n\r\n\t/*\r\n\t *\tPROTECTED MEMBER FUNCTIONS\r\n\t */\r\n\tprotected:\r\n\t\t/** Adds the index of a new constraint or bound to index set.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t \t\t\tRET_ADDINDEX_FAILED */\r\n\t\treturnValue addIndex(\tIndexlist* const indexlist,\t/**< Index list to which the new index shall be added. */\r\n\t\t\t\t\t\t\t\tint newnumber,\t\t\t\t/**< Number of new constraint or bound. */\r\n\t\t\t\t\t\t\t\tSubjectToStatus newstatus\t/**< Status of new constraint or bound. */\r\n\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Removes the index of a constraint or bound from index set.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t \t\t\tRET_UNKNOWN_BUG */\r\n\t\treturnValue removeIndex(\tIndexlist* const indexlist,\t/**< Index list from which the new index shall be removed. */\r\n\t\t\t\t\t\t\t\t\tint removenumber\t\t\t/**< Number of constraint or bound to be removed. */\r\n\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\t/** Swaps the indices of two constraints or bounds within the index set.\r\n\t\t *\t\\return SUCCESSFUL_RETURN \\n\r\n\t\t \t\t\tRET_SWAPINDEX_FAILED */\r\n\t\treturnValue swapIndex(\tIndexlist* const indexlist,\t/**< Index list in which the indices shold be swapped. */\r\n\t\t\t\t\t\t\t\tint number1,\t\t\t\t/**< Number of first constraint or bound. */\r\n\t\t\t\t\t\t\t\tint number2\t\t\t\t\t/**< Number of second constraint or bound. */\r\n\t\t\t\t\t\t\t\t);\r\n\r\n\r\n\t/*\r\n\t *\tPROTECTED MEMBER VARIABLES\r\n\t */\r\n\tprotected:\r\n\t\tSubjectToType type[NVMAX+NCMAX]; \t\t/**< Type of constraints/bounds. */\r\n\t\tSubjectToStatus status[NVMAX+NCMAX];\t/**< Status of constraints/bounds. */\r\n\r\n\t\tBooleanType noLower;\t\t\t\t \t/**< This flag indicates if there is no lower bound on any variable. */\r\n\t\tBooleanType noUpper;\t \t\t\t\t/**< This flag indicates if there is no upper bound on any variable. */\r\n\r\n\r\n\t/*\r\n\t *\tPRIVATE MEMBER VARIABLES\r\n\t */\r\n\tprivate:\r\n\t\tint size;\r\n};\r\n\r\n\r\n\r\n#include \r\n\r\n#endif\t/* QPOASES_SUBJECTTO_HPP */\r\n\r\n\r\n/*\r\n *\tend of file\r\n */\r\n"
},
{
"path": "externals/qpoases/INCLUDE/Types.hpp",
"content": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.\r\n *\r\n *\tqpOASES is free software; you can redistribute it and/or\r\n *\tmodify it under the terms of the GNU Lesser General Public\r\n *\tLicense as published by the Free Software Foundation; either\r\n *\tversion 2.1 of the License, or (at your option) any later version.\r\n *\r\n *\tqpOASES is distributed in the hope that it will be useful,\r\n *\tbut WITHOUT ANY WARRANTY; without even the implied warranty of\r\n *\tMERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU\r\n *\tLesser General Public License for more details.\r\n *\r\n *\tYou should have received a copy of the GNU Lesser General Public\r\n *\tLicense along with qpOASES; if not, write to the Free Software\r\n *\tFoundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA\r\n *\r\n */\r\n\r\n\r\n/**\r\n *\t\\file INCLUDE/Types.hpp\r\n *\t\\author Hans Joachim Ferreau\r\n *\t\\version 1.3embedded\r\n *\t\\date 2008\r\n *\r\n *\tDeclaration of all non-built-in types (except for classes).\r\n */\r\n\r\n\r\n#ifndef QPOASES_TYPES_HPP\r\n#define QPOASES_TYPES_HPP\r\n\r\n\r\n\r\n/** Define real_t for facilitating switching between double and float. */\r\n// typedef double real_t;\r\n\r\n\r\n/** Summarises all possible logical values. */\r\nenum BooleanType\r\n{\r\n\tBT_FALSE,\t\t\t\t\t/**< Logical value for \"false\". */\r\n\tBT_TRUE\t\t\t\t\t\t/**< Logical value for \"true\". */\r\n};\r\n\r\n\r\n/** Summarises all possible print levels. Print levels are used to describe\r\n *\tthe desired amount of output during runtime of qpOASES. */\r\nenum PrintLevel\r\n{\r\n\tPL_NONE,\t\t\t\t\t/**< No output. */\r\n\tPL_LOW,\t\t\t\t\t\t/**< Print error messages only. */\r\n\tPL_MEDIUM,\t\t\t\t\t/**< Print error and warning messages as well as concise info messages. */\r\n\tPL_HIGH\t\t\t\t\t\t/**< Print all messages with full details. */\r\n};\r\n\r\n\r\n/** Defines visibility status of a message. */\r\nenum VisibilityStatus\r\n{\r\n\tVS_VISIBLE,\t\t\t\t\t/**< Message visible. */\r\n\tVS_HIDDEN\t\t\t\t\t/**< Message not visible. */\r\n};\r\n\r\n\r\n/** Summarises all possible states of the (S)QProblem(B) object during the\r\nsolution process of a QP sequence. */\r\nenum QProblemStatus\r\n{\r\n\tQPS_NOTINITIALISED,\t\t\t/**< QProblem object is freshly instantiated or reset. */\r\n\tQPS_PREPARINGAUXILIARYQP,\t/**< An auxiliary problem is currently setup, either at the very beginning\r\n\t\t\t\t\t\t\t\t * via an initial homotopy or after changing the QP matrices. */\r\n\tQPS_AUXILIARYQPSOLVED,\t\t/**< An auxilary problem was solved, either at the very beginning\r\n\t\t\t\t\t\t\t\t * via an initial homotopy or after changing the QP matrices. */\r\n\tQPS_PERFORMINGHOMOTOPY,\t\t/**< A homotopy according to the main idea of the online active\r\n\t\t\t\t\t\t\t\t * set strategy is performed. */\r\n\tQPS_HOMOTOPYQPSOLVED,\t\t/**< An intermediate QP along the homotopy path was solved. */\r\n\tQPS_SOLVED\t\t\t\t\t/**< The solution of the actual QP was found. */\r\n};\r\n\r\n\r\n/** Summarises all possible types of bounds and constraints. */\r\nenum SubjectToType\r\n{\r\n\tST_UNBOUNDED,\t\t\t\t/**< Bound/constraint is unbounded. */\r\n\tST_BOUNDED,\t\t\t\t\t/**< Bound/constraint is bounded but not fixed. */\r\n\tST_EQUALITY,\t\t\t\t/**< Bound/constraint is fixed (implicit equality bound/constraint). */\r\n\tST_UNKNOWN\t\t\t\t\t/**< Type of bound/constraint unknown. */\r\n};\r\n\r\n\r\n/** Summarises all possible states of bounds and constraints. */\r\nenum SubjectToStatus\r\n{\r\n\tST_INACTIVE,\t\t\t\t/**< Bound/constraint is inactive. */\r\n\tST_LOWER,\t\t\t\t\t/**< Bound/constraint is at its lower bound. */\r\n\tST_UPPER,\t\t\t\t\t/**< Bound/constraint is at its upper bound. */\r\n\tST_UNDEFINED\t\t\t\t/**< Status of bound/constraint undefined. */\r\n};\r\n\r\n\r\n/** Summarises all possible cycling states of bounds and constraints. */\r\nenum CyclingStatus\r\n{\r\n\tCYC_NOT_INVOLVED,\t\t\t/**< Bound/constraint is not involved in current cycling. */\r\n\tCYC_PREV_ADDED,\t\t\t\t/**< Bound/constraint has previously been added during the current cycling. */\r\n\tCYC_PREV_REMOVED\t\t\t/**< Bound/constraint has previously been removed during the current cycling. */\r\n};\r\n\r\n\r\n/** Summarises all possible types of the QP's Hessian matrix. */\r\nenum HessianType\r\n{\r\n\tHST_SEMIDEF,\t\t\t\t/**< Hessian is positive semi-definite. */\r\n\tHST_POSDEF_NULLSPACE,\t\t/**< Hessian is positive definite on null space of active bounds/constraints. */\r\n\tHST_POSDEF,\t\t\t\t\t/**< Hessian is (strictly) positive definite. */\r\n\tHST_IDENTITY\t\t\t\t/**< Hessian is identity matrix. */\r\n};\r\n\r\n\r\n\r\n#endif\t/* QPOASES_TYPES_HPP */\r\n\r\n\r\n/*\r\n *\tend of file\r\n */\r\n"
},
{
"path": "externals/qpoases/INCLUDE/Utils.hpp",
"content": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.\r\n *\r\n *\tqpOASES is free software; you can redistribute it and/or\r\n *\tmodify it under the terms of the GNU Lesser General Public\r\n *\tLicense as published by the Free Software Foundation; either\r\n *\tversion 2.1 of the License, or (at your option) any later version.\r\n *\r\n *\tqpOASES is distributed in the hope that it will be useful,\r\n *\tbut WITHOUT ANY WARRANTY; without even the implied warranty of\r\n *\tMERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU\r\n *\tLesser General Public License for more details.\r\n *\r\n *\tYou should have received a copy of the GNU Lesser General Public\r\n *\tLicense along with qpOASES; if not, write to the Free Software\r\n *\tFoundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA\r\n *\r\n */\r\n\r\n\r\n/**\r\n *\t\\file INCLUDE/Utils.hpp\r\n *\t\\author Hans Joachim Ferreau\r\n *\t\\version 1.3embedded\r\n *\t\\date 2007-2008\r\n *\r\n *\tDeclaration of global utility functions for working with qpOASES.\r\n */\r\n\r\n\r\n#ifndef QPOASES_UTILS_HPP\r\n#define QPOASES_UTILS_HPP\r\n\r\n\r\n#include \r\n\r\n\r\n#ifdef PC_DEBUG /* Define print functions only for debugging! */\r\n\r\n/** Prints a vector.\r\n * \\return SUCCESSFUL_RETURN */\r\nreturnValue print(\tconst real_t* const v,\t/**< Vector to be printed. */\r\n\t\t\t\t\tint n\t\t\t\t\t/**< Length of vector. */\r\n\t\t\t\t\t);\r\n\r\n/** Prints a permuted vector.\r\n * \\return SUCCESSFUL_RETURN */\r\nreturnValue print(\tconst real_t* const v,\t\t/**< Vector to be printed. */\r\n\t\t\t\t\tint n,\t\t\t\t\t\t/**< Length of vector. */\r\n\t\t\t\t\tconst int* const V_idx\t\t/**< Pemutation vector. */\r\n\t\t\t\t\t);\r\n\r\n/** Prints a named vector.\r\n * \\return SUCCESSFUL_RETURN */\r\nreturnValue print(\tconst real_t* const v,\t/**< Vector to be printed. */\r\n\t\t\t\t\tint n,\t\t\t\t\t/**< Length of vector. */\r\n\t\t\t\t\tconst char* name\t\t/** Name of vector. */\r\n\t\t\t\t\t);\r\n\r\n/** Prints a matrix.\r\n * \\return SUCCESSFUL_RETURN */\r\nreturnValue print(\tconst real_t* const M,\t/**< Matrix to be printed. */\r\n\t\t\t\t\tint nrow,\t\t\t\t/**< Row number of matrix. */\r\n\t\t\t\t\tint ncol\t\t\t\t/**< Column number of matrix. */\r\n\t\t\t\t\t);\r\n\r\n/** Prints a permuted matrix.\r\n * \\return SUCCESSFUL_RETURN */\r\nreturnValue print(\tconst real_t* const M,\t\t/**< Matrix to be printed. */\r\n\t\t\t\t\tint nrow,\t\t\t\t\t/**< Row number of matrix. */\r\n\t\t\t\t\tint ncol\t,\t\t\t\t/**< Column number of matrix. */\r\n\t\t\t\t\tconst int* const ROW_idx,\t/**< Row pemutation vector. */\r\n\t\t\t\t\tconst int* const COL_idx\t/**< Column pemutation vector. */\r\n\t\t\t\t\t);\r\n\r\n/** Prints a named matrix.\r\n * \\return SUCCESSFUL_RETURN */\r\nreturnValue print(\tconst real_t* const M,\t/**< Matrix to be printed. */\r\n\t\t\t\t\tint nrow,\t\t\t\t/**< Row number of matrix. */\r\n\t\t\t\t\tint ncol,\t\t\t\t/**< Column number of matrix. */\r\n\t\t\t\t\tconst char* name\t\t/** Name of matrix. */\r\n\t\t\t\t\t);\r\n\r\n/** Prints an index array.\r\n * \\return SUCCESSFUL_RETURN */\r\nreturnValue print(\tconst int* const index,\t/**< Index array to be printed. */\r\n\t\t\t\t\tint n\t\t\t\t\t/**< Length of index array. */\r\n\t\t\t\t\t);\r\n\r\n/** Prints a named index array.\r\n * \\return SUCCESSFUL_RETURN */\r\nreturnValue print(\tconst int* const index,\t/**< Index array to be printed. */\r\n\t\t\t\t\tint n,\t\t\t\t\t/**< Length of index array. */\r\n\t\t\t\t\tconst char* name\t\t/**< Name of index array. */\r\n\t\t\t\t\t);\r\n\r\n\r\n/** Prints a string to desired output target (useful also for MATLAB output!).\r\n * \\return SUCCESSFUL_RETURN */\r\nreturnValue myPrintf(\tconst char* s\t/**< String to be written. */\r\n\t\t\t\t\t\t);\r\n\r\n\r\n/** Prints qpOASES copyright notice.\r\n * \\return SUCCESSFUL_RETURN */\r\nreturnValue printCopyrightNotice( );\r\n\r\n\r\n/** Reads a real_t matrix from file.\r\n * \\return SUCCESSFUL_RETURN \\n\r\n \t\t RET_UNABLE_TO_OPEN_FILE \\n\r\n\t\t RET_UNABLE_TO_READ_FILE */\r\nreturnValue readFromFile(\treal_t* data,\t\t\t\t/**< Matrix to be read from file. */\r\n\t\t\t\t\t\t\tint nrow,\t\t\t\t\t/**< Row number of matrix. */\r\n\t\t\t\t\t\t\tint ncol,\t\t\t\t\t/**< Column number of matrix. */\r\n\t\t\t\t\t\t\tconst char* datafilename\t/**< Data file name. */\r\n\t\t\t\t\t\t\t);\r\n\r\n/** Reads a real_t vector from file.\r\n * \\return SUCCESSFUL_RETURN \\n\r\n \t\t RET_UNABLE_TO_OPEN_FILE \\n\r\n\t\t RET_UNABLE_TO_READ_FILE */\r\nreturnValue readFromFile(\treal_t* data,\t\t\t\t/**< Vector to be read from file. */\r\n\t\t\t\t\t\t\tint n,\t\t\t\t\t\t/**< Length of vector. */\r\n\t\t\t\t\t\t\tconst char* datafilename\t/**< Data file name. */\r\n\t\t\t\t\t\t\t);\r\n\r\n/** Reads an integer (column) vector from file.\r\n * \\return SUCCESSFUL_RETURN \\n\r\n \t\t RET_UNABLE_TO_OPEN_FILE \\n\r\n\t\t RET_UNABLE_TO_READ_FILE */\r\nreturnValue readFromFile(\tint* data,\t\t\t\t\t/**< Vector to be read from file. */\r\n\t\t\t\t\t\t\tint n,\t\t\t\t\t\t/**< Length of vector. */\r\n\t\t\t\t\t\t\tconst char* datafilename\t/**< Data file name. */\r\n\t\t\t\t\t\t\t);\r\n\r\n\r\n/** Writes a real_t matrix into a file.\r\n * \\return SUCCESSFUL_RETURN \\n\r\n \t\t RET_UNABLE_TO_OPEN_FILE */\r\nreturnValue writeIntoFile(\tconst real_t* const data,\t/**< Matrix to be written into file. */\r\n\t\t\t\t\t\t\tint nrow,\t\t\t\t\t/**< Row number of matrix. */\r\n\t\t\t\t\t\t\tint ncol,\t\t\t\t\t/**< Column number of matrix. */\r\n\t\t\t\t\t\t\tconst char* datafilename,\t/**< Data file name. */\r\n\t\t\t\t\t\t\tBooleanType append\t\t\t/**< Indicates if data shall be appended if the file already exists (otherwise it is overwritten). */\r\n\t\t\t\t\t\t\t);\r\n\r\n/** Writes a real_t vector into a file.\r\n * \\return SUCCESSFUL_RETURN \\n\r\n \t\t RET_UNABLE_TO_OPEN_FILE */\r\nreturnValue writeIntoFile(\tconst real_t* const data,\t/**< Vector to be written into file. */\r\n\t\t\t\t\t\t\tint n,\t\t\t\t\t\t/**< Length of vector. */\r\n\t\t\t\t\t\t\tconst char* datafilename,\t/**< Data file name. */\r\n\t\t\t\t\t\t\tBooleanType append\t\t\t/**< Indicates if data shall be appended if the file already exists (otherwise it is overwritten). */\r\n\t\t\t\t\t\t\t);\r\n\r\n/** Writes an integer (column) vector into a file.\r\n * \\return SUCCESSFUL_RETURN \\n\r\n \t\t RET_UNABLE_TO_OPEN_FILE */\r\nreturnValue writeIntoFile(\tconst int* const integer,\t/**< Integer vector to be written into file. */\r\n\t\t\t\t\t\t\tint n,\t\t\t\t\t\t/**< Length of vector. */\r\n\t\t\t\t\t\t\tconst char* datafilename,\t/**< Data file name. */\r\n\t\t\t\t\t\t\tBooleanType append\t\t\t/**< Indicates if integer shall be appended if the file already exists (otherwise it is overwritten). */\r\n\t\t\t\t\t\t\t);\r\n\r\n#endif /* PC_DEBUG */\r\n\r\n\r\n/** Returns the current system time.\r\n * \\return current system time */\r\nreal_t getCPUtime( );\r\n\r\n\r\n/** Returns the Euclidean norm of a vector.\r\n * \\return 0: successful */\r\nreal_t getNorm(\tconst real_t* const v,\t/**< Vector. */\r\n\t\t\t\tint n\t\t\t\t\t/**< Vector's dimension. */\r\n\t\t\t\t);\r\n\r\n/** Returns the absolute value of a real_t.\r\n * \\return Absolute value of a real_t */\r\ninline real_t getAbs(\treal_t x\t\t/**< Input argument. */\r\n\t\t\t\t\t\t);\r\n\r\n\r\n\r\n#include \r\n\r\n#endif\t/* QPOASES_UTILS_HPP */\r\n\r\n\r\n/*\r\n *\tend of file\r\n */\r\n"
},
{
"path": "externals/qpoases/LICENSE.txt",
"content": "\t\t GNU LESSER GENERAL PUBLIC LICENSE\r\n\t\t Version 2.1, February 1999\r\n\r\n Copyright (C) 1991, 1999 Free Software Foundation, Inc.\r\n 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA\r\n Everyone is permitted to copy and distribute verbatim copies\r\n of this license document, but changing it is not allowed.\r\n\r\n[This is the first released version of the Lesser GPL. It also counts\r\n as the successor of the GNU Library Public License, version 2, hence\r\n the version number 2.1.]\r\n\r\n\t\t\t Preamble\r\n\r\n The licenses for most software are designed to take away your\r\nfreedom to share and change it. By contrast, the GNU General Public\r\nLicenses are intended to guarantee your freedom to share and change\r\nfree software--to make sure the software is free for all its users.\r\n\r\n This license, the Lesser General Public License, applies to some\r\nspecially designated software packages--typically libraries--of the\r\nFree Software Foundation and other authors who decide to use it. You\r\ncan use it too, but we suggest you first think carefully about whether\r\nthis license or the ordinary General Public License is the better\r\nstrategy to use in any particular case, based on the explanations below.\r\n\r\n When we speak of free software, we are referring to freedom of use,\r\nnot price. Our General Public Licenses are designed to make sure that\r\nyou have the freedom to distribute copies of free software (and charge\r\nfor this service if you wish); that you receive source code or can get\r\nit if you want it; that you can change the software and use pieces of\r\nit in new free programs; and that you are informed that you can do\r\nthese things.\r\n\r\n To protect your rights, we need to make restrictions that forbid\r\ndistributors to deny you these rights or to ask you to surrender these\r\nrights. These restrictions translate to certain responsibilities for\r\nyou if you distribute copies of the library or if you modify it.\r\n\r\n For example, if you distribute copies of the library, whether gratis\r\nor for a fee, you must give the recipients all the rights that we gave\r\nyou. You must make sure that they, too, receive or can get the source\r\ncode. If you link other code with the library, you must provide\r\ncomplete object files to the recipients, so that they can relink them\r\nwith the library after making changes to the library and recompiling\r\nit. And you must show them these terms so they know their rights.\r\n\r\n We protect your rights with a two-step method: (1) we copyright the\r\nlibrary, and (2) we offer you this license, which gives you legal\r\npermission to copy, distribute and/or modify the library.\r\n\r\n To protect each distributor, we want to make it very clear that\r\nthere is no warranty for the free library. Also, if the library is\r\nmodified by someone else and passed on, the recipients should know\r\nthat what they have is not the original version, so that the original\r\nauthor's reputation will not be affected by problems that might be\r\nintroduced by others.\r\n\f\r\n Finally, software patents pose a constant threat to the existence of\r\nany free program. We wish to make sure that a company cannot\r\neffectively restrict the users of a free program by obtaining a\r\nrestrictive license from a patent holder. Therefore, we insist that\r\nany patent license obtained for a version of the library must be\r\nconsistent with the full freedom of use specified in this license.\r\n\r\n Most GNU software, including some libraries, is covered by the\r\nordinary GNU General Public License. This license, the GNU Lesser\r\nGeneral Public License, applies to certain designated libraries, and\r\nis quite different from the ordinary General Public License. We use\r\nthis license for certain libraries in order to permit linking those\r\nlibraries into non-free programs.\r\n\r\n When a program is linked with a library, whether statically or using\r\na shared library, the combination of the two is legally speaking a\r\ncombined work, a derivative of the original library. The ordinary\r\nGeneral Public License therefore permits such linking only if the\r\nentire combination fits its criteria of freedom. The Lesser General\r\nPublic License permits more lax criteria for linking other code with\r\nthe library.\r\n\r\n We call this license the \"Lesser\" General Public License because it\r\ndoes Less to protect the user's freedom than the ordinary General\r\nPublic License. It also provides other free software developers Less\r\nof an advantage over competing non-free programs. These disadvantages\r\nare the reason we use the ordinary General Public License for many\r\nlibraries. However, the Lesser license provides advantages in certain\r\nspecial circumstances.\r\n\r\n For example, on rare occasions, there may be a special need to\r\nencourage the widest possible use of a certain library, so that it becomes\r\na de-facto standard. To achieve this, non-free programs must be\r\nallowed to use the library. A more frequent case is that a free\r\nlibrary does the same job as widely used non-free libraries. In this\r\ncase, there is little to gain by limiting the free library to free\r\nsoftware only, so we use the Lesser General Public License.\r\n\r\n In other cases, permission to use a particular library in non-free\r\nprograms enables a greater number of people to use a large body of\r\nfree software. For example, permission to use the GNU C Library in\r\nnon-free programs enables many more people to use the whole GNU\r\noperating system, as well as its variant, the GNU/Linux operating\r\nsystem.\r\n\r\n Although the Lesser General Public License is Less protective of the\r\nusers' freedom, it does ensure that the user of a program that is\r\nlinked with the Library has the freedom and the wherewithal to run\r\nthat program using a modified version of the Library.\r\n\r\n The precise terms and conditions for copying, distribution and\r\nmodification follow. Pay close attention to the difference between a\r\n\"work based on the library\" and a \"work that uses the library\". The\r\nformer contains code derived from the library, whereas the latter must\r\nbe combined with the library in order to run.\r\n\f\r\n\t\t GNU LESSER GENERAL PUBLIC LICENSE\r\n TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION\r\n\r\n 0. This License Agreement applies to any software library or other\r\nprogram which contains a notice placed by the copyright holder or\r\nother authorized party saying it may be distributed under the terms of\r\nthis Lesser General Public License (also called \"this License\").\r\nEach licensee is addressed as \"you\".\r\n\r\n A \"library\" means a collection of software functions and/or data\r\nprepared so as to be conveniently linked with application programs\r\n(which use some of those functions and data) to form executables.\r\n\r\n The \"Library\", below, refers to any such software library or work\r\nwhich has been distributed under these terms. A \"work based on the\r\nLibrary\" means either the Library or any derivative work under\r\ncopyright law: that is to say, a work containing the Library or a\r\nportion of it, either verbatim or with modifications and/or translated\r\nstraightforwardly into another language. (Hereinafter, translation is\r\nincluded without limitation in the term \"modification\".)\r\n\r\n \"Source code\" for a work means the preferred form of the work for\r\nmaking modifications to it. For a library, complete source code means\r\nall the source code for all modules it contains, plus any associated\r\ninterface definition files, plus the scripts used to control compilation\r\nand installation of the library.\r\n\r\n Activities other than copying, distribution and modification are not\r\ncovered by this License; they are outside its scope. The act of\r\nrunning a program using the Library is not restricted, and output from\r\nsuch a program is covered only if its contents constitute a work based\r\non the Library (independent of the use of the Library in a tool for\r\nwriting it). Whether that is true depends on what the Library does\r\nand what the program that uses the Library does.\r\n \r\n 1. You may copy and distribute verbatim copies of the Library's\r\ncomplete source code as you receive it, in any medium, provided that\r\nyou conspicuously and appropriately publish on each copy an\r\nappropriate copyright notice and disclaimer of warranty; keep intact\r\nall the notices that refer to this License and to the absence of any\r\nwarranty; and distribute a copy of this License along with the\r\nLibrary.\r\n\r\n You may charge a fee for the physical act of transferring a copy,\r\nand you may at your option offer warranty protection in exchange for a\r\nfee.\r\n\f\r\n 2. You may modify your copy or copies of the Library or any portion\r\nof it, thus forming a work based on the Library, and copy and\r\ndistribute such modifications or work under the terms of Section 1\r\nabove, provided that you also meet all of these conditions:\r\n\r\n a) The modified work must itself be a software library.\r\n\r\n b) You must cause the files modified to carry prominent notices\r\n stating that you changed the files and the date of any change.\r\n\r\n c) You must cause the whole of the work to be licensed at no\r\n charge to all third parties under the terms of this License.\r\n\r\n d) If a facility in the modified Library refers to a function or a\r\n table of data to be supplied by an application program that uses\r\n the facility, other than as an argument passed when the facility\r\n is invoked, then you must make a good faith effort to ensure that,\r\n in the event an application does not supply such function or\r\n table, the facility still operates, and performs whatever part of\r\n its purpose remains meaningful.\r\n\r\n (For example, a function in a library to compute square roots has\r\n a purpose that is entirely well-defined independent of the\r\n application. Therefore, Subsection 2d requires that any\r\n application-supplied function or table used by this function must\r\n be optional: if the application does not supply it, the square\r\n root function must still compute square roots.)\r\n\r\nThese requirements apply to the modified work as a whole. If\r\nidentifiable sections of that work are not derived from the Library,\r\nand can be reasonably considered independent and separate works in\r\nthemselves, then this License, and its terms, do not apply to those\r\nsections when you distribute them as separate works. But when you\r\ndistribute the same sections as part of a whole which is a work based\r\non the Library, the distribution of the whole must be on the terms of\r\nthis License, whose permissions for other licensees extend to the\r\nentire whole, and thus to each and every part regardless of who wrote\r\nit.\r\n\r\nThus, it is not the intent of this section to claim rights or contest\r\nyour rights to work written entirely by you; rather, the intent is to\r\nexercise the right to control the distribution of derivative or\r\ncollective works based on the Library.\r\n\r\nIn addition, mere aggregation of another work not based on the Library\r\nwith the Library (or with a work based on the Library) on a volume of\r\na storage or distribution medium does not bring the other work under\r\nthe scope of this License.\r\n\r\n 3. You may opt to apply the terms of the ordinary GNU General Public\r\nLicense instead of this License to a given copy of the Library. To do\r\nthis, you must alter all the notices that refer to this License, so\r\nthat they refer to the ordinary GNU General Public License, version 2,\r\ninstead of to this License. (If a newer version than version 2 of the\r\nordinary GNU General Public License has appeared, then you can specify\r\nthat version instead if you wish.) Do not make any other change in\r\nthese notices.\r\n\f\r\n Once this change is made in a given copy, it is irreversible for\r\nthat copy, so the ordinary GNU General Public License applies to all\r\nsubsequent copies and derivative works made from that copy.\r\n\r\n This option is useful when you wish to copy part of the code of\r\nthe Library into a program that is not a library.\r\n\r\n 4. You may copy and distribute the Library (or a portion or\r\nderivative of it, under Section 2) in object code or executable form\r\nunder the terms of Sections 1 and 2 above provided that you accompany\r\nit with the complete corresponding machine-readable source code, which\r\nmust be distributed under the terms of Sections 1 and 2 above on a\r\nmedium customarily used for software interchange.\r\n\r\n If distribution of object code is made by offering access to copy\r\nfrom a designated place, then offering equivalent access to copy the\r\nsource code from the same place satisfies the requirement to\r\ndistribute the source code, even though third parties are not\r\ncompelled to copy the source along with the object code.\r\n\r\n 5. A program that contains no derivative of any portion of the\r\nLibrary, but is designed to work with the Library by being compiled or\r\nlinked with it, is called a \"work that uses the Library\". Such a\r\nwork, in isolation, is not a derivative work of the Library, and\r\ntherefore falls outside the scope of this License.\r\n\r\n However, linking a \"work that uses the Library\" with the Library\r\ncreates an executable that is a derivative of the Library (because it\r\ncontains portions of the Library), rather than a \"work that uses the\r\nlibrary\". The executable is therefore covered by this License.\r\nSection 6 states terms for distribution of such executables.\r\n\r\n When a \"work that uses the Library\" uses material from a header file\r\nthat is part of the Library, the object code for the work may be a\r\nderivative work of the Library even though the source code is not.\r\nWhether this is true is especially significant if the work can be\r\nlinked without the Library, or if the work is itself a library. The\r\nthreshold for this to be true is not precisely defined by law.\r\n\r\n If such an object file uses only numerical parameters, data\r\nstructure layouts and accessors, and small macros and small inline\r\nfunctions (ten lines or less in length), then the use of the object\r\nfile is unrestricted, regardless of whether it is legally a derivative\r\nwork. (Executables containing this object code plus portions of the\r\nLibrary will still fall under Section 6.)\r\n\r\n Otherwise, if the work is a derivative of the Library, you may\r\ndistribute the object code for the work under the terms of Section 6.\r\nAny executables containing that work also fall under Section 6,\r\nwhether or not they are linked directly with the Library itself.\r\n\f\r\n 6. As an exception to the Sections above, you may also combine or\r\nlink a \"work that uses the Library\" with the Library to produce a\r\nwork containing portions of the Library, and distribute that work\r\nunder terms of your choice, provided that the terms permit\r\nmodification of the work for the customer's own use and reverse\r\nengineering for debugging such modifications.\r\n\r\n You must give prominent notice with each copy of the work that the\r\nLibrary is used in it and that the Library and its use are covered by\r\nthis License. You must supply a copy of this License. If the work\r\nduring execution displays copyright notices, you must include the\r\ncopyright notice for the Library among them, as well as a reference\r\ndirecting the user to the copy of this License. Also, you must do one\r\nof these things:\r\n\r\n a) Accompany the work with the complete corresponding\r\n machine-readable source code for the Library including whatever\r\n changes were used in the work (which must be distributed under\r\n Sections 1 and 2 above); and, if the work is an executable linked\r\n with the Library, with the complete machine-readable \"work that\r\n uses the Library\", as object code and/or source code, so that the\r\n user can modify the Library and then relink to produce a modified\r\n executable containing the modified Library. (It is understood\r\n that the user who changes the contents of definitions files in the\r\n Library will not necessarily be able to recompile the application\r\n to use the modified definitions.)\r\n\r\n b) Use a suitable shared library mechanism for linking with the\r\n Library. A suitable mechanism is one that (1) uses at run time a\r\n copy of the library already present on the user's computer system,\r\n rather than copying library functions into the executable, and (2)\r\n will operate properly with a modified version of the library, if\r\n the user installs one, as long as the modified version is\r\n interface-compatible with the version that the work was made with.\r\n\r\n c) Accompany the work with a written offer, valid for at\r\n least three years, to give the same user the materials\r\n specified in Subsection 6a, above, for a charge no more\r\n than the cost of performing this distribution.\r\n\r\n d) If distribution of the work is made by offering access to copy\r\n from a designated place, offer equivalent access to copy the above\r\n specified materials from the same place.\r\n\r\n e) Verify that the user has already received a copy of these\r\n materials or that you have already sent this user a copy.\r\n\r\n For an executable, the required form of the \"work that uses the\r\nLibrary\" must include any data and utility programs needed for\r\nreproducing the executable from it. However, as a special exception,\r\nthe materials to be distributed need not include anything that is\r\nnormally distributed (in either source or binary form) with the major\r\ncomponents (compiler, kernel, and so on) of the operating system on\r\nwhich the executable runs, unless that component itself accompanies\r\nthe executable.\r\n\r\n It may happen that this requirement contradicts the license\r\nrestrictions of other proprietary libraries that do not normally\r\naccompany the operating system. Such a contradiction means you cannot\r\nuse both them and the Library together in an executable that you\r\ndistribute.\r\n\f\r\n 7. You may place library facilities that are a work based on the\r\nLibrary side-by-side in a single library together with other library\r\nfacilities not covered by this License, and distribute such a combined\r\nlibrary, provided that the separate distribution of the work based on\r\nthe Library and of the other library facilities is otherwise\r\npermitted, and provided that you do these two things:\r\n\r\n a) Accompany the combined library with a copy of the same work\r\n based on the Library, uncombined with any other library\r\n facilities. This must be distributed under the terms of the\r\n Sections above.\r\n\r\n b) Give prominent notice with the combined library of the fact\r\n that part of it is a work based on the Library, and explaining\r\n where to find the accompanying uncombined form of the same work.\r\n\r\n 8. You may not copy, modify, sublicense, link with, or distribute\r\nthe Library except as expressly provided under this License. Any\r\nattempt otherwise to copy, modify, sublicense, link with, or\r\ndistribute the Library is void, and will automatically terminate your\r\nrights under this License. However, parties who have received copies,\r\nor rights, from you under this License will not have their licenses\r\nterminated so long as such parties remain in full compliance.\r\n\r\n 9. You are not required to accept this License, since you have not\r\nsigned it. However, nothing else grants you permission to modify or\r\ndistribute the Library or its derivative works. These actions are\r\nprohibited by law if you do not accept this License. Therefore, by\r\nmodifying or distributing the Library (or any work based on the\r\nLibrary), you indicate your acceptance of this License to do so, and\r\nall its terms and conditions for copying, distributing or modifying\r\nthe Library or works based on it.\r\n\r\n 10. Each time you redistribute the Library (or any work based on the\r\nLibrary), the recipient automatically receives a license from the\r\noriginal licensor to copy, distribute, link with or modify the Library\r\nsubject to these terms and conditions. You may not impose any further\r\nrestrictions on the recipients' exercise of the rights granted herein.\r\nYou are not responsible for enforcing compliance by third parties with\r\nthis License.\r\n\f\r\n 11. If, as a consequence of a court judgment or allegation of patent\r\ninfringement or for any other reason (not limited to patent issues),\r\nconditions are imposed on you (whether by court order, agreement or\r\notherwise) that contradict the conditions of this License, they do not\r\nexcuse you from the conditions of this License. If you cannot\r\ndistribute so as to satisfy simultaneously your obligations under this\r\nLicense and any other pertinent obligations, then as a consequence you\r\nmay not distribute the Library at all. For example, if a patent\r\nlicense would not permit royalty-free redistribution of the Library by\r\nall those who receive copies directly or indirectly through you, then\r\nthe only way you could satisfy both it and this License would be to\r\nrefrain entirely from distribution of the Library.\r\n\r\nIf any portion of this section is held invalid or unenforceable under any\r\nparticular circumstance, the balance of the section is intended to apply,\r\nand the section as a whole is intended to apply in other circumstances.\r\n\r\nIt is not the purpose of this section to induce you to infringe any\r\npatents or other property right claims or to contest validity of any\r\nsuch claims; this section has the sole purpose of protecting the\r\nintegrity of the free software distribution system which is\r\nimplemented by public license practices. Many people have made\r\ngenerous contributions to the wide range of software distributed\r\nthrough that system in reliance on consistent application of that\r\nsystem; it is up to the author/donor to decide if he or she is willing\r\nto distribute software through any other system and a licensee cannot\r\nimpose that choice.\r\n\r\nThis section is intended to make thoroughly clear what is believed to\r\nbe a consequence of the rest of this License.\r\n\r\n 12. If the distribution and/or use of the Library is restricted in\r\ncertain countries either by patents or by copyrighted interfaces, the\r\noriginal copyright holder who places the Library under this License may add\r\nan explicit geographical distribution limitation excluding those countries,\r\nso that distribution is permitted only in or among countries not thus\r\nexcluded. In such case, this License incorporates the limitation as if\r\nwritten in the body of this License.\r\n\r\n 13. The Free Software Foundation may publish revised and/or new\r\nversions of the Lesser General Public License from time to time.\r\nSuch new versions will be similar in spirit to the present version,\r\nbut may differ in detail to address new problems or concerns.\r\n\r\nEach version is given a distinguishing version number. If the Library\r\nspecifies a version number of this License which applies to it and\r\n\"any later version\", you have the option of following the terms and\r\nconditions either of that version or of any later version published by\r\nthe Free Software Foundation. If the Library does not specify a\r\nlicense version number, you may choose any version ever published by\r\nthe Free Software Foundation.\r\n\f\r\n 14. If you wish to incorporate parts of the Library into other free\r\nprograms whose distribution conditions are incompatible with these,\r\nwrite to the author to ask for permission. For software which is\r\ncopyrighted by the Free Software Foundation, write to the Free\r\nSoftware Foundation; we sometimes make exceptions for this. Our\r\ndecision will be guided by the two goals of preserving the free status\r\nof all derivatives of our free software and of promoting the sharing\r\nand reuse of software generally.\r\n\r\n\t\t\t NO WARRANTY\r\n\r\n 15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO\r\nWARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.\r\nEXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR\r\nOTHER PARTIES PROVIDE THE LIBRARY \"AS IS\" WITHOUT WARRANTY OF ANY\r\nKIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE\r\nIMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR\r\nPURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE\r\nLIBRARY IS WITH YOU. SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME\r\nTHE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.\r\n\r\n 16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN\r\nWRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY\r\nAND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU\r\nFOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR\r\nCONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE\r\nLIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING\r\nRENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A\r\nFAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF\r\nSUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH\r\nDAMAGES.\r\n\r\n\t\t END OF TERMS AND CONDITIONS\r\n\f\r\n How to Apply These Terms to Your New Libraries\r\n\r\n If you develop a new library, and you want it to be of the greatest\r\npossible use to the public, we recommend making it free software that\r\neveryone can redistribute and change. You can do so by permitting\r\nredistribution under these terms (or, alternatively, under the terms of the\r\nordinary General Public License).\r\n\r\n To apply these terms, attach the following notices to the library. It is\r\nsafest to attach them to the start of each source file to most effectively\r\nconvey the exclusion of warranty; and each file should have at least the\r\n\"copyright\" line and a pointer to where the full notice is found.\r\n\r\n \r\n Copyright (C) \r\n\r\n This library is free software; you can redistribute it and/or\r\n modify it under the terms of the GNU Lesser General Public\r\n License as published by the Free Software Foundation; either\r\n version 2.1 of the License, or (at your option) any later version.\r\n\r\n This library is distributed in the hope that it will be useful,\r\n but WITHOUT ANY WARRANTY; without even the implied warranty of\r\n MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU\r\n Lesser General Public License for more details.\r\n\r\n You should have received a copy of the GNU Lesser General Public\r\n License along with this library; if not, write to the Free Software\r\n Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA\r\n\r\nAlso add information on how to contact you by electronic and paper mail.\r\n\r\nYou should also get your employer (if you work as a programmer) or your\r\nschool, if any, to sign a \"copyright disclaimer\" for the library, if\r\nnecessary. Here is a sample; alter the names:\r\n\r\n Yoyodyne, Inc., hereby disclaims all copyright interest in the\r\n library `Frob' (a library for tweaking knobs) written by James Random Hacker.\r\n\r\n , 1 April 1990\r\n Ty Coon, President of Vice\r\n\r\nThat's all there is to it!\r\n\r\n\r\n"
},
{
"path": "externals/qpoases/README.txt",
"content": "##\r\n##\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n##\tCopyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.\r\n##\r\n##\tqpOASES is free software; you can redistribute it and/or\r\n##\tmodify it under the terms of the GNU Lesser General Public\r\n##\tLicense as published by the Free Software Foundation; either\r\n##\tversion 2.1 of the License, or (at your option) any later version.\r\n##\r\n##\tqpOASES is distributed in the hope that it will be useful,\r\n##\tbut WITHOUT ANY WARRANTY; without even the implied warranty of\r\n##\tMERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU\r\n##\tLesser General Public License for more details.\r\n##\r\n##\tYou should have received a copy of the GNU Lesser General Public\r\n##\tLicense along with qpOASES; if not, write to the Free Software\r\n##\tFoundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA\r\n##\r\n\r\n\r\n\r\nINTRODUCTION\r\n=============\r\n\r\nqpOASES is an open-source C++ implementation of the recently proposed \r\nonline active set strategy (see [1], [2]), which was inspired by important \r\nobservations from the field of parametric quadratic programming. It has \r\nseveral theoretical features that make it particularly suited for model \r\npredictive control (MPC) applications.\r\n\r\nThe software package qpOASES implements these ideas and has already been \r\nsuccessfully used for closed-loop control of a real-world Diesel engine [3].\r\n\r\n\r\nReferences:\r\n\r\n[1] H.J. Ferreau. An Online Active Set Strategy for Fast Solution of \r\nParametric Quadratic Programs with Applications to Predictive Engine Control. \r\nDiplom thesis, University of Heidelberg, 2006.\r\n\r\n[2] H.J. Ferreau, H.G. Bock, M. Diehl. An online active set strategy to \r\novercome the limitations of explicit MPC. International Journal of Robust \r\nand Nonlinear Control, 18 (8), pp. 816-830, 2008.\r\n\r\n[3] H.J. Ferreau, P. Ortner, P. Langthaler, L. del Re, M. Diehl. Predictive \r\nControl of a Real-World Diesel Engine using an Extended Online Active Set \r\nStrategy. Annual Reviews in Control, 31 (2), pp. 293-301, 2007.\r\n\r\n\r\n\r\nGETTING STARTED\r\n================\r\n\r\n1. For installation, usage and additional information on this software package \r\n see the qpOASES User's Manual located at ./DOC/manual.pdf!\r\n\r\n\r\n2. The file ./LICENSE.txt contains a copy of the GNU Lesser General Public \r\n License. Please read it carefully before using qpOASES!\r\n\r\n\r\n3. The whole software package can be downloaded from \r\n\r\n http://homes.esat.kuleuven.be/~optec/software/qpOASES/ \r\n\r\n On this webpage you will also find a list of frequently asked questions.\r\n\r\n\r\n\r\nCONTACT THE AUTHORS\r\n====================\r\n\r\nIf you have got questions, remarks or comments on qpOASES \r\nplease contact the main author:\r\n\r\n Hans Joachim Ferreau\r\n Katholieke Universiteit Leuven\r\n Department of Electrical Engineering (ESAT)\r\n Kasteelpark Arenberg 10, bus 2446\r\n B-3001 Leuven-Heverlee, Belgium\r\n\r\n Phone: +32 16 32 03 63\r\n E-mail: joachim.ferreau@esat.kuleuven.be\r\n qpOASES@esat.kuleuven.be\r\n\r\nAlso bug reports and source code extensions are most welcome!\r\n\r\n\r\n\r\n##\r\n##\tend of file\r\n##\r\n"
},
{
"path": "externals/qpoases/SRC/Bounds.cpp",
"content": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.\r\n *\r\n *\tqpOASES is free software; you can redistribute it and/or\r\n *\tmodify it under the terms of the GNU Lesser General Public\r\n *\tLicense as published by the Free Software Foundation; either\r\n *\tversion 2.1 of the License, or (at your option) any later version.\r\n *\r\n *\tqpOASES is distributed in the hope that it will be useful,\r\n *\tbut WITHOUT ANY WARRANTY; without even the implied warranty of\r\n *\tMERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU\r\n *\tLesser General Public License for more details.\r\n *\r\n *\tYou should have received a copy of the GNU Lesser General Public\r\n *\tLicense along with qpOASES; if not, write to the Free Software\r\n *\tFoundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA\r\n *\r\n */\r\n\r\n\r\n/**\r\n *\t\\file SRC/Bounds.cpp\r\n *\t\\author Hans Joachim Ferreau\r\n *\t\\version 1.3embedded\r\n *\t\\date 2007-2008\r\n *\r\n *\tImplementation of the Bounds class designed to manage working sets of\r\n *\tbounds within a QProblem.\r\n */\r\n\r\n\r\n#include \r\n\r\n\r\n\r\n/*****************************************************************************\r\n * P U B L I C *\r\n *****************************************************************************/\r\n\r\n\r\n/*\r\n *\tB o u n d s\r\n */\r\nBounds::Bounds( ) :\tSubjectTo( ),\r\n\t\t\t\t\tnV( 0 ),\r\n\t\t\t\t\tnFV( 0 ),\r\n\t\t\t\t\tnBV( 0 ),\r\n\t\t\t\t\tnUV( 0 )\r\n{\r\n}\r\n\r\n\r\n/*\r\n *\tB o u n d s\r\n */\r\nBounds::Bounds( const Bounds& rhs ) :\tSubjectTo( rhs ),\r\n\t\t\t\t\t\t\t\t\t\tnV( rhs.nV ),\r\n\t\t\t\t\t\t\t\t\t\tnFV( rhs.nFV ),\r\n\t\t\t\t\t\t\t\t\t\tnBV( rhs.nBV ),\r\n\t\t\t\t\t\t\t\t\t\tnUV( rhs.nUV )\r\n{\r\n\tfree = rhs.free;\r\n\tfixed = rhs.fixed;\r\n}\r\n\r\n\r\n/*\r\n *\t~ B o u n d s\r\n */\r\nBounds::~Bounds( )\r\n{\r\n}\r\n\r\n\r\n/*\r\n *\to p e r a t o r =\r\n */\r\nBounds& Bounds::operator=( const Bounds& rhs )\r\n{\r\n\tif ( this != &rhs )\r\n\t{\r\n\t\tSubjectTo::operator=( rhs );\r\n\r\n\t\tnV = rhs.nV;\r\n\t\tnFV = rhs.nFV;\r\n\t\tnBV = rhs.nBV;\r\n\t\tnUV = rhs.nUV;\r\n\r\n\t\tfree = rhs.free;\r\n\t\tfixed = rhs.fixed;\r\n\t}\r\n\r\n\treturn *this;\r\n}\r\n\r\n\r\n/*\r\n *\ti n i t\r\n */\r\nreturnValue Bounds::init( int n )\r\n{\r\n\tnV = n;\r\n\tnFV = 0;\r\n\tnBV = 0;\r\n\tnUV = 0;\r\n\r\n\tfree.init( );\r\n\tfixed.init( );\r\n\r\n\treturn SubjectTo::init( n );\r\n}\r\n\r\n\r\n/*\r\n *\ts e t u p B o u n d\r\n */\r\nreturnValue Bounds::setupBound(\tint _number, SubjectToStatus _status\r\n\t\t\t\t\t\t\t\t)\r\n{\r\n\t/* consistency check */\r\n\tif ( ( _number < 0 ) || ( _number >= getNV( ) ) )\r\n\t\treturn THROWERROR( RET_INDEX_OUT_OF_BOUNDS );\r\n\t\r\n\t/* Add bound index to respective index list. */\r\n\tswitch ( _status )\r\n\t{\r\n\t\tcase ST_INACTIVE:\r\n\t\t\tif ( this->addIndex( this->getFree( ),_number,_status ) != SUCCESSFUL_RETURN )\r\n\t\t\t\treturn THROWERROR( RET_SETUP_BOUND_FAILED );\r\n\t\t\tbreak;\r\n\r\n\t\tcase ST_LOWER:\r\n\t\t\tif ( this->addIndex( this->getFixed( ),_number,_status ) != SUCCESSFUL_RETURN )\r\n\t\t\t\treturn THROWERROR( RET_SETUP_BOUND_FAILED );\r\n\t\t\tbreak;\r\n\r\n\t\tcase ST_UPPER:\r\n\t\t\tif ( this->addIndex( this->getFixed( ),_number,_status ) != SUCCESSFUL_RETURN )\r\n\t\t\t\treturn THROWERROR( RET_SETUP_BOUND_FAILED );\r\n\t\t\tbreak;\r\n\r\n\t\tdefault:\r\n\t\t\treturn THROWERROR( RET_INVALID_ARGUMENTS );\r\n\t}\r\n\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n/*\r\n *\ts e t u p A l l F r e e\r\n */\r\nreturnValue Bounds::setupAllFree( )\r\n{\r\n\tint i;\r\n\r\n\t/* 1) Place unbounded variables at the beginning of the index list of free variables. */\r\n\tfor( i=0; i= getNV( ) ) )\r\n\t\treturn THROWERROR( RET_INDEX_OUT_OF_BOUNDS );\r\n\r\n\t/* Move index from indexlist of fixed variables to that of free ones. */\r\n\tif ( this->removeIndex( this->getFixed( ),_number ) != SUCCESSFUL_RETURN )\r\n\t\treturn THROWERROR( RET_MOVING_BOUND_FAILED );\r\n\r\n\tif ( this->addIndex( this->getFree( ),_number,ST_INACTIVE ) != SUCCESSFUL_RETURN )\r\n\t\treturn THROWERROR( RET_MOVING_BOUND_FAILED );\r\n\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n/*\r\n *\tm o v e F r e e T o F i x e d\r\n */\r\nreturnValue Bounds::moveFreeToFixed(\tint _number, SubjectToStatus _status\r\n\t\t\t\t\t\t\t\t\t\t)\r\n{\r\n\t/* consistency check */\r\n\tif ( ( _number < 0 ) || ( _number >= getNV( ) ) )\r\n\t\treturn THROWERROR( RET_INDEX_OUT_OF_BOUNDS );\r\n\r\n\t/* Move index from indexlist of free variables to that of fixed ones. */\r\n\tif ( this->removeIndex( this->getFree( ),_number ) != SUCCESSFUL_RETURN )\r\n\t\treturn THROWERROR( RET_MOVING_BOUND_FAILED );\r\n\r\n\tif ( this->addIndex( this->getFixed( ),_number,_status ) != SUCCESSFUL_RETURN )\r\n\t\treturn THROWERROR( RET_MOVING_BOUND_FAILED );\r\n\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n/*\r\n *\ts w a p F r e e\r\n */\r\nreturnValue Bounds::swapFree(\tint number1, int number2\r\n\t\t\t\t\t\t\t\t)\r\n{\r\n\t/* consistency check */\r\n\tif ( ( number1 < 0 ) || ( number1 >= getNV( ) ) || ( number2 < 0 ) || ( number2 >= getNV( ) ) )\r\n\t\treturn THROWERROR( RET_INDEX_OUT_OF_BOUNDS );\r\n\r\n\t/* Swap index within indexlist of free variables. */\r\n\treturn this->swapIndex( this->getFree( ),number1,number2 );\r\n}\r\n\r\n\r\n/*\r\n *\tend of file\r\n */\r\n"
},
{
"path": "externals/qpoases/SRC/Bounds.ipp",
"content": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.\r\n *\r\n *\tqpOASES is free software; you can redistribute it and/or\r\n *\tmodify it under the terms of the GNU Lesser General Public\r\n *\tLicense as published by the Free Software Foundation; either\r\n *\tversion 2.1 of the License, or (at your option) any later version.\r\n *\r\n *\tqpOASES is distributed in the hope that it will be useful,\r\n *\tbut WITHOUT ANY WARRANTY; without even the implied warranty of\r\n *\tMERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU\r\n *\tLesser General Public License for more details.\r\n *\r\n *\tYou should have received a copy of the GNU Lesser General Public\r\n *\tLicense along with qpOASES; if not, write to the Free Software\r\n *\tFoundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA\r\n *\r\n */\r\n\r\n\r\n/**\r\n *\t\\file SRC/Bounds.ipp\r\n *\t\\author Hans Joachim Ferreau\r\n *\t\\version 1.3embedded\r\n *\t\\date 2007-2008\r\n *\r\n *\tImplementation of inlined member functions of the Bounds class designed \r\n *\tto manage working sets of bounds within a QProblem.\r\n */\r\n\r\n\r\n/*****************************************************************************\r\n * P U B L I C *\r\n *****************************************************************************/\r\n\r\n/*\r\n *\tg e t N V\r\n */\r\ninline int Bounds::getNV( ) const\r\n{\r\n \treturn nV;\r\n}\r\n\r\n\r\n/*\r\n *\tg e t N F X\r\n */\r\ninline int Bounds::getNFV( ) const\r\n{\r\n \treturn nFV;\r\n}\r\n\r\n\r\n/*\r\n *\tg e t N B V\r\n */\r\ninline int Bounds::getNBV( ) const\r\n{\r\n \treturn nBV;\r\n}\r\n \r\n\r\n/*\r\n *\tg e t N U V\r\n */\r\ninline int Bounds::getNUV( ) const\r\n{\r\n\treturn nUV;\r\n}\r\n\r\n\r\n\r\n/*\r\n *\ts e t N F X\r\n */\r\ninline returnValue Bounds::setNFV( int n )\r\n{\r\n \tnFV = n;\r\n\treturn SUCCESSFUL_RETURN;\t\r\n}\r\n \r\n \r\n/*\r\n *\ts e t N B V\r\n */\r\ninline returnValue Bounds::setNBV( int n )\r\n{\r\n \tnBV = n;\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n \r\n\r\n/*\r\n *\ts e t N U V\r\n */\r\ninline returnValue Bounds::setNUV( int n )\r\n{\r\n\tnUV = n;\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n/*\r\n *\tg e t N F R\r\n */\r\ninline int Bounds::getNFR( )\r\n{\r\n \treturn free.getLength( );\r\n}\r\n\r\n\r\n/*\r\n *\tg e t N F X\r\n */\r\ninline int Bounds::getNFX( )\r\n{\r\n \treturn fixed.getLength( );\r\n}\r\n\r\n\r\n/*\r\n *\tg e t F r e e\r\n */\r\ninline Indexlist* Bounds::getFree( )\r\n{\r\n\treturn &free;\r\n}\r\n\r\n\r\n/*\r\n *\tg e t F i x e d\r\n */\r\ninline Indexlist* Bounds::getFixed( )\r\n{\r\n\treturn &fixed;\r\n}\r\n\r\n\r\n/*\r\n *\tend of file\r\n */\r\n"
},
{
"path": "externals/qpoases/SRC/Constraints.cpp",
"content": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.\r\n *\r\n *\tqpOASES is free software; you can redistribute it and/or\r\n *\tmodify it under the terms of the GNU Lesser General Public\r\n *\tLicense as published by the Free Software Foundation; either\r\n *\tversion 2.1 of the License, or (at your option) any later version.\r\n *\r\n *\tqpOASES is distributed in the hope that it will be useful,\r\n *\tbut WITHOUT ANY WARRANTY; without even the implied warranty of\r\n *\tMERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU\r\n *\tLesser General Public License for more details.\r\n *\r\n *\tYou should have received a copy of the GNU Lesser General Public\r\n *\tLicense along with qpOASES; if not, write to the Free Software\r\n *\tFoundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA\r\n *\r\n */\r\n\r\n\r\n/**\r\n *\t\\file SRC/Constraints.cpp\r\n *\t\\author Hans Joachim Ferreau\r\n *\t\\version 1.3embedded\r\n *\t\\date 2007-2008\r\n *\r\n *\tImplementation of the Constraints class designed to manage working sets of\r\n *\tconstraints within a QProblem.\r\n */\r\n\r\n\r\n#include \r\n\r\n\r\n/*****************************************************************************\r\n * P U B L I C *\r\n *****************************************************************************/\r\n\r\n\r\n/*\r\n *\tC o n s t r a i n t s\r\n */\r\nConstraints::Constraints( ) :\tSubjectTo( ),\r\n\t\t\t\t\t\t\t\tnC( 0 ),\r\n\t\t\t\t\t\t\t\tnEC( 0 ),\r\n\t\t\t\t\t\t\t\tnIC( 0 ),\r\n\t\t\t\t\t\t\t\tnUC( 0 )\r\n{\r\n}\r\n\r\n\r\n/*\r\n *\tC o n s t r a i n t s\r\n */\r\nConstraints::Constraints( const Constraints& rhs ) :\tSubjectTo( rhs ),\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\tnC( rhs.nC ),\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\tnEC( rhs.nEC ),\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\tnIC( rhs.nIC ),\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\tnUC( rhs.nUC )\r\n{\r\n\tactive = rhs.active;\r\n\tinactive = rhs.inactive;\r\n}\r\n\r\n\r\n/*\r\n *\t~ C o n s t r a i n t s\r\n */\r\nConstraints::~Constraints( )\r\n{\r\n}\r\n\r\n\r\n/*\r\n *\to p e r a t o r =\r\n */\r\nConstraints& Constraints::operator=( const Constraints& rhs )\r\n{\r\n\tif ( this != &rhs )\r\n\t{\r\n\t\tSubjectTo::operator=( rhs );\r\n\r\n\t\tnC = rhs.nC;\r\n\t\tnEC = rhs.nEC;\r\n\t\tnIC = rhs.nIC;\r\n\t\tnUC = rhs.nUC;\r\n\r\n\t\tactive = rhs.active;\r\n\t\tinactive = rhs.inactive;\r\n\t}\r\n\r\n\treturn *this;\r\n}\r\n\r\n\r\n/*\r\n *\ti n i t\r\n */\r\nreturnValue Constraints::init( int n )\r\n{\r\n\tnC = n;\r\n\tnEC = 0;\r\n\tnIC = 0;\r\n\tnUC = 0;\r\n\r\n\tactive.init( );\r\n\tinactive.init( );\r\n\r\n\treturn SubjectTo::init( n );\r\n}\r\n\r\n\r\n/*\r\n *\ts e t u p C o n s t r a i n t\r\n */\r\nreturnValue Constraints::setupConstraint(\tint _number, SubjectToStatus _status\r\n\t\t\t\t\t\t\t\t\t\t\t)\r\n{\r\n\t/* consistency check */\r\n\tif ( ( _number < 0 ) || ( _number >= getNC( ) ) )\r\n\t\treturn THROWERROR( RET_INDEX_OUT_OF_BOUNDS );\r\n\t\r\n\t/* Add constraint index to respective index list. */\r\n\tswitch ( _status )\r\n\t{\r\n\t\tcase ST_INACTIVE:\r\n\t\t\tif ( this->addIndex( this->getInactive( ),_number,_status ) != SUCCESSFUL_RETURN )\r\n\t\t\t\treturn THROWERROR( RET_SETUP_CONSTRAINT_FAILED );\r\n\t\t\tbreak;\r\n\r\n\t\tcase ST_LOWER:\r\n\t\t\tif ( this->addIndex( this->getActive( ),_number,_status ) != SUCCESSFUL_RETURN )\r\n\t\t\t\treturn THROWERROR( RET_SETUP_CONSTRAINT_FAILED );\r\n\t\t\tbreak;\r\n\r\n\t\tcase ST_UPPER:\r\n\t\t\tif ( this->addIndex( this->getActive( ),_number,_status ) != SUCCESSFUL_RETURN )\r\n\t\t\t\treturn THROWERROR( RET_SETUP_CONSTRAINT_FAILED );\r\n\t\t\tbreak;\r\n\r\n\t\tdefault:\r\n\t\t\treturn THROWERROR( RET_INVALID_ARGUMENTS );\r\n\t}\r\n\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n/*\r\n *\ts e t u p A l l I n a c t i v e\r\n */\r\nreturnValue Constraints::setupAllInactive( )\r\n{\r\n\tint i;\r\n\r\n\r\n\t/* 1) Place unbounded constraints at the beginning of the index list of inactive constraints. */\r\n\tfor( i=0; i= getNC( ) ) )\r\n\t\treturn THROWERROR( RET_INDEX_OUT_OF_BOUNDS );\r\n\r\n\t/* Move index from indexlist of active constraints to that of inactive ones. */\r\n\tif ( this->removeIndex( this->getActive( ),_number ) != SUCCESSFUL_RETURN )\r\n\t\treturn THROWERROR( RET_MOVING_BOUND_FAILED );\r\n\r\n\tif ( this->addIndex( this->getInactive( ),_number,ST_INACTIVE ) != SUCCESSFUL_RETURN )\r\n\t\treturn THROWERROR( RET_MOVING_BOUND_FAILED );\r\n\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n/*\r\n *\tm o v e I n a c t i v e T o A c t i v e\r\n */\r\nreturnValue Constraints::moveInactiveToActive(\tint _number, SubjectToStatus _status\r\n\t\t\t\t\t\t\t\t\t\t\t\t)\r\n{\r\n\t/* consistency check */\r\n\tif ( ( _number < 0 ) || ( _number >= getNC( ) ) )\r\n\t\treturn THROWERROR( RET_INDEX_OUT_OF_BOUNDS );\r\n\r\n\t/* Move index from indexlist of inactive constraints to that of active ones. */\r\n\tif ( this->removeIndex( this->getInactive( ),_number ) != SUCCESSFUL_RETURN )\r\n\t\treturn THROWERROR( RET_MOVING_BOUND_FAILED );\r\n\r\n\tif ( this->addIndex( this->getActive( ),_number,_status ) != SUCCESSFUL_RETURN )\r\n\t\treturn THROWERROR( RET_MOVING_BOUND_FAILED );\r\n\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n\r\n/*\r\n *\tend of file\r\n */\r\n"
},
{
"path": "externals/qpoases/SRC/Constraints.ipp",
"content": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.\r\n *\r\n *\tqpOASES is free software; you can redistribute it and/or\r\n *\tmodify it under the terms of the GNU Lesser General Public\r\n *\tLicense as published by the Free Software Foundation; either\r\n *\tversion 2.1 of the License, or (at your option) any later version.\r\n *\r\n *\tqpOASES is distributed in the hope that it will be useful,\r\n *\tbut WITHOUT ANY WARRANTY; without even the implied warranty of\r\n *\tMERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU\r\n *\tLesser General Public License for more details.\r\n *\r\n *\tYou should have received a copy of the GNU Lesser General Public\r\n *\tLicense along with qpOASES; if not, write to the Free Software\r\n *\tFoundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA\r\n *\r\n */\r\n\r\n\r\n/**\r\n *\t\\file SRC/Constraints.ipp\r\n *\t\\author Hans Joachim Ferreau\r\n *\t\\version 1.3embedded\r\n *\t\\date 2007-2008\r\n *\r\n *\tDeclaration of inlined member functions of the Constraints class designed \r\n *\tto manage working sets of constraints within a QProblem.\r\n */\r\n\r\n\r\n\r\n/*****************************************************************************\r\n * P U B L I C *\r\n *****************************************************************************/\r\n\r\n/*\r\n *\tg e t N C\r\n */\r\ninline int Constraints::getNC( ) const\r\n{\r\n \treturn nC;\r\n}\r\n \r\n\r\n/*\r\n *\tg e t N E C\r\n */\r\ninline int Constraints::getNEC( ) const\r\n{\r\n \treturn nEC;\r\n}\r\n \r\n\r\n/*\r\n *\tg e t N I C\r\n */\r\ninline int Constraints::getNIC( ) const\r\n{\r\n \treturn nIC;\r\n}\r\n \r\n\r\n/*\r\n *\tg e t N U C\r\n */\r\ninline int Constraints::getNUC( ) const\r\n{\r\n \treturn nUC;\r\n}\r\n\r\n\r\n/*\r\n *\ts e t N E C\r\n */\r\ninline returnValue Constraints::setNEC( int n )\r\n{\r\n \tnEC = n;\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n \r\n\r\n/*\r\n *\ts e t N I C\r\n */\r\ninline returnValue Constraints::setNIC( int n )\r\n{\r\n \tnIC = n;\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n/*\r\n *\ts e t N U C\r\n */\r\ninline returnValue Constraints::setNUC( int n )\r\n{\r\n \tnUC = n;\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n/*\r\n *\tg e t N A C\r\n */\r\ninline int Constraints::getNAC( )\r\n{\r\n \treturn active.getLength( );\r\n}\r\n\r\n\r\n/*\r\n *\tg e t N I A C\r\n */\r\ninline int Constraints::getNIAC( )\r\n{\r\n \treturn inactive.getLength( );\r\n}\r\n\r\n\r\n/*\r\n *\tg e t A c t i v e\r\n */\r\ninline Indexlist* Constraints::getActive( )\r\n{\r\n\treturn &active;\r\n}\r\n\r\n\r\n/*\r\n *\tg e t I n a c t i v e\r\n */\r\ninline Indexlist* Constraints::getInactive( )\r\n{\r\n\treturn &inactive;\r\n}\r\n\r\n\r\n/*\r\n *\tend of file\r\n */\r\n"
},
{
"path": "externals/qpoases/SRC/CyclingManager.cpp",
"content": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.\r\n *\r\n *\tqpOASES is free software; you can redistribute it and/or\r\n *\tmodify it under the terms of the GNU Lesser General Public\r\n *\tLicense as published by the Free Software Foundation; either\r\n *\tversion 2.1 of the License, or (at your option) any later version.\r\n *\r\n *\tqpOASES is distributed in the hope that it will be useful,\r\n *\tbut WITHOUT ANY WARRANTY; without even the implied warranty of\r\n *\tMERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU\r\n *\tLesser General Public License for more details.\r\n *\r\n *\tYou should have received a copy of the GNU Lesser General Public\r\n *\tLicense along with qpOASES; if not, write to the Free Software\r\n *\tFoundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA\r\n *\r\n */\r\n\r\n\r\n/**\r\n *\t\\file SRC/CyclingManager.cpp\r\n *\t\\author Hans Joachim Ferreau\r\n *\t\\version 1.3embedded\r\n *\t\\date 2007-2008\r\n *\r\n *\tImplementation of the CyclingManager class designed to detect\r\n *\tand handle possible cycling during QP iterations.\r\n *\r\n */\r\n\r\n\r\n#include \r\n\r\n\r\n/*****************************************************************************\r\n * P U B L I C *\r\n *****************************************************************************/\r\n\r\n\r\n/*\r\n *\tC y c l i n g M a n a g e r\r\n */\r\nCyclingManager::CyclingManager( ) :\tnV( 0 ),\r\n\t\t\t\t\t\t\t\t\tnC( 0 )\r\n{\r\n\tcyclingDetected = BT_FALSE;\r\n}\r\n\r\n\r\n/*\r\n *\tC y c l i n g M a n a g e r\r\n */\r\nCyclingManager::CyclingManager( const CyclingManager& rhs ) :\tnV( rhs.nV ),\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\tnC( rhs.nC ),\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\tcyclingDetected( rhs.cyclingDetected )\r\n{\r\n\tint i;\r\n\r\n\tfor( i=0; i= 0 ) && ( number < nV ) )\r\n\t\t{\r\n\t\t\tstatus[number] = _status;\r\n\t\t\treturn SUCCESSFUL_RETURN;\r\n\t\t}\r\n\t\telse\r\n\t\t\treturn THROWERROR( RET_INDEX_OUT_OF_BOUNDS );\r\n\t}\r\n\telse\r\n\t{\r\n\t\t/* Set cycling status of a constraint. */\r\n\t\tif ( ( number >= 0 ) && ( number < nC ) )\r\n\t\t{\r\n\t\t\tstatus[nV+number] = _status;\r\n\t\t\treturn SUCCESSFUL_RETURN;\r\n\t\t}\r\n\t\telse\r\n\t\t\treturn THROWERROR( RET_INDEX_OUT_OF_BOUNDS );\r\n\t}\r\n}\r\n\r\n\r\n/*\r\n *\tg e t C y c l i n g S t a t u s\r\n */\r\nCyclingStatus CyclingManager::getCyclingStatus( int number, BooleanType isBound ) const\r\n{\r\n\tif ( isBound == BT_TRUE )\r\n\t{\r\n\t\t/* Return cycling status of a bound. */\r\n\t\tif ( ( number >= 0 ) && ( number < nV ) )\r\n\t\t\treturn status[number];\r\n\t}\r\n\telse\r\n\t{\r\n\t\t/* Return cycling status of a constraint. */\r\n\t\tif ( ( number >= 0 ) && ( number < nC ) )\r\n\t\t\treturn status[nV+number];\r\n\t}\r\n\r\n\treturn CYC_NOT_INVOLVED;\r\n}\r\n\r\n\r\n/*\r\n *\tc l e a r C y c l i n g D a t a\r\n */\r\nreturnValue CyclingManager::clearCyclingData( )\r\n{\r\n\tint i;\r\n\r\n\t/* Reset all status values ... */\r\n\tfor( i=0; i\n\n/*\n *\tS o l u t i o n A n a l y s i s\n */\nSolutionAnalysis::SolutionAnalysis( )\n{\n\t\n}\n\n/*\n *\tS o l u t i o n A n a l y s i s\n */\nSolutionAnalysis::SolutionAnalysis( const SolutionAnalysis& rhs )\n{\n\t\n}\n\n/*\n *\t~ S o l u t i o n A n a l y s i s\n */\nSolutionAnalysis::~SolutionAnalysis( )\n{\n\t\n}\n\n/*\n *\to p e r a t o r =\n */\nSolutionAnalysis& SolutionAnalysis::operator=( const SolutionAnalysis& rhs )\n{\n\tif ( this != &rhs )\n\t{\n\t\t\n\t}\n\t\n\treturn *this;\n}\n\n/*\n * g e t H e s s i a n I n v e r s e\n */\nreturnValue SolutionAnalysis::getHessianInverse( QProblem* qp, real_t* hessianInverse )\n{\n\treturnValue returnvalue; /* the return value */\n\tBooleanType Delta_bC_isZero = BT_FALSE; /* (just use FALSE here) */\n\tBooleanType Delta_bB_isZero = BT_FALSE; /* (just use FALSE here) */\n\t\n\tregister int run1, run2, run3;\n\t\n\tregister int nFR, nFX;\n\t\n\t/* Ask for the number of free and fixed variables, assumes that active set\n\t * is constant for the covariance evaluation */\n\tnFR = qp->getNFR( );\n\tnFX = qp->getNFX( );\n\t\n\t/* Ask for the corresponding index arrays: */\n\tif ( qp->bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )\n\t\treturn THROWERROR( RET_HOTSTART_FAILED );\n\t\n\tif ( qp->bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN )\n\t\treturn THROWERROR( RET_HOTSTART_FAILED );\n\t\n\tif ( qp->constraints.getActive( )->getNumberArray( AC_idx ) != SUCCESSFUL_RETURN )\n\t\treturn THROWERROR( RET_HOTSTART_FAILED );\n\t\n\t/* Initialization: */\n\tfor( run1 = 0; run1 < NVMAX; run1++ )\n\t\tdelta_g_cov[ run1 ] = 0.0;\n\t\n\tfor( run1 = 0; run1 < NVMAX; run1++ )\n\t\tdelta_lb_cov[ run1 ] = 0.0;\n\t\n\tfor( run1 = 0; run1 < NVMAX; run1++ )\n\t\tdelta_ub_cov[ run1 ] = 0.0;\n\t\n\tfor( run1 = 0; run1 < NCMAX; run1++ )\n\t\tdelta_lbA_cov[ run1 ] = 0.0;\n\t\n\tfor( run1 = 0; run1 < NCMAX; run1++ )\n\t\tdelta_ubA_cov[ run1 ] = 0.0;\n\t\n\t/* The following loop solves the following:\n\t *\n\t * KKT * x =\n\t * [delta_g_cov', delta_lbA_cov', delta_ubA_cov', delta_lb_cov', delta_ub_cov]'\n\t *\n\t * for the first NVMAX (negative) elementary vectors in order to get\n\t * transposed inverse of the Hessian. Assuming that the Hessian is\n\t * symmetric, the function will return transposed inverse, instead of the\n\t * true inverse.\n\t *\n\t * Note, that we use negative elementary vectors due because internal\n\t * implementation of the function hotstart_determineStepDirection requires\n\t * so.\n\t *\n\t * */\n\t\n\tfor( run3 = 0; run3 < NVMAX; run3++ )\n\t{\n\t\t/* Line wise loading of the corresponding (negative) elementary vector: */\n\t\tdelta_g_cov[ run3 ] = -1.0;\n\t\t\n\t\t/* Evaluation of the step: */\n\t\treturnvalue = qp->hotstart_determineStepDirection(\n\t\t\tFR_idx, FX_idx, AC_idx,\n\t\t\tdelta_g_cov, delta_lbA_cov, delta_ubA_cov, delta_lb_cov, delta_ub_cov,\n\t\t\tDelta_bC_isZero, Delta_bB_isZero,\n\t\t\tdelta_xFX, delta_xFR, delta_yAC, delta_yFX\n\t\t\t);\n\t\tif ( returnvalue != SUCCESSFUL_RETURN )\n\t\t{\n\t\t\treturn returnvalue;\n\t\t}\n\t\t\n\t\t/* Line wise storage of the QP reaction: */\n\t\tfor( run1 = 0; run1 < nFR; run1++ )\n\t\t{\n\t\t\trun2 = FR_idx[ run1 ];\n\t\t\t\n\t\t\thessianInverse[run3 * NVMAX + run2] = delta_xFR[ run1 ];\n\t\t} \n\t\t\n\t\tfor( run1 = 0; run1 < nFX; run1++ )\n\t\t{ \n\t\t\trun2 = FX_idx[ run1 ];\n\t\t\t\n\t\t\thessianInverse[run3 * NVMAX + run2] = delta_xFX[ run1 ];\n\t\t}\n\t\t\n\t\t/* Prepare for the next iteration */\n\t\tdelta_g_cov[ run3 ] = 0.0;\n\t}\n\t\n\t// TODO: Perform the transpose of the inverse of the Hessian matrix\n\t\n\treturn SUCCESSFUL_RETURN; \n}\n\n/*\n * g e t H e s s i a n I n v e r s e\n */\nreturnValue SolutionAnalysis::getHessianInverse( QProblemB* qp, real_t* hessianInverse )\n{\n\treturnValue returnvalue; /* the return value */\n\tBooleanType Delta_bB_isZero = BT_FALSE; /* (just use FALSE here) */\n\t\n\tregister int run1, run2, run3;\n\t\n\tregister int nFR, nFX;\n\t\n\t/* Ask for the number of free and fixed variables, assumes that active set\n\t * is constant for the covariance evaluation */\n\tnFR = qp->getNFR( );\n\tnFX = qp->getNFX( );\n\t\n\t/* Ask for the corresponding index arrays: */\n\tif ( qp->bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )\n\t\treturn THROWERROR( RET_HOTSTART_FAILED );\n\t\n\tif ( qp->bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN )\n\t\treturn THROWERROR( RET_HOTSTART_FAILED );\n\t\n\t/* Initialization: */\n\tfor( run1 = 0; run1 < NVMAX; run1++ )\n\t\tdelta_g_cov[ run1 ] = 0.0;\n\t\n\tfor( run1 = 0; run1 < NVMAX; run1++ )\n\t\tdelta_lb_cov[ run1 ] = 0.0;\n\t\n\tfor( run1 = 0; run1 < NVMAX; run1++ )\n\t\tdelta_ub_cov[ run1 ] = 0.0;\n\t\n\t/* The following loop solves the following:\n\t *\n\t * KKT * x =\n\t * [delta_g_cov', delta_lb_cov', delta_ub_cov']'\n\t *\n\t * for the first NVMAX (negative) elementary vectors in order to get\n\t * transposed inverse of the Hessian. Assuming that the Hessian is\n\t * symmetric, the function will return transposed inverse, instead of the\n\t * true inverse.\n\t *\n\t * Note, that we use negative elementary vectors due because internal\n\t * implementation of the function hotstart_determineStepDirection requires\n\t * so.\n\t *\n\t * */\n\t\n\tfor( run3 = 0; run3 < NVMAX; run3++ )\n\t{\n\t\t/* Line wise loading of the corresponding (negative) elementary vector: */\n\t\tdelta_g_cov[ run3 ] = -1.0;\n\t\t\n\t\t/* Evaluation of the step: */\n\t\treturnvalue = qp->hotstart_determineStepDirection(\n\t\t\tFR_idx, FX_idx,\n\t\t\tdelta_g_cov, delta_lb_cov, delta_ub_cov,\n\t\t\tDelta_bB_isZero,\n\t\t\tdelta_xFX, delta_xFR, delta_yFX\n\t\t\t);\n\t\tif ( returnvalue != SUCCESSFUL_RETURN )\n\t\t{\n\t\t\treturn returnvalue;\n\t\t}\n\t\t\t\t\n\t\t/* Line wise storage of the QP reaction: */\n\t\tfor( run1 = 0; run1 < nFR; run1++ )\n\t\t{\n\t\t\trun2 = FR_idx[ run1 ];\n\t\t\t\n\t\t\thessianInverse[run3 * NVMAX + run2] = delta_xFR[ run1 ];\n\t\t} \n\t\t\n\t\tfor( run1 = 0; run1 < nFX; run1++ )\n\t\t{ \n\t\t\trun2 = FX_idx[ run1 ];\n\t\t\t\n\t\t\thessianInverse[run3 * NVMAX + run2] = delta_xFX[ run1 ];\n\t\t}\n\t\t\n\t\t/* Prepare for the next iteration */\n\t\tdelta_g_cov[ run3 ] = 0.0;\n\t}\n\t\n\t// TODO: Perform the transpose of the inverse of the Hessian matrix\n\t\n\treturn SUCCESSFUL_RETURN; \n}\n\n/*\n * g e t V a r i a n c e C o v a r i a n c e\n */\n\n#if QPOASES_USE_OLD_VERSION\n\nreturnValue SolutionAnalysis::getVarianceCovariance( QProblem* qp, real_t* g_b_bA_VAR, real_t* Primal_Dual_VAR )\n{\n\tint run1, run2, run3; /* simple run variables (for loops). */\n\t\n\treturnValue returnvalue; /* the return value */\n\tBooleanType Delta_bC_isZero = BT_FALSE; /* (just use FALSE here) */\n\tBooleanType Delta_bB_isZero = BT_FALSE; /* (just use FALSE here) */\n\t\n\t/* ASK FOR THE NUMBER OF FREE AND FIXED VARIABLES:\n\t * (ASSUMES THAT ACTIVE SET IS CONSTANT FOR THE\n\t * VARIANCE-COVARIANCE EVALUATION)\n\t * ----------------------------------------------- */\n\tint nFR, nFX, nAC;\n\t\n\tnFR = qp->getNFR( );\n\tnFX = qp->getNFX( );\n\tnAC = qp->getNAC( );\n\t\n\tif ( qp->bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )\n\t\treturn THROWERROR( RET_HOTSTART_FAILED );\n\t\n\tif ( qp->bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN )\n\t\treturn THROWERROR( RET_HOTSTART_FAILED );\n\t\n\tif ( qp->constraints.getActive( )->getNumberArray( AC_idx ) != SUCCESSFUL_RETURN )\n\t\treturn THROWERROR( RET_HOTSTART_FAILED );\n\t\n\t/* SOME INITIALIZATIONS:\n\t * --------------------- */\n\tfor( run1 = 0; run1 < KKT_DIM * KKT_DIM; run1++ )\n\t{\n\t\tK [run1] = 0.0;\n\t\tPrimal_Dual_VAR[run1] = 0.0;\n\t}\n\t\n\t/* ================================================================= */\n\t\n\t/* FIRST MATRIX MULTIPLICATION (OBTAINS THE INTERMEDIATE RESULT\n\t * K := [ (\"ACTIVE\" KKT-MATRIX OF THE QP)^(-1) * g_b_bA_VAR ]^T )\n\t * THE EVALUATION OF THE INVERSE OF THE KKT-MATRIX OF THE QP\n\t * WITH RESPECT TO THE CURRENT ACTIVE SET\n\t * USES THE EXISTING CHOLESKY AND TQ-DECOMPOSITIONS. FOR DETAILS\n\t * cf. THE (protected) FUNCTION determineStepDirection. */\n\t\n\tfor( run3 = 0; run3 < KKT_DIM; run3++ )\n\t{\n\t\t\n\t\tfor( run1 = 0; run1 < NVMAX; run1++ )\n\t\t{\n\t\t\tdelta_g_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+run1];\n\t\t\tdelta_lb_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+NVMAX+run1]; /* LINE-WISE LOADING OF THE INPUT */\n\t\t\tdelta_ub_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+NVMAX+run1]; /* VARIANCE-COVARIANCE */\n\t\t}\n\t\tfor( run1 = 0; run1 < NCMAX; run1++ )\n\t\t{\n\t\t\tdelta_lbA_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+2*NVMAX+run1];\n\t\t\tdelta_ubA_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+2*NVMAX+run1];\n\t\t}\n\t\t\n\t\t/* EVALUATION OF THE STEP:\n\t\t * ------------------------------------------------------------------------------ */\n\t\t\n\t\treturnvalue = qp->hotstart_determineStepDirection(\n\t\t\tFR_idx, FX_idx, AC_idx,\n\t\t\tdelta_g_cov, delta_lbA_cov, delta_ubA_cov, delta_lb_cov, delta_ub_cov,\n\t\t\tDelta_bC_isZero, Delta_bB_isZero, delta_xFX,delta_xFR,\n\t\t\tdelta_yAC,delta_yFX );\n\t\t\n\t\t/* ------------------------------------------------------------------------------ */\n\t\t\n\t\t/* STOP THE ALGORITHM IN THE CASE OF NO SUCCESFUL RETURN:\n\t\t * ------------------------------------------------------ */\n\t\tif ( returnvalue != SUCCESSFUL_RETURN )\n\t\t{\n\t\t\treturn returnvalue;\n\t\t}\n\t\t\n\t\t/* LINE WISE */\n\t\t/* STORAGE OF THE QP-REACTION */\n\t\t/* (uses the index list) */\n\t\t\n\t\tfor( run1=0; run1hotstart_determineStepDirection(\n\t\t\tFR_idx, FX_idx, AC_idx,\n\t\t\tdelta_g_cov, delta_lbA_cov, delta_ubA_cov, delta_lb_cov, delta_ub_cov,\n\t\t\tDelta_bC_isZero, Delta_bB_isZero, delta_xFX,delta_xFR,\n\t\t\tdelta_yAC,delta_yFX );\n\t\t\n\t\t/* ------------------------------------------------------------------------------ */\n\t\t\n\t\t/* STOP THE ALGORITHM IN THE CASE OF NO SUCCESFUL RETURN:\n\t\t * ------------------------------------------------------ */\n\t\tif ( returnvalue != SUCCESSFUL_RETURN )\n\t\t{\n\t\t\treturn returnvalue;\n\t\t}\n\t\t\n\t\t/* ROW-WISE STORAGE */\n\t\t/* OF THE RESULT. */\n\t\t\n\t\tfor( run1=0; run1\r\n\r\n\r\n/*****************************************************************************\r\n * P U B L I C *\r\n *****************************************************************************/\r\n\r\n\r\n/*\r\n *\tI n d e x l i s t\r\n */\r\nIndexlist::Indexlist( ) :\tlength( 0 ),\r\n\t\t\t\t\t\t\tfirst( -1 ),\r\n\t\t\t\t\t\t\tlast( -1 ),\r\n\t\t\t\t\t\t\tlastusedindex( -1 ),\r\n\t\t\t\t\t\t\tphysicallength( INDEXLISTFACTOR*(NVMAX+NCMAX) )\r\n{\r\n\tint i;\r\n\r\n\tfor( i=0; i= 0 ) && ( number[n] >= 0 ) )\r\n\t\t\tnumberarray[i] = number[n];\r\n\t\telse\r\n\t\t\treturn THROWERROR( RET_INDEXLIST_CORRUPTED );\r\n\r\n\t\tn = next[n];\r\n\t}\r\n\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n/*\r\n *\tg e t I n d e x\r\n */\r\nint Indexlist::getIndex( int givennumber ) const\r\n{\r\n\tint i;\r\n\tint n = first;\r\n\tint index = -1;\t/* return -1 by default */\r\n\r\n\t/* Run trough indexlist until number is found, if so return it index. */\r\n\tfor ( i=0; i length ) )\r\n\t\treturn -RET_INDEXLIST_OUTOFBOUNDS;\r\n\r\n\treturn number[physicalindex];\r\n}\r\n\r\n\r\n/*\r\n *\tg e t L e n g t h\r\n */\r\ninline int Indexlist::getLength( )\r\n{\r\n\treturn length;\r\n}\r\n\r\n\r\n/*\r\n *\tg e t L a s t N u m b e r\r\n */\r\ninline int Indexlist::getLastNumber( ) const\r\n{\r\n\treturn number[last];\r\n}\r\n\r\n\r\n/*\r\n *\tg e t L a s t N u m b e r\r\n */\r\ninline BooleanType Indexlist::isMember( int _number ) const\r\n{\r\n\tif ( getIndex( _number ) >= 0 )\r\n\t\treturn BT_TRUE;\r\n\telse\r\n\t\treturn BT_FALSE;\r\n}\r\n\r\n\r\n/*\r\n *\tend of file\r\n */\r\n"
},
{
"path": "externals/qpoases/SRC/MessageHandling.cpp",
"content": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.\r\n *\r\n *\tqpOASES is free software; you can redistribute it and/or\r\n *\tmodify it under the terms of the GNU Lesser General Public\r\n *\tLicense as published by the Free Software Foundation; either\r\n *\tversion 2.1 of the License, or (at your option) any later version.\r\n *\r\n *\tqpOASES is distributed in the hope that it will be useful,\r\n *\tbut WITHOUT ANY WARRANTY; without even the implied warranty of\r\n *\tMERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU\r\n *\tLesser General Public License for more details.\r\n *\r\n *\tYou should have received a copy of the GNU Lesser General Public\r\n *\tLicense along with qpOASES; if not, write to the Free Software\r\n *\tFoundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA\r\n *\r\n */\r\n\r\n\r\n/**\r\n *\t\\file SRC/MessageHandling.cpp\r\n *\t\\author Hans Joachim Ferreau\r\n *\t\\version 1.3embedded\r\n *\t\\date 2007-2008\r\n *\r\n *\tImplementation of the MessageHandling class including global return values.\r\n *\r\n */\r\n\r\n\r\n\r\n#include \r\n#include \r\n\r\n\r\n\r\n\r\n/** Defines pairs of global return values and messages. */\r\nMessageHandling::ReturnValueList returnValueList[] =\r\n{\r\n/* miscellaneous */\r\n{ SUCCESSFUL_RETURN, \"Successful return\", VS_VISIBLE },\r\n{ RET_DIV_BY_ZERO, \"Division by zero\", VS_VISIBLE },\r\n{ RET_INDEX_OUT_OF_BOUNDS, \"Index out of bounds\", VS_VISIBLE },\r\n{ RET_INVALID_ARGUMENTS, \"At least one of the arguments is invalid\", VS_VISIBLE },\r\n{ RET_ERROR_UNDEFINED, \"Error number undefined\", VS_VISIBLE },\r\n{ RET_WARNING_UNDEFINED, \"Warning number undefined\", VS_VISIBLE },\r\n{ RET_INFO_UNDEFINED, \"Info number undefined\", VS_VISIBLE },\r\n{ RET_EWI_UNDEFINED, \"Error/warning/info number undefined\", VS_VISIBLE },\r\n{ RET_AVAILABLE_WITH_LINUX_ONLY, \"This function is available under Linux only\", VS_HIDDEN },\r\n{ RET_UNKNOWN_BUG, \"The error occured is not yet known\", VS_VISIBLE },\r\n{ RET_PRINTLEVEL_CHANGED, \"Print level changed\", VS_VISIBLE },\r\n{ RET_NOT_YET_IMPLEMENTED, \"Requested function is not yet implemented.\", VS_VISIBLE },\r\n/* Indexlist */\r\n{ RET_INDEXLIST_MUST_BE_REORDERD, \"Index list has to be reordered\", VS_VISIBLE },\r\n{ RET_INDEXLIST_EXCEEDS_MAX_LENGTH, \"Index list exceeds its maximal physical length\", VS_VISIBLE },\r\n{ RET_INDEXLIST_CORRUPTED, \"Index list corrupted\", VS_VISIBLE },\r\n{ RET_INDEXLIST_OUTOFBOUNDS, \"Physical index is out of bounds\", VS_VISIBLE },\r\n{ RET_INDEXLIST_ADD_FAILED, \"Adding indices from another index set failed\", VS_VISIBLE },\r\n{ RET_INDEXLIST_INTERSECT_FAILED, \"Intersection with another index set failed\", VS_VISIBLE },\r\n/* SubjectTo / Bounds / Constraints */\r\n{ RET_INDEX_ALREADY_OF_DESIRED_STATUS, \"Index is already of desired status\", VS_VISIBLE },\r\n{ RET_SWAPINDEX_FAILED, \"Cannot swap between different indexsets\", VS_VISIBLE },\r\n{ RET_ADDINDEX_FAILED, \"Adding index to index set failed\", VS_VISIBLE },\r\n{ RET_NOTHING_TO_DO, \"Nothing to do\", VS_VISIBLE },\r\n{ RET_SETUP_BOUND_FAILED, \"Setting up bound index failed\", VS_VISIBLE },\r\n{ RET_SETUP_CONSTRAINT_FAILED, \"Setting up constraint index failed\", VS_VISIBLE },\r\n{ RET_MOVING_BOUND_FAILED, \"Moving bound between index sets failed\", VS_VISIBLE },\r\n{ RET_MOVING_CONSTRAINT_FAILED, \"Moving constraint between index sets failed\", VS_VISIBLE },\r\n/* QProblem */\r\n{ RET_QP_ALREADY_INITIALISED, \"QProblem has already been initialised\", VS_VISIBLE },\r\n{ RET_NO_INIT_WITH_STANDARD_SOLVER, \"Initialisation via extern QP solver is not yet implemented\", VS_VISIBLE },\r\n{ RET_RESET_FAILED, \"Reset failed\", VS_VISIBLE },\r\n{ RET_INIT_FAILED, \"Initialisation failed\", VS_VISIBLE },\r\n{ RET_INIT_FAILED_TQ, \"Initialisation failed due to TQ factorisation\", VS_VISIBLE },\r\n{ RET_INIT_FAILED_CHOLESKY, \"Initialisation failed due to Cholesky decomposition\", VS_VISIBLE },\r\n{ RET_INIT_FAILED_HOTSTART, \"Initialisation failed! QP could not be solved!\", VS_VISIBLE },\r\n{ RET_INIT_FAILED_INFEASIBILITY, \"Initial QP could not be solved due to infeasibility!\", VS_VISIBLE },\r\n{ RET_INIT_FAILED_UNBOUNDEDNESS, \"Initial QP could not be solved due to unboundedness!\", VS_VISIBLE },\r\n{ RET_INIT_SUCCESSFUL, \"Initialisation done\", VS_VISIBLE },\r\n{ RET_OBTAINING_WORKINGSET_FAILED, \"Failed to obtain working set for auxiliary QP\", VS_VISIBLE },\r\n{ RET_SETUP_WORKINGSET_FAILED, \"Failed to setup working set for auxiliary QP\", VS_VISIBLE },\r\n{ RET_SETUP_AUXILIARYQP_FAILED, \"Failed to setup auxiliary QP for initialised homotopy\", VS_VISIBLE },\r\n{ RET_NO_EXTERN_SOLVER, \"No extern QP solver available\", VS_VISIBLE },\r\n{ RET_QP_UNBOUNDED, \"QP is unbounded\", VS_VISIBLE },\r\n{ RET_QP_INFEASIBLE, \"QP is infeasible\", VS_VISIBLE },\r\n{ RET_QP_NOT_SOLVED, \"Problems occured while solving QP with standard solver\", VS_VISIBLE },\r\n{ RET_QP_SOLVED, \"QP successfully solved\", VS_VISIBLE },\r\n{ RET_UNABLE_TO_SOLVE_QP, \"Problems occured while solving QP\", VS_VISIBLE },\r\n{ RET_INITIALISATION_STARTED, \"Starting problem initialisation...\", VS_VISIBLE },\r\n{ RET_HOTSTART_FAILED, \"Unable to perform homotopy due to internal error\", VS_VISIBLE },\r\n{ RET_HOTSTART_FAILED_TO_INIT, \"Unable to initialise problem\", VS_VISIBLE },\r\n{ RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED, \"Unable to perform homotopy as previous QP is not solved\", VS_VISIBLE },\r\n{ RET_ITERATION_STARTED, \"Iteration\", VS_VISIBLE },\r\n{ RET_SHIFT_DETERMINATION_FAILED, \"Determination of shift of the QP data failed\", VS_VISIBLE },\r\n{ RET_STEPDIRECTION_DETERMINATION_FAILED, \"Determination of step direction failed\", VS_VISIBLE },\r\n{ RET_STEPLENGTH_DETERMINATION_FAILED, \"Determination of step direction failed\", VS_VISIBLE },\r\n{ RET_OPTIMAL_SOLUTION_FOUND, \"Optimal solution of neighbouring QP found\", VS_VISIBLE },\r\n{ RET_HOMOTOPY_STEP_FAILED, \"Unable to perform homotopy step\", VS_VISIBLE },\r\n{ RET_HOTSTART_STOPPED_INFEASIBILITY, \"Premature homotopy termination because QP is infeasible\", VS_VISIBLE },\r\n{ RET_HOTSTART_STOPPED_UNBOUNDEDNESS, \"Premature homotopy termination because QP is unbounded\", VS_VISIBLE },\r\n{ RET_WORKINGSET_UPDATE_FAILED, \"Unable to update working sets according to initial guesses\", VS_VISIBLE },\r\n{ RET_MAX_NWSR_REACHED, \"Maximum number of working set recalculations performed\", VS_VISIBLE },\r\n{ RET_CONSTRAINTS_NOT_SPECIFIED, \"Problem does comprise constraints! You have to specify new constraints' bounds\", VS_VISIBLE },\r\n{ RET_INVALID_FACTORISATION_FLAG, \"Invalid factorisation flag\", VS_VISIBLE },\r\n{ RET_UNABLE_TO_SAVE_QPDATA, \"Unable to save QP data\", VS_VISIBLE },\r\n{ RET_STEPDIRECTION_FAILED_TQ, \"Abnormal termination due to TQ factorisation\", VS_VISIBLE },\r\n{ RET_STEPDIRECTION_FAILED_CHOLESKY, \"Abnormal termination due to Cholesky factorisation\", VS_VISIBLE },\r\n{ RET_CYCLING_DETECTED, \"Cycling detected\", VS_VISIBLE },\r\n{ RET_CYCLING_NOT_RESOLVED, \"Cycling cannot be resolved, QP is probably infeasible\", VS_VISIBLE },\r\n{ RET_CYCLING_RESOLVED, \"Cycling probably resolved\", VS_VISIBLE },\r\n{ RET_STEPSIZE, \"\", VS_VISIBLE },\r\n{ RET_STEPSIZE_NONPOSITIVE, \"\", VS_VISIBLE },\r\n{ RET_SETUPSUBJECTTOTYPE_FAILED, \"Setup of SubjectToTypes failed\", VS_VISIBLE },\r\n{ RET_ADDCONSTRAINT_FAILED, \"Addition of constraint to working set failed\", VS_VISIBLE },\r\n{ RET_ADDCONSTRAINT_FAILED_INFEASIBILITY, \"Addition of constraint to working set failed\", VS_VISIBLE },\r\n{ RET_ADDBOUND_FAILED, \"Addition of bound to working set failed\", VS_VISIBLE },\r\n{ RET_ADDBOUND_FAILED_INFEASIBILITY, \"Addition of bound to working set failed\", VS_VISIBLE },\r\n{ RET_REMOVECONSTRAINT_FAILED, \"Removal of constraint from working set failed\", VS_VISIBLE },\r\n{ RET_REMOVEBOUND_FAILED, \"Removal of bound from working set failed\", VS_VISIBLE },\r\n{ RET_REMOVE_FROM_ACTIVESET, \"Removing from active set:\", VS_VISIBLE },\r\n{ RET_ADD_TO_ACTIVESET, \"Adding to active set:\", VS_VISIBLE },\r\n{ RET_REMOVE_FROM_ACTIVESET_FAILED, \"Removing from active set failed\", VS_VISIBLE },\r\n{ RET_ADD_TO_ACTIVESET_FAILED, \"Adding to active set failed\", VS_VISIBLE },\r\n{ RET_CONSTRAINT_ALREADY_ACTIVE, \"Constraint is already active\", VS_VISIBLE },\r\n{ RET_ALL_CONSTRAINTS_ACTIVE, \"All constraints are active, no further constraint can be added\", VS_VISIBLE },\r\n{ RET_LINEARLY_DEPENDENT, \"New bound/constraint is linearly dependent\", VS_VISIBLE },\r\n{ RET_LINEARLY_INDEPENDENT, \"New bound/constraint is linearly independent\", VS_VISIBLE },\r\n{ RET_LI_RESOLVED, \"Linear independence of active contraint matrix successfully resolved\", VS_VISIBLE },\r\n{ RET_ENSURELI_FAILED, \"Failed to ensure linear indepence of active contraint matrix\", VS_VISIBLE },\r\n{ RET_ENSURELI_FAILED_TQ, \"Abnormal termination due to TQ factorisation\", VS_VISIBLE },\r\n{ RET_ENSURELI_FAILED_NOINDEX, \"No index found, QP is probably infeasible\", VS_VISIBLE },\r\n{ RET_ENSURELI_FAILED_CYCLING, \"Cycling detected, QP is probably infeasible\", VS_VISIBLE },\r\n{ RET_BOUND_ALREADY_ACTIVE, \"Bound is already active\", VS_VISIBLE },\r\n{ RET_ALL_BOUNDS_ACTIVE, \"All bounds are active, no further bound can be added\", VS_VISIBLE },\r\n{ RET_CONSTRAINT_NOT_ACTIVE, \"Constraint is not active\", VS_VISIBLE },\r\n{ RET_BOUND_NOT_ACTIVE, \"Bound is not active\", VS_VISIBLE },\r\n{ RET_HESSIAN_NOT_SPD, \"Projected Hessian matrix not positive definite\", VS_VISIBLE },\r\n{ RET_MATRIX_SHIFT_FAILED, \"Unable to update matrices or to transform vectors\", VS_VISIBLE },\r\n{ RET_MATRIX_FACTORISATION_FAILED, \"Unable to calculate new matrix factorisations\", VS_VISIBLE },\r\n{ RET_PRINT_ITERATION_FAILED, \"Unable to print information on current iteration\", VS_VISIBLE },\r\n{ RET_NO_GLOBAL_MESSAGE_OUTPUTFILE, \"No global message output file initialised\", VS_VISIBLE },\r\n/* Utils */\r\n{ RET_UNABLE_TO_OPEN_FILE, \"Unable to open file\", VS_VISIBLE },\r\n{ RET_UNABLE_TO_WRITE_FILE, \"Unable to write into file\", VS_VISIBLE },\r\n{ RET_UNABLE_TO_READ_FILE, \"Unable to read from file\", VS_VISIBLE },\r\n{ RET_FILEDATA_INCONSISTENT, \"File contains inconsistent data\", VS_VISIBLE },\r\n/* SolutionAnalysis */\r\n{ RET_NO_SOLUTION, \"QP solution does not satisfy KKT optimality conditions\", VS_VISIBLE },\r\n{ RET_INACCURATE_SOLUTION, \"KKT optimality conditions not satisfied to sufficient accuracy\", VS_VISIBLE },\r\n{ TERMINAL_LIST_ELEMENT, \"\", VS_HIDDEN } /* IMPORTANT: Terminal list element! */\r\n};\r\n\r\n\r\n\r\n/*****************************************************************************\r\n * P U B L I C *\r\n *****************************************************************************/\r\n\r\n\r\n/*\r\n *\tM e s s a g e H a n d l i n g\r\n */\r\nMessageHandling::MessageHandling( ) :\terrorVisibility( VS_VISIBLE ),\r\n\t\t\t\t\t\t\t\t\t\twarningVisibility( VS_VISIBLE ),\r\n\t\t\t\t\t\t\t\t\t\tinfoVisibility( VS_VISIBLE ),\r\n\t\t\t\t\t\t\t\t\t\toutputFile( myStdout ),\r\n\t\t\t\t\t\t\t\t\t\terrorCount( 0 )\r\n{\r\n}\r\n\r\n/*\r\n *\tM e s s a g e H a n d l i n g\r\n */\r\nMessageHandling::MessageHandling( myFILE* _outputFile ) :\r\n\t\t\t\t\t\t\t\t\t\terrorVisibility( VS_VISIBLE ),\r\n\t\t\t\t\t\t\t\t\t\twarningVisibility( VS_VISIBLE ),\r\n\t\t\t\t\t\t\t\t\t\tinfoVisibility( VS_VISIBLE ),\r\n\t\t\t\t\t\t\t\t\t\toutputFile( _outputFile ),\r\n\t\t\t\t\t\t\t\t\t\terrorCount( 0 )\r\n{\r\n}\r\n\r\n/*\r\n *\tM e s s a g e H a n d l i n g\r\n */\r\nMessageHandling::MessageHandling(\tVisibilityStatus _errorVisibility,\r\n\t\t\t\t\t\t\t\t\tVisibilityStatus _warningVisibility,\r\n\t\t \t\t\t\t\t\t\tVisibilityStatus _infoVisibility\r\n\t\t\t\t\t\t\t\t\t) :\r\n\t\t\t\t\t\t\t\t\t\terrorVisibility( _errorVisibility ),\r\n\t\t\t\t\t\t\t\t\t\twarningVisibility( _warningVisibility ),\r\n\t\t\t\t\t\t\t\t\t\tinfoVisibility( _infoVisibility ),\r\n\t\t\t\t\t\t\t\t\t\toutputFile( myStderr ),\r\n\t\t\t\t\t\t\t\t\t\terrorCount( 0 )\r\n{\r\n}\r\n\r\n/*\r\n *\tM e s s a g e H a n d l i n g\r\n */\r\nMessageHandling::MessageHandling( \tmyFILE* _outputFile,\r\n\t\t\t\t\t\t\t\t\tVisibilityStatus _errorVisibility,\r\n\t\t\t\t\t\t\t\t\tVisibilityStatus _warningVisibility,\r\n\t\t \t\t\t\t\t\t\tVisibilityStatus _infoVisibility\r\n\t\t\t\t\t\t\t\t\t) :\r\n\t\t\t\t\t\t\t\t\t\terrorVisibility( _errorVisibility ),\r\n\t\t\t\t\t\t\t\t\t\twarningVisibility( _warningVisibility ),\r\n\t\t\t\t\t\t\t\t\t\tinfoVisibility( _infoVisibility ),\r\n\t\t\t\t\t\t\t\t\t\toutputFile( _outputFile ),\r\n\t\t\t\t\t\t\t\t\t\terrorCount( 0 )\r\n{\r\n}\r\n\r\n\r\n\r\n/*\r\n *\tM e s s a g e H a n d l i n g\r\n */\r\nMessageHandling::MessageHandling( const MessageHandling& rhs ) :\r\n\t\t\t\t\t\t\t\t\t\terrorVisibility( rhs.errorVisibility ),\r\n\t\t\t\t\t\t\t\t\t\twarningVisibility( rhs.warningVisibility ),\r\n\t\t\t\t\t\t\t\t\t\tinfoVisibility( rhs.infoVisibility ),\r\n\t\t\t\t\t\t\t\t\t\toutputFile( rhs.outputFile ),\r\n\t\t\t\t\t\t\t\t\t\terrorCount( rhs.errorCount )\r\n{\r\n}\r\n\r\n\r\n/*\r\n *\t~ M e s s a g e H a n d l i n g\r\n */\r\nMessageHandling::~MessageHandling( )\r\n{\r\n\t#ifdef PC_DEBUG\r\n\tif ( outputFile != 0 )\r\n\t\tfclose( outputFile );\r\n\t#endif\r\n}\r\n\r\n\r\n/*\r\n *\to p e r a t o r =\r\n */\r\nMessageHandling& MessageHandling::operator=( const MessageHandling& rhs )\r\n{\r\n\tif ( this != &rhs )\r\n\t{\r\n\t\terrorVisibility = rhs.errorVisibility;\r\n\t\twarningVisibility = rhs.warningVisibility;\r\n\t\tinfoVisibility = rhs.infoVisibility;\r\n\t\toutputFile = rhs.outputFile;\r\n\t\terrorCount = rhs.errorCount;\r\n\t}\r\n\r\n\treturn *this;\r\n}\r\n\r\n\r\n/*\r\n *\tt h r o w E r r o r\r\n */\r\nreturnValue MessageHandling::throwError(\r\n\treturnValue Enumber,\r\n\tconst char* additionaltext,\r\n\tconst char* functionname,\r\n\tconst char* filename,\r\n\tconst unsigned long linenumber,\r\n\tVisibilityStatus localVisibilityStatus\r\n\t)\r\n{\r\n\t/* consistency check */\r\n\tif ( Enumber <= SUCCESSFUL_RETURN )\r\n\t\treturn throwError( RET_ERROR_UNDEFINED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );\r\n\r\n\t/* Call to common throwMessage function if error shall be displayed. */\r\n\tif ( errorVisibility == VS_VISIBLE )\r\n\t\treturn throwMessage( Enumber,additionaltext,functionname,filename,linenumber,localVisibilityStatus,\"ERROR\" );\r\n\telse\r\n\t\treturn Enumber;\r\n}\r\n\r\n\r\n/*\r\n *\tt h r o w W a r n i n g\r\n */\r\nreturnValue MessageHandling::throwWarning(\r\n\treturnValue Wnumber,\r\n\tconst char* additionaltext,\r\n\tconst char* functionname,\r\n\tconst char* filename,\r\n\tconst unsigned long linenumber,\r\n\tVisibilityStatus localVisibilityStatus\r\n \t)\r\n{\r\n\t/* consistency check */\r\n \tif ( Wnumber <= SUCCESSFUL_RETURN )\r\n\t\treturn throwError( RET_WARNING_UNDEFINED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );\r\n\r\n\t/* Call to common throwMessage function if warning shall be displayed. */\r\n\tif ( warningVisibility == VS_VISIBLE )\r\n\t\treturn throwMessage( Wnumber,additionaltext,functionname,filename,linenumber,localVisibilityStatus,\"WARNING\" );\r\n \telse\r\n \t\treturn Wnumber;\r\n}\r\n\r\n\r\n/*\r\n *\tt h r o w I n f o\r\n */\r\nreturnValue MessageHandling::throwInfo(\r\n \treturnValue Inumber,\r\n\tconst char* additionaltext,\r\n \tconst char* functionname,\r\n\tconst char* filename,\r\n\tconst unsigned long linenumber,\r\n\tVisibilityStatus localVisibilityStatus\r\n \t)\r\n{\r\n\t/* consistency check */\r\n\tif ( Inumber < SUCCESSFUL_RETURN )\r\n\t\treturn throwError( RET_INFO_UNDEFINED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );\r\n\r\n\t/* Call to common throwMessage function if info shall be displayed. */\r\n\tif ( infoVisibility == VS_VISIBLE )\r\n\t\treturn throwMessage( Inumber,additionaltext,functionname,filename,linenumber,localVisibilityStatus,\"INFO\" );\r\n\telse\r\n\t\treturn Inumber;\r\n}\r\n\r\n\r\n/*\r\n *\tr e s e t\r\n */\r\nreturnValue MessageHandling::reset( )\r\n{\r\n\tsetErrorVisibilityStatus( VS_VISIBLE );\r\n\tsetWarningVisibilityStatus( VS_VISIBLE );\r\n\tsetInfoVisibilityStatus( VS_VISIBLE );\r\n\r\n\tsetOutputFile( myStderr );\r\n\tsetErrorCount( 0 );\r\n\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n/*\r\n *\tl i s t A l l M e s s a g e s\r\n */\r\nreturnValue MessageHandling::listAllMessages( )\r\n{\r\n\t#ifdef PC_DEBUG\r\n\tint keypos = 0;\r\n\tchar myPrintfString[160];\r\n\r\n\t/* Run through whole returnValueList and print each item. */\r\n\twhile ( returnValueList[keypos].key != TERMINAL_LIST_ELEMENT )\r\n\t{\r\n\t\tsprintf( myPrintfString,\" %d - %s \\n\",keypos,returnValueList[keypos].data );\r\n\t\tmyPrintf( myPrintfString );\r\n\r\n\t\t++keypos;\r\n\t}\r\n\t#endif\r\n\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n\r\n/*****************************************************************************\r\n * P R O T E C T E D *\r\n *****************************************************************************/\r\n\r\n\r\n#ifdef PC_DEBUG /* Re-define throwMessage function for embedded code! */\r\n\r\n/*\r\n *\tt h r o w M e s s a g e\r\n */\r\nreturnValue MessageHandling::throwMessage(\r\n\treturnValue RETnumber,\r\n\tconst char* additionaltext,\r\n\tconst char* functionname,\r\n\tconst char* filename,\r\n\tconst unsigned long linenumber,\r\n\tVisibilityStatus localVisibilityStatus,\r\n\tconst char* RETstring\r\n \t)\r\n{\r\n\tint i;\r\n\r\n\tint keypos = 0;\r\n\tchar myPrintfString[160];\r\n\r\n\t/* 1) Determine number of whitespace for output. */\r\n\tchar whitespaces[41];\r\n\tint numberOfWhitespaces = (errorCount-1)*2;\r\n\r\n\tif ( numberOfWhitespaces < 0 )\r\n\t\tnumberOfWhitespaces = 0;\r\n\r\n\tif ( numberOfWhitespaces > 40 )\r\n\t\tnumberOfWhitespaces = 40;\r\n\r\n\tfor( i=0; i 0 )\r\n\t\t{\r\n\t\t\tsprintf( myPrintfString,\"%s->\", whitespaces );\r\n\t\t\tmyPrintf( myPrintfString );\r\n\t\t}\r\n\r\n\t\tif ( additionaltext == 0 )\r\n\t\t{\r\n\t\t\tsprintf(\tmyPrintfString,\"%s (%s, %s:%d): \\t%s\\n\",\r\n\t\t\t\t\t\tRETstring,functionname,filename,(int)linenumber,returnValueList[keypos].data\r\n\t\t\t\t\t\t);\r\n\t\t\tmyPrintf( myPrintfString );\r\n\t\t}\r\n\t\telse\r\n\t\t{\r\n\t\t\tsprintf(\tmyPrintfString,\"%s (%s, %s:%d): \\t%s %s\\n\",\r\n\t\t\t\t\t\tRETstring,functionname,filename,(int)linenumber,returnValueList[keypos].data,additionaltext\r\n\t\t\t\t\t\t);\r\n\t\t\tmyPrintf( myPrintfString );\r\n\t\t}\r\n\r\n\t\t/* take care of proper indention for subsequent error messages */\r\n\t\tif ( RETstring[0] == 'E' )\r\n\t\t{\r\n\t\t\t++errorCount;\r\n\t\t}\r\n\t\telse\r\n\t\t{\r\n\t\t\tif ( errorCount > 0 )\r\n\t\t\t\tmyPrintf( \"\\n\" );\r\n\t\t\terrorCount = 0;\r\n\t\t}\r\n\t}\r\n\r\n\treturn RETnumber;\r\n}\r\n\r\n#else /* = PC_DEBUG not defined */\r\n\r\n/*\r\n *\tt h r o w M e s s a g e\r\n */\r\nreturnValue MessageHandling::throwMessage(\r\n\treturnValue RETnumber,\r\n\tconst char* additionaltext,\r\n\tconst char* functionname,\r\n\tconst char* filename,\r\n\tconst unsigned long linenumber,\r\n\tVisibilityStatus localVisibilityStatus,\r\n\tconst char* RETstring\r\n \t)\r\n{\r\n\t/* DUMMY CODE FOR PRETENDING USE OF ARGUMENTS\r\n\t * FOR SUPPRESSING COMPILER WARNINGS! */\r\n\tint i = 0;\r\n\tif ( additionaltext == 0 ) i++;\r\n\tif ( functionname == 0 ) i++;\r\n\tif ( filename == 0 ) i++;\r\n\tif ( linenumber == 0 ) i++;\r\n\tif ( localVisibilityStatus == VS_VISIBLE ) i++;\r\n\tif ( RETstring == 0 ) i++;\r\n\t/* END OF DUMMY CODE */\r\n\r\n\treturn RETnumber;\r\n}\r\n\r\n#endif /* PC_DEBUG */\r\n\r\n\r\n\r\n/*****************************************************************************\r\n * G L O B A L M E S S A G E H A N D L E R *\r\n *****************************************************************************/\r\n\r\n\r\n/** Global message handler for all qpOASES modules.*/\r\nMessageHandling globalMessageHandler( myStderr,VS_VISIBLE,VS_VISIBLE,VS_VISIBLE );\r\n\r\n\r\n/*\r\n *\tg e t G l o b a l M e s s a g e H a n d l e r\r\n */\r\nMessageHandling* getGlobalMessageHandler( )\r\n{\r\n\treturn &globalMessageHandler;\r\n}\r\n\r\nconst char* MessageHandling::getErrorString(int error)\r\n{\r\n\treturn returnValueList[ error ].data;\r\n}\r\n\r\n\r\n\r\n/*\r\n *\tend of file\r\n */\r\n"
},
{
"path": "externals/qpoases/SRC/MessageHandling.ipp",
"content": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.\r\n *\r\n *\tqpOASES is free software; you can redistribute it and/or\r\n *\tmodify it under the terms of the GNU Lesser General Public\r\n *\tLicense as published by the Free Software Foundation; either\r\n *\tversion 2.1 of the License, or (at your option) any later version.\r\n *\r\n *\tqpOASES is distributed in the hope that it will be useful,\r\n *\tbut WITHOUT ANY WARRANTY; without even the implied warranty of\r\n *\tMERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU\r\n *\tLesser General Public License for more details.\r\n *\r\n *\tYou should have received a copy of the GNU Lesser General Public\r\n *\tLicense along with qpOASES; if not, write to the Free Software\r\n *\tFoundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA\r\n *\r\n */\r\n\r\n\r\n/**\r\n *\t\\file SRC/MessageHandling.ipp\r\n *\t\\author Hans Joachim Ferreau\r\n *\t\\version 1.3embedded\r\n *\t\\date 2007-2008\r\n *\r\n *\tImplementation of inlined member functions of the MessageHandling class. \r\n */\r\n\r\n\r\n\r\n/*****************************************************************************\r\n * P U B L I C *\r\n *****************************************************************************/\r\n \r\n/*\r\n *\tg e t E r r o r V i s i b i l i t y S t a t u s\r\n */\r\ninline VisibilityStatus MessageHandling::getErrorVisibilityStatus( ) const\r\n{\r\n \treturn errorVisibility;\r\n}\r\n\r\n\r\n/*\r\n *\tg 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\r\n */\r\ninline VisibilityStatus MessageHandling::getWarningVisibilityStatus( ) const\r\n{\r\n \treturn warningVisibility;\r\n}\r\n\r\n\r\n/*\r\n *\tg e t I n f o V i s i b i l i t y S t a t u s\r\n */\r\ninline VisibilityStatus MessageHandling::getInfoVisibilityStatus( ) const\r\n{\r\n \treturn infoVisibility;\r\n}\r\n\r\n\r\n/*\r\n *\tg e t O u t p u t F i l e\r\n */\r\ninline myFILE* MessageHandling::getOutputFile( ) const\r\n{\r\n \treturn outputFile;\r\n}\r\n\r\n\r\n/*\r\n *\tg e t E r r o r C o u n t\r\n */\r\ninline int MessageHandling::getErrorCount( ) const\r\n{\r\n \treturn errorCount;\r\n}\r\n\r\n\r\n/*\r\n *\ts e t E r r o r V i s i b i l i t y S t a t u s\r\n */\r\ninline void MessageHandling::setErrorVisibilityStatus( VisibilityStatus _errorVisibility ) \r\n{\r\n \terrorVisibility = _errorVisibility;\r\n}\r\n\r\n\r\n/*\r\n *\ts 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\r\n */\r\ninline void MessageHandling::setWarningVisibilityStatus( VisibilityStatus _warningVisibility ) \r\n{\r\n \twarningVisibility = _warningVisibility;\r\n}\r\n\r\n\r\n/*\r\n *\ts e t I n f o V i s i b i l i t y S t a t u s\r\n */\r\ninline void MessageHandling::setInfoVisibilityStatus( VisibilityStatus _infoVisibility ) \r\n{\r\n \tinfoVisibility = _infoVisibility;\r\n}\r\n\r\n\r\n/*\r\n *\ts e t O u t p u t F i l e\r\n */\r\ninline void MessageHandling::setOutputFile( myFILE* _outputFile ) \r\n{\r\n \toutputFile = _outputFile;\r\n}\r\n\r\n\r\n/*\r\n *\ts e t E r r o r C o u n t\r\n */\r\ninline returnValue MessageHandling::setErrorCount( int _errorCount )\r\n{\r\n\tif ( _errorCount >= 0 ) \t\r\n\t{\r\n\t\terrorCount = _errorCount;\r\n\t\treturn SUCCESSFUL_RETURN;\r\n\t}\r\n\telse\r\n\t\treturn RET_INVALID_ARGUMENTS;\r\n}\r\n\r\n\r\n/*\r\n *\tend of file\r\n */\r\n"
},
{
"path": "externals/qpoases/SRC/QProblem.cpp",
"content": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.\r\n *\r\n *\tqpOASES is free software; you can redistribute it and/or\r\n *\tmodify it under the terms of the GNU Lesser General Public\r\n *\tLicense as published by the Free Software Foundation; either\r\n *\tversion 2.1 of the License, or (at your option) any later version.\r\n *\r\n *\tqpOASES is distributed in the hope that it will be useful,\r\n *\tbut WITHOUT ANY WARRANTY; without even the implied warranty of\r\n *\tMERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU\r\n *\tLesser General Public License for more details.\r\n *\r\n *\tYou should have received a copy of the GNU Lesser General Public\r\n *\tLicense along with qpOASES; if not, write to the Free Software\r\n *\tFoundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA\r\n *\r\n */\r\n\r\n\r\n/**\r\n *\t\\file SRC/QProblem.cpp\r\n *\t\\author Hans Joachim Ferreau\r\n *\t\\version 1.3embedded\r\n *\t\\date 2007-2008\r\n *\r\n *\tImplementation of the QProblem class which is able to use the newly\r\n *\tdeveloped online active set strategy for parametric quadratic programming.\r\n */\r\n\r\n\r\n#include \r\n\r\n#include \r\n\r\nvoid printmatrix2(char *name, double *A, int m, int n) {\r\n int i, j;\r\n\r\n printf(\"%s = [...\\n\", name);\r\n for (i = 0; i < m; i++) {\r\n for (j = 0; j < n; j++)\r\n printf(\" % 9.4f\", A[i*n+j]);\r\n printf(\",\\n\");\r\n }\r\n printf(\"];\\n\");\r\n}\r\n\r\n//#define __PERFORM_KKT_TEST__\r\n\r\n\r\n/*****************************************************************************\r\n * P U B L I C *\r\n *****************************************************************************/\r\n\r\n\r\n/*\r\n *\tQ P r o b l e m\r\n */\r\nQProblem::QProblem( ) : QProblemB( )\r\n{\r\n\tconstraints.init( 0 );\r\n\r\n\tsizeT = 0;\r\n\r\n\tcyclingManager.init( 0,0 );\r\n}\r\n\r\n\r\n/*\r\n *\tQ P r o b l e m\r\n */\r\nQProblem::QProblem( int _nV, int _nC ) : QProblemB( _nV )\r\n{\r\n\t/* consistency checks */\r\n\tif ( _nV <= 0 )\r\n\t\t_nV = 1;\r\n\r\n\tif ( _nC < 0 )\r\n\t{\r\n\t\t_nC = 0;\r\n\t\tTHROWERROR( RET_INVALID_ARGUMENTS );\r\n\t}\r\n\r\n\tconstraints.init( _nC );\r\n\r\n\r\n\tsizeT = _nC;\r\n\tif ( _nC > _nV )\r\n\t\tsizeT = _nV;\r\n\r\n\tcyclingManager.init( _nV,_nC );\r\n}\r\n\r\n\r\n/*\r\n *\tQ P r o b l e m\r\n */\r\nQProblem::QProblem( const QProblem& rhs ) :\tQProblemB( rhs )\r\n{\r\n\tint i, j;\r\n\r\n\tint _nV = rhs.bounds.getNV( );\r\n\tint _nC = rhs.constraints.getNC( );\r\n\r\n\tfor( i=0; i<_nC; ++i )\r\n\t\tfor( j=0; j<_nV; ++j )\r\n\t\t\tA[i*NVMAX + j] = rhs.A[i*NVMAX + j];\r\n\r\n\tfor( i=0; i<_nC; ++i )\r\n\t\tlbA[i] = rhs.lbA[i];\r\n\r\n\tfor( i=0; i<_nC; ++i )\r\n\t\t\tubA[i] = rhs.ubA[i];\r\n\r\n\tconstraints = rhs.constraints;\r\n\r\n\tfor( i=0; i<(_nV+_nC); ++i )\r\n\t\ty[i] = rhs.y[i];\r\n\r\n\r\n\tsizeT = rhs.sizeT;\r\n\r\n\tfor( i=0; ithrowInfo( RET_ITERATION_STARTED,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );\r\n\t\t\t#endif\r\n\t\t}\r\n\r\n\t\t/* 1) Setup index arrays. */\r\n\t\tif ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )\r\n\t\t\treturn THROWERROR( RET_HOTSTART_FAILED );\r\n\r\n\t\tif ( bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN )\r\n\t\t\treturn THROWERROR( RET_HOTSTART_FAILED );\r\n\r\n\t\tif ( constraints.getActive( )->getNumberArray( AC_idx ) != SUCCESSFUL_RETURN )\r\n\t\t\treturn THROWERROR( RET_HOTSTART_FAILED );\r\n\r\n\t\tif ( constraints.getInactive( )->getNumberArray( IAC_idx ) != SUCCESSFUL_RETURN )\r\n\t\t\treturn THROWERROR( RET_HOTSTART_FAILED );\r\n\r\n\t\t/* 2) Detemination of shift direction of the gradient and the (constraints') bounds. */\r\n\t\treturnvalue = hotstart_determineDataShift( FX_idx, AC_idx,\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tg_new,lbA_new,ubA_new,lb_new,ub_new,\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tdelta_g,delta_lbA,delta_ubA,delta_lb,delta_ub,\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tDelta_bC_isZero, Delta_bB_isZero );\r\n\t\tif ( returnvalue != SUCCESSFUL_RETURN )\r\n\t\t{\r\n\t\t\tnWSR = l;\r\n\t\t\tTHROWERROR( RET_SHIFT_DETERMINATION_FAILED );\r\n\t\t\treturn returnvalue;\r\n\t\t}\r\n\r\n\t\t/* 3) Determination of step direction of X and Y. */\r\n\t\treturnvalue = hotstart_determineStepDirection(\tFR_idx,FX_idx,AC_idx,\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\tdelta_g,delta_lbA,delta_ubA,delta_lb,delta_ub,\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\tDelta_bC_isZero, Delta_bB_isZero,\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\tdelta_xFX,delta_xFR,delta_yAC,delta_yFX\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t);\r\n\t\tif ( returnvalue != SUCCESSFUL_RETURN )\r\n\t\t{\r\n\t\t\tnWSR = l;\r\n\t\t\tTHROWERROR( RET_STEPDIRECTION_DETERMINATION_FAILED );\r\n\t\t\treturn returnvalue;\r\n\t\t}\r\n\r\n\t\t/* 4) Determination of step length TAU. */\r\n\t\treturnvalue = hotstart_determineStepLength(\tFR_idx,FX_idx,AC_idx,IAC_idx,\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tdelta_lbA,delta_ubA,delta_lb,delta_ub,\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tdelta_xFX,delta_xFR,delta_yAC,delta_yFX,delta_Ax,\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tBC_idx,BC_status,BC_isBound\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t);\r\n\t\tif ( returnvalue != SUCCESSFUL_RETURN )\r\n\t\t{\r\n\t\t\tnWSR = l;\r\n\t\t\tTHROWERROR( RET_STEPLENGTH_DETERMINATION_FAILED );\r\n\t\t\treturn returnvalue;\r\n\t\t}\r\n\r\n\t\t/* 5) Realisation of the homotopy step. */\r\n\t\treturnvalue = hotstart_performStep(\tFR_idx,FX_idx,AC_idx,IAC_idx,\r\n\t\t\t\t\t\t\t\t\t\t\tdelta_g,delta_lbA,delta_ubA,delta_lb,delta_ub,\r\n\t\t\t\t\t\t\t\t\t\t\tdelta_xFX,delta_xFR,delta_yAC,delta_yFX,delta_Ax,\r\n\t\t\t\t\t\t\t\t\t\t\tBC_idx,BC_status,BC_isBound\r\n\t\t\t\t\t\t\t\t\t\t\t);\r\n\r\n\t\tif ( returnvalue != SUCCESSFUL_RETURN )\r\n\t\t{\r\n\t\t\tnWSR = l;\r\n\r\n\t\t\t/* stop runtime measurement */\r\n\t\t\tif ( cputime != 0 )\r\n\t\t\t\t\t*cputime = getCPUtime( ) - starttime;\r\n\r\n\t\t\t/* optimal solution found? */\r\n\t\t\tif ( returnvalue == RET_OPTIMAL_SOLUTION_FOUND )\r\n\t\t\t{\r\n\t\t\t\tstatus = QPS_SOLVED;\r\n\r\n\t\t\t\tif ( printlevel == PL_HIGH )\r\n\t\t\t\t\tTHROWINFO( RET_OPTIMAL_SOLUTION_FOUND );\r\n\r\n\t\t\t\t#ifdef PC_DEBUG\r\n\t \t\t\tif ( printIteration( l,BC_idx,BC_status,BC_isBound ) != SUCCESSFUL_RETURN )\r\n\t\t\t\t\tTHROWERROR( RET_PRINT_ITERATION_FAILED ); /* do not pass this as return value! */\r\n\t\t\t\t#endif\r\n\r\n\t\t\t\t/* check KKT optimality conditions */\r\n\t\t\t\treturn checkKKTconditions( );\r\n\t\t\t}\r\n\t\t\telse\r\n\t\t\t{\r\n\t\t\t\t/* checks for infeasibility... */\r\n\t\t\t\tif ( isInfeasible( ) == BT_TRUE )\r\n\t\t\t\t{\r\n\t\t\t\t\tstatus = QPS_HOMOTOPYQPSOLVED;\r\n\t\t\t\t\treturn THROWERROR( RET_HOTSTART_STOPPED_INFEASIBILITY );\r\n\t\t\t\t}\r\n\r\n\t\t\t\t/* ...unboundedness... */\r\n\t\t\t\tif ( unbounded == BT_TRUE ) /* not necessary since objective function convex! */\r\n\t\t\t\t\treturn THROWERROR( RET_HOTSTART_STOPPED_UNBOUNDEDNESS );\r\n\r\n\t\t\t\t/* ... and throw unspecific error otherwise */\r\n\t\t\t\tTHROWERROR( RET_HOMOTOPY_STEP_FAILED );\r\n\t\t\t\treturn returnvalue;\r\n\t\t\t}\r\n\t\t}\r\n\r\n\t\t/* 6) Output information of successful QP iteration. */\r\n\t\tstatus = QPS_HOMOTOPYQPSOLVED;\r\n\r\n\t\t#ifdef PC_DEBUG\r\n\t\tif ( printIteration( l,BC_idx,BC_status,BC_isBound ) != SUCCESSFUL_RETURN )\r\n\t\t\tTHROWERROR( RET_PRINT_ITERATION_FAILED ); /* do not pass this as return value! */\r\n\t\t#endif\r\n\t}\r\n\r\n\r\n\t/* stop runtime measurement */\r\n\tif ( cputime != 0 )\r\n\t\t*cputime = getCPUtime( ) - starttime;\r\n\r\n\r\n\t/* if programm gets to here, output information that QP could not be solved\r\n\t * within the given maximum numbers of working set changes */\r\n\tif ( printlevel == PL_HIGH )\r\n\t{\r\n\t\t#ifdef PC_DEBUG\r\n\t\tsprintf( messageString,\"(nWSR = %d)\",nWSR );\r\n\t\treturn getGlobalMessageHandler( )->throwWarning( RET_MAX_NWSR_REACHED,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );\r\n\t\t#endif\r\n\t}\r\n\r\n\t/* Finally check KKT optimality conditions. */\r\n\treturnValue returnvalueKKTcheck = checkKKTconditions( );\r\n\r\n\tif ( returnvalueKKTcheck != SUCCESSFUL_RETURN )\r\n\t\treturn returnvalueKKTcheck;\r\n\telse\r\n\t\treturn RET_MAX_NWSR_REACHED;\r\n}\r\n\r\n\r\n/*\r\n *\tg e t N Z\r\n */\r\nint QProblem::getNZ( )\r\n{\r\n\t/* nZ = nFR - nAC */\r\n\treturn bounds.getFree( )->getLength( ) - constraints.getActive( )->getLength( );\r\n}\r\n\r\n\r\n/*\r\n *\tg e t D u a l S o l u t i o n\r\n */\r\nreturnValue QProblem::getDualSolution( real_t* const yOpt ) const\r\n{\r\n\tint i;\r\n\r\n\t/* return optimal dual solution vector\r\n\t * only if current QP has been solved */\r\n\tif ( ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||\r\n\t\t ( getStatus( ) == QPS_HOMOTOPYQPSOLVED ) ||\r\n\t\t ( getStatus( ) == QPS_SOLVED ) )\r\n\t{\r\n\t\tfor( i=0; i -INFTY )\r\n\t\t{\r\n\t\t\tconstraints.setNoLower( BT_FALSE );\r\n\t\t\tbreak;\r\n\t\t}\r\n\t}\r\n\r\n\t/* 2) Check if upper constraints' bounds are present. */\r\n\tconstraints.setNoUpper( BT_TRUE );\r\n\tfor( i=0; i INFTY - BOUNDTOL ) )\r\n\t\t{\r\n\t\t\tconstraints.setType( i,ST_UNBOUNDED );\r\n\t\t\t++nUC;\r\n\t\t}\r\n\t\telse\r\n\t\t{\r\n\t\t\tif ( lbA[i] > ubA[i] - BOUNDTOL )\r\n\t\t\t{\r\n\t\t\t\tconstraints.setType( i,ST_EQUALITY );\r\n\t\t\t\t++nEC;\r\n\t\t\t}\r\n\t\t\telse\r\n\t\t\t{\r\n\t\t\t\tconstraints.setType( i,ST_BOUNDED );\r\n\t\t\t}\r\n\t\t}\r\n\t}\r\n\r\n\t/* 4) Set dimensions of constraints structure. */\r\n\tconstraints.setNEC( nEC );\r\n\tconstraints.setNUC( nUC );\r\n\tconstraints.setNIC( nC - nEC - nUC );\r\n\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n/*\r\n *\tc h o l e s k y D e c o m p o s i t i o n P r o j e c t e d\r\n */\r\nreturnValue QProblem::setupCholeskyDecompositionProjected( )\r\n{\r\n\tint i, j, k, ii, kk;\r\n\tint nV = getNV( );\r\n\tint nFR = getNFR( );\r\n\tint nZ = getNZ( );\r\n\r\n\t/* 1) Initialises R with all zeros. */\r\n\tfor( i=0; i 0 )\r\n\t\t{\r\n\t\t\tint FR_idx[NVMAX];\r\n\t\t\tif ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )\r\n\t\t\t\treturn THROWERROR( RET_INDEXLIST_CORRUPTED );\r\n\r\n#if 0\r\n\t\t\treal_t HZ[NVMAX*NVMAX];\r\n\t\t\treal_t ZHZ[NVMAX*NVMAX];\r\n\r\n\t\t\t/* calculate H*Z */\r\n\t\t\tfor ( i=0; i=0; --k )\r\n\t\t\t\t\tsum -= R[k*NVMAX + i] * R[k*NVMAX + i];\r\n\r\n\t\t\t\tif ( sum > 0.0 )\r\n\t\t\t\t{\r\n\t\t\t\t\tR[i*NVMAX + i] = sqrt( sum );\r\n\t\t\t\t\tinv = 1.0 / R[i * NVMAX + i];\r\n\t\t\t\t}\r\n\t\t\t\telse\r\n\t\t\t\t{\r\n\t\t\t\t\thessianType = HST_SEMIDEF;\r\n\t\t\t\t\treturn THROWERROR( RET_HESSIAN_NOT_SPD );\r\n\t\t\t\t}\r\n\r\n\t\t\t\tfor( j=(i+1); j=0; --k )\r\n\t\t\t\t\t\tsum -= R[k*NVMAX + i] * R[k*NVMAX + j];\r\n\r\n\t\t\t\t\tR[i*NVMAX + j] = sum * inv;\r\n\t\t\t\t}\r\n\t\t\t}\r\n#else\r\n\t\t\treal_t HZ[NVMAX];\r\n\t\t\treal_t ZHZ[NVMAX];\r\n\r\n\t\t\treal_t sum, inv;\r\n\t\t\tfor (j = 0; j < nZ; ++j)\r\n\t\t\t{\r\n\t\t\t\t/* Cache one column of Z. */\r\n\t\t\t\tfor (i = 0; i < NVMAX; ++i)\r\n\t\t\t\t\tZHZ[i] = Q[i * NVMAX + j];\r\n\r\n\t\t\t\t/* Create one column of the product H * Z. */\r\n\t\t\t\tfor (i = 0; i < nFR; ++i)\r\n\t\t\t\t{\r\n\t\t\t\t\tii = FR_idx[i];\r\n\r\n\t\t\t\t\tsum = 0.0;\r\n\t\t\t\t\tfor (k = 0; k < nFR; ++k)\r\n\t\t\t\t\t{\r\n\t\t\t\t\t\tkk = FR_idx[k];\r\n\t\t\t\t\t\tsum += H[ii * NVMAX + kk] * ZHZ[kk];\r\n\t\t\t\t\t}\r\n\t\t\t\t\tHZ[ii] = sum;\r\n\t\t\t\t}\r\n\r\n\t\t\t\t/* Create one column of the product Z^T * H * Z. */\r\n\t\t\t\tfor (i = j; i < nZ; ++i)\r\n\t\t\t\t\tZHZ[ i ] = 0.0;\r\n\r\n\t\t\t\tfor (k = 0; k < nFR; ++k)\r\n\t\t\t\t{\r\n\t\t\t\t\tkk = FR_idx[k];\r\n\t\t\t\t\treal_t q = HZ[kk];\r\n\t\t\t\t\tfor (i = j; i < nZ; ++i)\r\n\t\t\t\t\t{\r\n\t\t\t\t\t\tZHZ[i] += Q[kk * NVMAX + i] * q;\r\n\t\t\t\t\t}\r\n\t\t\t\t}\r\n\r\n\t\t\t\t/* Use the computed column to update the factorization. */\r\n\t\t\t\t/* j == i */\r\n\t\t\t\tsum = ZHZ[j];\r\n\r\n\t\t\t\tfor (k = (j - 1); k >= 0; --k)\r\n\t\t\t\t\tsum -= R[k * NVMAX + j] * R[k * NVMAX + j];\r\n\r\n\t\t\t\tif (sum > 0.0)\r\n\t\t\t\t{\r\n\t\t\t\t\tR[j * NVMAX + j] = sqrt(sum);\r\n\t\t\t\t\tinv = 1.0 / R[j * NVMAX + j];\r\n\t\t\t\t}\r\n\t\t\t\telse\r\n\t\t\t\t{\r\n\t\t\t\t\thessianType = HST_SEMIDEF;\r\n\t\t\t\t\treturn THROWERROR( RET_HESSIAN_NOT_SPD );\r\n\t\t\t\t}\r\n\r\n\t\t\t\tfor (i = (j + 1); i < nZ; ++i)\r\n\t\t\t\t{\r\n\t\t\t\t\tsum = ZHZ[i];\r\n\r\n\t\t\t\t\tfor (k = (j - 1); k >= 0; --k)\r\n\t\t\t\t\t\tsum -= R[k * NVMAX + j] * R[k * NVMAX + i];\r\n\r\n\t\t\t\t\tR[j * NVMAX + i] = sum * inv;\r\n\t\t\t\t}\r\n\t\t\t}\r\n#endif\r\n\t\t}\r\n\t}\r\n\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n/*\r\n *\ts e t u p T Q f a c t o r i s a t i o n\r\n */\r\nreturnValue QProblem::setupTQfactorisation( )\r\n{\r\n\tint i, j, ii;\r\n\tint nV = getNV( );\r\n\tint nFR = getNFR( );\r\n\r\n\tint FR_idx[NVMAX];\r\n\tif ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )\r\n\t\treturn THROWERROR( RET_INDEXLIST_CORRUPTED );\r\n\r\n\t/* 1) Set Q to unity matrix. */\r\n\tfor( i=0; igetStatus( i );\r\n\r\n\t\t\tif ( constraints.getType( i ) == ST_EQUALITY )\r\n\t\t\t{\r\n\t\t\t\tif ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN )\r\n\t\t\t\t\treturn THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );\r\n\t\t\t}\r\n\t\t\telse\r\n\t\t\t{\r\n\t\t\t\tif ( auxiliaryConstraints->setupConstraint( i,guessedStatus ) != SUCCESSFUL_RETURN )\r\n\t\t\t\t\treturn THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );\r\n\t\t\t}\r\n\t\t}\r\n\t}\r\n\telse\t/* No initial working set specified. */\r\n\t{\r\n\t\t/* Obtain initial working set by \"clipping\". */\r\n\t\tif ( ( xOpt != 0 ) && ( yOpt == 0 ) )\r\n\t\t{\r\n\t\t\tfor( i=0; isetupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN )\r\n\t\t\t\t\t\treturn THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );\r\n\t\t\t\t\tcontinue;\r\n\t\t\t\t}\r\n\r\n\t\t\t\tif ( Ax[i] >= ubA[i] - BOUNDTOL )\r\n\t\t\t\t{\r\n\t\t\t\t\tif ( auxiliaryConstraints->setupConstraint( i,ST_UPPER ) != SUCCESSFUL_RETURN )\r\n\t\t\t\t\t\t\treturn THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );\r\n\t\t\t\t\tcontinue;\r\n\t\t\t\t}\r\n\r\n\t\t\t\t/* Moreover, add all equality constraints if specified. */\r\n\t\t\t\tif ( constraints.getType( i ) == ST_EQUALITY )\r\n\t\t\t\t{\r\n\t\t\t\t\tif ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN )\r\n\t\t\t\t\t\treturn THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );\r\n\t\t\t\t}\r\n\t\t\t\telse\r\n\t\t\t\t{\r\n\t\t\t\t\tif ( auxiliaryConstraints->setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )\r\n\t\t\t\t\t\treturn THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );\r\n\t\t\t\t}\r\n\t\t\t}\r\n\t\t}\r\n\r\n\t\t/* Obtain initial working set in accordance to sign of dual solution vector. */\r\n\t\tif ( ( xOpt == 0 ) && ( yOpt != 0 ) )\r\n\t\t{\r\n\t\t\tfor( i=0; i ZERO )\r\n\t\t\t\t{\r\n\t\t\t\t\tif ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN )\r\n\t\t\t\t\t\treturn THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );\r\n\t\t\t\t\tcontinue;\r\n\t\t\t\t}\r\n\r\n\t\t\t\tif ( yOpt[nV+i] < -ZERO )\r\n\t\t\t\t{\r\n\t\t\t\t\tif ( auxiliaryConstraints->setupConstraint( i,ST_UPPER ) != SUCCESSFUL_RETURN )\r\n\t\t\t\t\t\treturn THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );\r\n\t\t\t\t\tcontinue;\r\n\t\t\t\t}\r\n\r\n\t\t\t\t/* Moreover, add all equality constraints if specified. */\r\n\t\t\t\tif ( constraints.getType( i ) == ST_EQUALITY )\r\n\t\t\t\t{\r\n\t\t\t\t\tif ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN )\r\n\t\t\t\t\t\treturn THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );\r\n\t\t\t\t}\r\n\t\t\t\telse\r\n\t\t\t\t{\r\n\t\t\t\t\tif ( auxiliaryConstraints->setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )\r\n\t\t\t\t\t\treturn THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );\r\n\t\t\t\t}\r\n\t\t\t}\r\n\t\t}\r\n\r\n\t\t/* If xOpt and yOpt are null pointer and no initial working is specified,\r\n\t\t * start with empty working set (or implicitly fixed bounds and equality constraints only)\r\n\t\t * for auxiliary QP. */\r\n\t\tif ( ( xOpt == 0 ) && ( yOpt == 0 ) )\r\n\t\t{\r\n\t\t\tfor( i=0; isetupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN )\r\n\t\t\t\t\t\treturn THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );\r\n\t\t\t\t}\r\n\t\t\t\telse\r\n\t\t\t\t{\r\n\t\t\t\t\tif ( auxiliaryConstraints->setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )\r\n\t\t\t\t\t\treturn THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );\r\n\t\t\t\t}\r\n\t\t\t}\r\n\t\t}\r\n\t}\r\n\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n\r\n/*\r\n *\ts e t u p A u x i l i a r y W o r k i n g S e t\r\n */\r\nreturnValue QProblem::setupAuxiliaryWorkingSet(\tconst Bounds* const auxiliaryBounds,\r\n\t\t\t\t\t\t\t\t\t\t\t\tconst Constraints* const auxiliaryConstraints,\r\n\t\t\t\t\t\t\t\t\t\t\t\tBooleanType setupAfresh\r\n\t\t\t\t\t\t\t\t\t\t\t\t)\r\n{\r\n\tint i;\r\n\tint nV = getNV( );\r\n\tint nC = getNC( );\r\n\r\n\t/* consistency checks */\r\n\tif ( auxiliaryBounds != 0 )\r\n\t{\r\n\t\tfor( i=0; igetStatus( i ) == ST_UNDEFINED ) )\r\n\t\t\t\treturn THROWERROR( RET_UNKNOWN_BUG );\r\n\t}\r\n\telse\r\n\t{\r\n\t\treturn THROWERROR( RET_INVALID_ARGUMENTS );\r\n\t}\r\n\r\n\tif ( auxiliaryConstraints != 0 )\r\n\t{\r\n\t\tfor( i=0; igetStatus( i ) == ST_UNDEFINED ) )\r\n\t\t\t\treturn THROWERROR( RET_UNKNOWN_BUG );\r\n\t}\r\n\telse\r\n\t{\r\n\t\treturn THROWERROR( RET_INVALID_ARGUMENTS );\r\n\t}\r\n\r\n\r\n\t/* I) SETUP CHOLESKY FLAG:\r\n\t * Cholesky decomposition shall only be updated if working set\r\n\t * shall be updated (i.e. NOT setup afresh!) */\r\n\tBooleanType updateCholesky;\r\n\tif ( setupAfresh == BT_TRUE )\r\n\t\tupdateCholesky = BT_FALSE;\r\n\telse\r\n\t\tupdateCholesky = BT_TRUE;\r\n\r\n\r\n\t/* II) REMOVE FORMERLY ACTIVE (CONSTRAINTS') BOUNDS (IF NECESSARY): */\r\n\tif ( setupAfresh == BT_FALSE )\r\n\t{\r\n\t\t/* 1) Remove all active constraints that shall be inactive AND\r\n\t\t* all active constraints that are active at the wrong bound. */\r\n\t\tfor( i=0; igetStatus( i ) != ST_LOWER ) )\r\n\t\t\t\tif ( removeConstraint( i,updateCholesky ) != SUCCESSFUL_RETURN )\r\n\t\t\t\t\treturn THROWERROR( RET_SETUP_WORKINGSET_FAILED );\r\n\r\n\t\t\tif ( ( constraints.getStatus( i ) == ST_UPPER ) && ( auxiliaryConstraints->getStatus( i ) != ST_UPPER ) )\r\n\t\t\t\tif ( removeConstraint( i,updateCholesky ) != SUCCESSFUL_RETURN )\r\n\t\t\t\t\treturn THROWERROR( RET_SETUP_WORKINGSET_FAILED );\r\n\t\t}\r\n\r\n\t\t/* 2) Remove all active bounds that shall be inactive AND\r\n\t\t* all active bounds that are active at the wrong bound. */\r\n\t\tfor( i=0; igetStatus( i ) != ST_LOWER ) )\r\n\t\t\t\tif ( removeBound( i,updateCholesky ) != SUCCESSFUL_RETURN )\r\n\t\t\t\t\treturn THROWERROR( RET_SETUP_WORKINGSET_FAILED );\r\n\r\n\t\t\tif ( ( bounds.getStatus( i ) == ST_UPPER ) && ( auxiliaryBounds->getStatus( i ) != ST_UPPER ) )\r\n\t\t\t\tif ( removeBound( i,updateCholesky ) != SUCCESSFUL_RETURN )\r\n\t\t\t\t\treturn THROWERROR( RET_SETUP_WORKINGSET_FAILED );\r\n\t\t}\r\n\t}\r\n\r\n\r\n\t/* III) ADD NEWLY ACTIVE (CONSTRAINTS') BOUNDS: */\r\n\t/* 1) Add all inactive bounds that shall be active AND\r\n\t * all formerly active bounds that have been active at the wrong bound. */\r\n\tfor( i=0; igetStatus( i ) != ST_INACTIVE ) )\r\n\t\t{\r\n\t\t\t/* Add bound only if it is linearly independent from the current working set. */\r\n\t\t\tif ( addBound_checkLI( i ) == RET_LINEARLY_INDEPENDENT )\r\n\t\t\t{\r\n\t\t\t\tif ( addBound( i,auxiliaryBounds->getStatus( i ),updateCholesky ) != SUCCESSFUL_RETURN )\r\n\t\t\t\t\treturn THROWERROR( RET_SETUP_WORKINGSET_FAILED );\r\n\t\t\t}\r\n\t\t}\r\n\t}\r\n\r\n\t/* 2) Add all inactive constraints that shall be active AND\r\n\t * all formerly active constraints that have been active at the wrong bound. */\r\n\tfor( i=0; igetStatus( i ) == ST_LOWER ) || ( auxiliaryConstraints->getStatus( i ) == ST_UPPER ) )\r\n\t\t{\r\n\t\t\t/* formerly inactive */\r\n\t\t\tif ( constraints.getStatus( i ) == ST_INACTIVE )\r\n\t\t\t{\r\n\t\t\t\t/* Add constraint only if it is linearly independent from the current working set. */\r\n\t\t\t\tif ( addConstraint_checkLI( i ) == RET_LINEARLY_INDEPENDENT )\r\n\t\t\t\t{\r\n\t\t\t\t\tif ( addConstraint( i,auxiliaryConstraints->getStatus( i ),updateCholesky ) != SUCCESSFUL_RETURN )\r\n\t\t\t\t\t\treturn THROWERROR( RET_SETUP_WORKINGSET_FAILED );\r\n\t\t\t\t}\r\n\t\t\t}\r\n\t\t}\r\n\t}\r\n\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n/*\r\n *\ts e t u p A u x i l i a r y Q P s o l u t i o n\r\n */\r\nreturnValue QProblem::setupAuxiliaryQPsolution(\tconst real_t* const xOpt, const real_t* const yOpt\r\n\t\t\t\t\t\t\t\t\t\t\t\t)\r\n{\r\n\tint i, j;\r\n\tint nV = getNV( );\r\n\tint nC = getNC( );\r\n\r\n\r\n\t/* Setup primal/dual solution vector for auxiliary initial QP:\r\n\t * if a null pointer is passed, a zero vector is assigned;\r\n\t * old solution vector is kept if pointer to internal solution vevtor is passed. */\r\n\tif ( xOpt != 0 )\r\n\t{\r\n\t\tif ( xOpt != x )\r\n\t\t\tfor( i=0; igetStatus( i ) == ST_LOWER )\r\n\t\t\t\t\t\t\tlb[i] = x[i];\r\n\t\t\t\t\t\telse\r\n\t\t\t\t\t\t\tlb[i] = x[i] - BOUNDRELAXATION;\r\n\r\n\t\t\t\t\t\tif ( auxiliaryBounds->getStatus( i ) == ST_UPPER )\r\n\t\t\t\t\t\t\tub[i] = x[i];\r\n\t\t\t\t\t\telse\r\n\t\t\t\t\t\t\tub[i] = x[i] + BOUNDRELAXATION;\r\n\t\t\t\t\t}\r\n\t\t\t\t}\r\n\t\t\t\tbreak;\r\n\r\n\t\t\tcase ST_LOWER:\r\n\t\t\t\tlb[i] = x[i];\r\n\t\t\t\tif ( bounds.getType( i ) == ST_EQUALITY )\r\n\t\t\t\t{\r\n\t\t\t\t\tub[i] = x[i];\r\n\t\t\t\t}\r\n\t\t\t\telse\r\n\t\t\t\t{\r\n\t\t\t\t\tif ( useRelaxation == BT_TRUE )\r\n\t\t\t\t\t\tub[i] = x[i] + BOUNDRELAXATION;\r\n\t\t\t\t}\r\n\t\t\t\tbreak;\r\n\r\n\t\t\tcase ST_UPPER:\r\n\t\t\t\tub[i] = x[i];\r\n\t\t\t\tif ( bounds.getType( i ) == ST_EQUALITY )\r\n\t\t\t\t{\r\n\t\t\t\t\tlb[i] = x[i];\r\n\t\t\t\t}\r\n\t\t\t\telse\r\n\t\t\t\t{\r\n\t\t\t\t\tif ( useRelaxation == BT_TRUE )\r\n\t\t\t\t\t\tlb[i] = x[i] - BOUNDRELAXATION;\r\n\t\t\t\t}\r\n\t\t\t\tbreak;\r\n\r\n\t\t\tdefault:\r\n\t\t\t\treturn THROWERROR( RET_UNKNOWN_BUG );\r\n\t\t}\r\n\t}\r\n\r\n\t/* 2) Setup constraints vectors. */\r\n\tfor ( i=0; igetStatus( i ) == ST_LOWER )\r\n\t\t\t\t\t\t\tlbA[i] = Ax[i];\r\n\t\t\t\t\t\telse\r\n\t\t\t\t\t\t\tlbA[i] = Ax[i] - BOUNDRELAXATION;\r\n\r\n\t\t\t\t\t\tif ( auxiliaryConstraints->getStatus( i ) == ST_UPPER )\r\n\t\t\t\t\t\t\tubA[i] = Ax[i];\r\n\t\t\t\t\t\telse\r\n\t\t\t\t\t\t\tubA[i] = Ax[i] + BOUNDRELAXATION;\r\n\t\t\t\t\t}\r\n\t\t\t\t}\r\n\t\t\t\tbreak;\r\n\r\n\t\t\tcase ST_LOWER:\r\n\t\t\t\tlbA[i] = Ax[i];\r\n\t\t\t\tif ( constraints.getType( i ) == ST_EQUALITY )\r\n\t\t\t\t{\r\n\t\t\t\t\tubA[i] = Ax[i];\r\n\t\t\t\t}\r\n\t\t\t\telse\r\n\t\t\t\t{\r\n\t\t\t\t\tif ( useRelaxation == BT_TRUE )\r\n\t\t\t\t\t\tubA[i] = Ax[i] + BOUNDRELAXATION;\r\n\t\t\t\t}\r\n\t\t\t\tbreak;\r\n\r\n\t\t\tcase ST_UPPER:\r\n\t\t\t\tubA[i] = Ax[i];\r\n\t\t\t\tif ( constraints.getType( i ) == ST_EQUALITY )\r\n\t\t\t\t{\r\n\t\t\t\t\tlbA[i] = Ax[i];\r\n\t\t\t\t}\r\n\t\t\t\telse\r\n\t\t\t\t{\r\n\t\t\t\t\tif ( useRelaxation == BT_TRUE )\r\n\t\t\t\t\t\tlbA[i] = Ax[i] - BOUNDRELAXATION;\r\n\t\t\t\t}\r\n\t\t\t\tbreak;\r\n\r\n\t\t\tdefault:\r\n\t\t\t\treturn THROWERROR( RET_UNKNOWN_BUG );\r\n\t\t}\r\n\t}\r\n\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n/*\r\n *\ta d d C o n s t r a i n t\r\n */\r\nreturnValue QProblem::addConstraint(\tint number, SubjectToStatus C_status,\r\n\t\t\t\t\t\t\t\t\t\tBooleanType updateCholesky\r\n\t\t\t\t\t\t\t\t\t\t)\r\n{\r\n\tint i, j, ii;\r\n\r\n\t/* consistency checks */\r\n\tif ( constraints.getStatus( number ) != ST_INACTIVE )\r\n\t\treturn THROWERROR( RET_CONSTRAINT_ALREADY_ACTIVE );\r\n\r\n\tif ( ( constraints.getNC( ) - getNAC( ) ) == constraints.getNUC( ) )\r\n\t\treturn THROWERROR( RET_ALL_CONSTRAINTS_ACTIVE );\r\n\r\n\tif ( ( getStatus( ) == QPS_NOTINITIALISED ) ||\r\n\t\t ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||\r\n\t\t ( getStatus( ) == QPS_HOMOTOPYQPSOLVED ) ||\r\n\t\t ( getStatus( ) == QPS_SOLVED ) )\r\n\t{\r\n\t\treturn THROWERROR( RET_UNKNOWN_BUG );\r\n\t}\r\n\r\n\r\n\t/* I) ENSURE LINEAR INDEPENDENCE OF THE WORKING SET,\r\n\t * i.e. remove a constraint or bound if linear dependence occurs. */\r\n\t/* check for LI only if Cholesky decomposition shall be updated! */\r\n\tif ( updateCholesky == BT_TRUE )\r\n\t{\r\n\t\treturnValue ensureLIreturnvalue = addConstraint_ensureLI( number,C_status );\r\n\r\n\t\tswitch ( ensureLIreturnvalue )\r\n\t\t{\r\n\t\t\tcase SUCCESSFUL_RETURN:\r\n\t\t\t\tbreak;\r\n\r\n\t\t\tcase RET_LI_RESOLVED:\r\n\t\t\t\tbreak;\r\n\r\n\t\t\tcase RET_ENSURELI_FAILED_NOINDEX:\r\n\t\t\t\treturn THROWERROR( RET_ADDCONSTRAINT_FAILED_INFEASIBILITY );\r\n\r\n\t\t\tcase RET_ENSURELI_FAILED_CYCLING:\r\n\t\t\t\treturn THROWERROR( RET_ADDCONSTRAINT_FAILED_INFEASIBILITY );\r\n\r\n\t\t\tdefault:\r\n\t\t\t\treturn THROWERROR( RET_ENSURELI_FAILED );\r\n\t\t}\r\n\t}\r\n\r\n\t/* some definitions */\r\n\tint nFR = getNFR( );\r\n\tint nAC = getNAC( );\r\n\tint nZ = getNZ( );\r\n\r\n\tint tcol = sizeT - nAC;\r\n\r\n\r\n\tint FR_idx[NVMAX];\r\n\tif ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )\r\n\t\treturn THROWERROR( RET_ADDCONSTRAINT_FAILED );\r\n\r\n\treal_t aFR[NVMAX];\r\n\treal_t wZ[NVMAX];\r\n\tfor( i=0; i 0 )\r\n\t{\r\n\t\tfor( j=0; j 0 )\r\n\t{\r\n\t\t/* II) RESTORE TRIANGULAR FORM OF T: */\r\n\t\t/* Use column-wise Givens rotations to restore reverse triangular form\r\n\t\t* of T, simultanenous change of Q (i.e. Z) and R. */\r\n\t\tfor( j=0; jgetNumberArray( FR_idx ) != SUCCESSFUL_RETURN )\r\n\t\treturn THROWERROR( RET_INDEXLIST_CORRUPTED );\r\n\r\n\t/* Check if constraint is linearly independent from the\r\n\t the active ones (<=> is element of null space of Afr). */\r\n\treal_t sum;\r\n\r\n\tfor( i=0; i 10.0*EPS )\r\n\t\t\treturn RET_LINEARLY_INDEPENDENT;\r\n\t}\r\n\r\n\treturn RET_LINEARLY_DEPENDENT;\r\n}\r\n\r\n\r\n/*\r\n *\ta d d C o n s t r a i n t _ e n s u r e L I\r\n */\r\nreturnValue QProblem::addConstraint_ensureLI( int number, SubjectToStatus C_status )\r\n{\r\n\tint i, j, ii, jj;\r\n\tint nV = getNV( );\r\n\tint nFR = getNFR( );\r\n\tint nFX = getNFX( );\r\n\tint nAC = getNAC( );\r\n\tint nZ = getNZ( );\r\n\r\n\r\n\t/* I) Check if new constraint is linearly independent from the active ones. */\r\n\treturnValue returnvalueCheckLI = addConstraint_checkLI( number );\r\n\r\n\tif ( returnvalueCheckLI == RET_INDEXLIST_CORRUPTED )\r\n\t\treturn THROWERROR( RET_ENSURELI_FAILED );\r\n\r\n\tif ( returnvalueCheckLI == RET_LINEARLY_INDEPENDENT )\r\n\t\treturn SUCCESSFUL_RETURN;\r\n\r\n\r\n \t/* II) NEW CONSTRAINT IS LINEARLY DEPENDENT: */\r\n\t/* 1) Determine coefficients of linear combination,\r\n\t * cf. M.J. Best. Applied Mathematics and Parallel Computing, chapter:\r\n\t * An Algorithm for the Solution of the Parametric Quadratic Programming\r\n\t * Problem, pages 57-76. Physica-Verlag, Heidelberg, 1996. */\r\n\tint FR_idx[NVMAX];\r\n\tif ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )\r\n\t\treturn THROWERROR( RET_ENSURELI_FAILED );\r\n\r\n\tint FX_idx[NVMAX];\r\n\tif ( bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN )\r\n\t\treturn THROWERROR( RET_ENSURELI_FAILED );\r\n\r\n\treal_t xiC[NCMAX_ALLOC];\r\n\treal_t xiC_TMP[NCMAX_ALLOC];\r\n\treal_t xiB[NVMAX];\r\n\r\n\t/* 2) Calculate xiC */\r\n\tif ( nAC > 0 )\r\n\t{\r\n\t\tif ( C_status == ST_LOWER )\r\n\t\t{\r\n\t\t\tfor( i=0; igetNumberArray( AC_idx ) != SUCCESSFUL_RETURN )\r\n\t\treturn THROWERROR( RET_ENSURELI_FAILED );\r\n\r\n\tif ( C_status == ST_LOWER )\r\n\t{\r\n\t\tfor( i=0; i ZERO ) && ( y[nV+ii] >= 0.0 ) )\r\n\t\t\t{\r\n\t\t\t\tif ( y[nV+ii]/xiC[i] < y_min )\r\n\t\t\t\t{\r\n\t\t\t\t\ty_min = y[nV+ii]/xiC[i];\r\n\t\t\t\t\ty_min_number = ii;\r\n\t\t\t\t}\r\n\t\t\t}\r\n\t\t}\r\n\t\telse\r\n\t\t{\r\n\t\t\tif ( ( xiC[i] < -ZERO ) && ( y[nV+ii] <= 0.0 ) )\r\n\t\t\t{\r\n\t\t\t\tif ( y[nV+ii]/xiC[i] < y_min )\r\n\t\t\t\t{\r\n\t\t\t\t\ty_min = y[nV+ii]/xiC[i];\r\n\t\t\t\t\ty_min_number = ii;\r\n\t\t\t\t}\r\n\t\t\t}\r\n\t\t}\r\n\t}\r\n\r\n\t/* 2) Bounds. */\r\n\tfor( i=0; i ZERO ) && ( y[ii] >= 0.0 ) )\r\n\t\t\t{\r\n\t\t\t\tif ( y[ii]/xiB[i] < y_min )\r\n\t\t\t\t{\r\n\t\t\t\t\ty_min = y[ii]/xiB[i];\r\n\t\t\t\t\ty_min_number = ii;\r\n\t\t\t\t\ty_min_isBound = BT_TRUE;\r\n\t\t\t\t}\r\n\t\t\t}\r\n\t\t}\r\n\t\telse\r\n\t\t{\r\n\t\t\tif ( ( xiB[i] < -ZERO ) && ( y[ii] <= 0.0 ) )\r\n\t\t\t{\r\n\t\t\t\tif ( y[ii]/xiB[i] < y_min )\r\n\t\t\t\t{\r\n\t\t\t\t\ty_min = y[ii]/xiB[i];\r\n\t\t\t\t\ty_min_number = ii;\r\n\t\t\t\t\ty_min_isBound = BT_TRUE;\r\n\t\t\t\t}\r\n\t\t\t}\r\n\t\t}\r\n\t}\r\n\r\n\t/* setup output preferences */\r\n\t#ifdef PC_DEBUG\r\n\tchar messageString[80];\r\n\tVisibilityStatus visibilityStatus;\r\n\r\n\tif ( printlevel == PL_HIGH )\r\n\t\tvisibilityStatus = VS_VISIBLE;\r\n\telse\r\n\t\tvisibilityStatus = VS_HIDDEN;\r\n\t#endif\r\n\r\n\r\n\t/* IV) REMOVE CONSTRAINT/BOUND FOR RESOLVING LINEAR DEPENDENCE: */\r\n\tif ( y_min_number >= 0 )\r\n\t{\r\n\t\t/* 1) Check for cycling due to infeasibility. */\r\n\t\tif ( ( cyclingManager.getCyclingStatus( number,BT_FALSE ) == CYC_PREV_REMOVED ) &&\r\n\t\t\t ( cyclingManager.getCyclingStatus( y_min_number,y_min_isBound ) == CYC_PREV_ADDED ) )\r\n\t\t{\r\n\t\t\tinfeasible = BT_TRUE;\r\n\r\n\t\t\treturn THROWERROR( RET_ENSURELI_FAILED_CYCLING );\r\n\t\t}\r\n\t\telse\r\n\t\t{\r\n\t\t\t/* set cycling data */\r\n\t\t\tcyclingManager.clearCyclingData( );\r\n\t\t\tcyclingManager.setCyclingStatus( number,BT_FALSE, CYC_PREV_ADDED );\r\n\t\t\tcyclingManager.setCyclingStatus( y_min_number,y_min_isBound, CYC_PREV_REMOVED );\r\n\t\t}\r\n\r\n\t\t/* 2) Update Lagrange multiplier... */\r\n\t\tfor( i=0; ithrowInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus );\r\n\t\t\t#endif\r\n\r\n\t\t\tif ( removeBound( y_min_number,BT_TRUE ) != SUCCESSFUL_RETURN )\r\n\t\t\t\treturn THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );\r\n\r\n\t\t\ty[y_min_number] = 0.0;\r\n\t\t}\r\n\t\telse\r\n\t\t{\r\n\t\t\t#ifdef PC_DEBUG\r\n\t\t\tsprintf( messageString,\"constraint no. %d.\",y_min_number );\r\n\t\t\tgetGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus );\r\n\t\t\t#endif\r\n\r\n\t\t\tif ( removeConstraint( y_min_number,BT_TRUE ) != SUCCESSFUL_RETURN )\r\n\t\t\t\treturn THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );\r\n\r\n\t\t\ty[nV+y_min_number] = 0.0;\r\n\t\t}\r\n\t}\r\n\telse\r\n\t{\r\n\t\t/* no constraint/bound can be removed => QP is infeasible! */\r\n\t\tinfeasible = BT_TRUE;\r\n\r\n\t\treturn THROWERROR( RET_ENSURELI_FAILED_NOINDEX );\r\n\t}\r\n\r\n\treturn getGlobalMessageHandler( )->throwInfo( RET_LI_RESOLVED,0,__FUNCTION__,__FILE__,__LINE__,VS_HIDDEN );\r\n}\r\n\r\n\r\n\r\n/*\r\n *\ta d d B o u n d\r\n */\r\nreturnValue QProblem::addBound(\tint number, SubjectToStatus B_status,\r\n\t\t\t\t\t\t\t\tBooleanType updateCholesky\r\n\t\t\t\t\t\t\t\t)\r\n{\r\n\tint i, j, ii;\r\n\r\n\t/* consistency checks */\r\n\tif ( bounds.getStatus( number ) != ST_INACTIVE )\r\n\t\treturn THROWERROR( RET_BOUND_ALREADY_ACTIVE );\r\n\r\n\tif ( getNFR( ) == bounds.getNUV( ) )\r\n\t\treturn THROWERROR( RET_ALL_BOUNDS_ACTIVE );\r\n\r\n\tif ( ( getStatus( ) == QPS_NOTINITIALISED ) ||\r\n\t\t ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||\r\n\t\t ( getStatus( ) == QPS_HOMOTOPYQPSOLVED ) ||\r\n \t\t ( getStatus( ) == QPS_SOLVED ) )\r\n\t{\r\n\t\treturn THROWERROR( RET_UNKNOWN_BUG );\r\n\t}\r\n\r\n\r\n\t/* I) ENSURE LINEAR INDEPENDENCE OF THE WORKING SET,\r\n\t * i.e. remove a constraint or bound if linear dependence occurs. */\r\n\t/* check for LI only if Cholesky decomposition shall be updated! */\r\n\tif ( updateCholesky == BT_TRUE )\r\n\t{\r\n\t\treturnValue ensureLIreturnvalue = addBound_ensureLI( number,B_status );\r\n\r\n\t\tswitch ( ensureLIreturnvalue )\r\n\t\t{\r\n\t\t\tcase SUCCESSFUL_RETURN:\r\n\t\t\t\tbreak;\r\n\r\n\t\t\tcase RET_LI_RESOLVED:\r\n\t\t\t\tbreak;\r\n\r\n\t\t\tcase RET_ENSURELI_FAILED_NOINDEX:\r\n\t\t\t\treturn THROWERROR( RET_ADDBOUND_FAILED_INFEASIBILITY );\r\n\r\n\t\t\tcase RET_ENSURELI_FAILED_CYCLING:\r\n\t\t\t\treturn THROWERROR( RET_ADDBOUND_FAILED_INFEASIBILITY );\r\n\r\n\t\t\tdefault:\r\n\t\t\t\treturn THROWERROR( RET_ENSURELI_FAILED );\r\n\t\t}\r\n\t}\r\n\r\n\r\n\t/* some definitions */\r\n\tint nFR = getNFR( );\r\n\tint nAC = getNAC( );\r\n\tint nZ = getNZ( );\r\n\r\n\tint tcol = sizeT - nAC;\r\n\r\n\r\n\t/* I) SWAP INDEXLIST OF FREE VARIABLES:\r\n\t * move the variable to be fixed to the end of the list of free variables. */\r\n\tint lastfreenumber = bounds.getFree( )->getLastNumber( );\r\n\tif ( lastfreenumber != number )\r\n\t\tif ( bounds.swapFree( number,lastfreenumber ) != SUCCESSFUL_RETURN )\r\n\t\t\tTHROWERROR( RET_ADDBOUND_FAILED );\r\n\r\n\r\n\tint FR_idx[NVMAX];\r\n\tif ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )\r\n\t\treturn THROWERROR( RET_ADDBOUND_FAILED );\r\n\r\n\treal_t w[NVMAX];\r\n\r\n\r\n\t/* II) ADD NEW ACTIVE BOUND TO TOP OF MATRIX T: */\r\n\t/* 1) add row [wZ wY] = [Z Y](number) at the top of T: assign w */\r\n\tfor( i=0; i 0 )\t /* ( nAC == 0 ) <=> ( nZ == nFR ) <=> Y and T are empty => nothing to do */\r\n\t{\r\n\t\t/* store new column a in a temporary vector instead of shifting T one column to the left */\r\n\t\treal_t tmp[NCMAX_ALLOC];\r\n\t\tfor( i=0; i is linearly independent from the\r\n\t the active ones (<=> is element of null space of Afr). */\r\n\tfor( i=0; i EPS )\r\n\t\t\treturn RET_LINEARLY_INDEPENDENT;\r\n\t}\r\n\r\n\treturn RET_LINEARLY_DEPENDENT;\r\n}\r\n\r\n\r\n/*\r\n *\ta d d B o u n d _ e n s u r e L I\r\n */\r\nreturnValue QProblem::addBound_ensureLI( int number, SubjectToStatus B_status )\r\n{\r\n\tint i, j, ii, jj;\r\n\tint nV = getNV( );\r\n\tint nFX = getNFX( );\r\n\tint nAC = getNAC( );\r\n\tint nZ = getNZ( );\r\n\r\n\r\n\t/* I) Check if new constraint is linearly independent from the active ones. */\r\n\treturnValue returnvalueCheckLI = addBound_checkLI( number );\r\n\r\n\tif ( returnvalueCheckLI == RET_LINEARLY_INDEPENDENT )\r\n\t\treturn SUCCESSFUL_RETURN;\r\n\r\n\r\n \t/* II) NEW BOUND IS LINEARLY DEPENDENT: */\r\n\t/* 1) Determine coefficients of linear combination,\r\n\t * cf. M.J. Best. Applied Mathematics and Parallel Computing, chapter:\r\n\t * An Algorithm for the Solution of the Parametric Quadratic Programming\r\n\t * Problem, pages 57-76. Physica-Verlag, Heidelberg, 1996. */\r\n\tint FR_idx[NVMAX];\r\n\tif ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )\r\n\t\treturn THROWERROR( RET_ENSURELI_FAILED );\r\n\r\n\tint FX_idx[NVMAX];\r\n\tif ( bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN )\r\n\t\treturn THROWERROR( RET_ENSURELI_FAILED );\r\n\r\n\tint AC_idx[NCMAX_ALLOC];\r\n\tif ( constraints.getActive( )->getNumberArray( AC_idx ) != SUCCESSFUL_RETURN )\r\n\t\treturn THROWERROR( RET_ENSURELI_FAILED );\r\n\r\n\treal_t xiC[NCMAX_ALLOC];\r\n\treal_t xiC_TMP[NCMAX_ALLOC];\r\n\treal_t xiB[NVMAX];\r\n\r\n\t/* 2) Calculate xiC. */\r\n\tif ( nAC > 0 )\r\n\t{\r\n\t\tif ( B_status == ST_LOWER )\r\n\t\t{\r\n\t\t\tfor( i=0; i ZERO ) && ( y[nV+ii] >= 0.0 ) )\r\n\t\t\t{\r\n\t\t\t\tif ( y[nV+ii]/xiC[i] < y_min )\r\n\t\t\t\t{\r\n\t\t\t\t\ty_min = y[nV+ii]/xiC[i];\r\n\t\t\t\t\ty_min_number = ii;\r\n\t\t\t\t}\r\n\t\t\t}\r\n\t\t}\r\n\t\telse\r\n\t\t{\r\n\t\t\tif ( ( xiC[i] < -ZERO ) && ( y[nV+ii] <= 0.0 ) )\r\n\t\t\t{\r\n\t\t\t\tif ( y[nV+ii]/xiC[i] < y_min )\r\n\t\t\t\t{\r\n\t\t\t\t\ty_min = y[nV+ii]/xiC[i];\r\n\t\t\t\t\ty_min_number = ii;\r\n\t\t\t\t}\r\n\t\t\t}\r\n\t\t}\r\n\t}\r\n\r\n\t/* 2) Bounds. */\r\n\tfor( i=0; i ZERO ) && ( y[ii] >= 0.0 ) )\r\n\t\t\t{\r\n\t\t\t\tif ( y[ii]/xiB[i] < y_min )\r\n\t\t\t\t{\r\n\t\t\t\t\ty_min = y[ii]/xiB[i];\r\n\t\t\t\t\ty_min_number = ii;\r\n\t\t\t\t\ty_min_isBound = BT_TRUE;\r\n\t\t\t\t}\r\n\t\t\t}\r\n\t\t}\r\n\t\telse\r\n\t\t{\r\n\t\t\tif ( ( xiB[i] < -ZERO ) && ( y[ii] <= 0.0 ) )\r\n\t\t\t{\r\n\t\t\t\tif ( y[ii]/xiB[i] < y_min )\r\n\t\t\t\t{\r\n\t\t\t\t\ty_min = y[ii]/xiB[i];\r\n\t\t\t\t\ty_min_number = ii;\r\n\t\t\t\t\ty_min_isBound = BT_TRUE;\r\n\t\t\t\t}\r\n\t\t\t}\r\n\t\t}\r\n\t}\r\n\r\n\t/* setup output preferences */\r\n\t#ifdef PC_DEBUG\r\n\tchar messageString[80];\r\n\tVisibilityStatus visibilityStatus;\r\n\r\n\tif ( printlevel == PL_HIGH )\r\n\t\tvisibilityStatus = VS_VISIBLE;\r\n\telse\r\n\t\tvisibilityStatus = VS_HIDDEN;\r\n\t#endif\r\n\r\n\r\n\t/* IV) REMOVE CONSTRAINT/BOUND FOR RESOLVING LINEAR DEPENDENCE: */\r\n\tif ( y_min_number >= 0 )\r\n\t{\r\n\t\t/* 1) Check for cycling due to infeasibility. */\r\n\t\tif ( ( cyclingManager.getCyclingStatus( number,BT_TRUE ) == CYC_PREV_REMOVED ) &&\r\n\t\t\t ( cyclingManager.getCyclingStatus( y_min_number,y_min_isBound ) == CYC_PREV_ADDED ) )\r\n\t\t{\r\n\t\t\tinfeasible = BT_TRUE;\r\n\r\n\t\t\treturn THROWERROR( RET_ENSURELI_FAILED_CYCLING );\r\n\t\t}\r\n\t\telse\r\n\t\t{\r\n\t\t\t/* set cycling data */\r\n\t\t\tcyclingManager.clearCyclingData( );\r\n\t\t\tcyclingManager.setCyclingStatus( number,BT_TRUE, CYC_PREV_ADDED );\r\n\t\t\tcyclingManager.setCyclingStatus( y_min_number,y_min_isBound, CYC_PREV_REMOVED );\r\n\t\t}\r\n\r\n\r\n\t\t/* 2) Update Lagrange multiplier... */\r\n\t\tfor( i=0; ithrowInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus );\r\n\t\t\t#endif\r\n\r\n\t\t\tif ( removeBound( y_min_number,BT_TRUE ) != SUCCESSFUL_RETURN )\r\n\t\t\t\treturn THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );\r\n\r\n\t\t\ty[y_min_number] = 0.0;\r\n\t\t}\r\n\t\telse\r\n\t\t{\r\n\t\t\t#ifdef PC_DEBUG\r\n\t\t\tsprintf( messageString,\"constraint no. %d.\",y_min_number );\r\n\t\t\tgetGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus );\r\n\t\t\t#endif\r\n\r\n\t\t\tif ( removeConstraint( y_min_number,BT_TRUE ) != SUCCESSFUL_RETURN )\r\n\t\t\t\treturn THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );\r\n\r\n\t\t\ty[nV+y_min_number] = 0.0;\r\n\t\t}\r\n\t}\r\n\telse\r\n\t{\r\n\t\t/* no constraint/bound can be removed => QP is infeasible! */\r\n\t\tinfeasible = BT_TRUE;\r\n\r\n\t\treturn THROWERROR( RET_ENSURELI_FAILED_NOINDEX );\r\n\t}\r\n\r\n\treturn getGlobalMessageHandler( )->throwInfo( RET_LI_RESOLVED,0,__FUNCTION__,__FILE__,__LINE__,VS_HIDDEN );\r\n}\r\n\r\n\r\n\r\n/*\r\n *\tr e m o v e C o n s t r a i n t\r\n */\r\nreturnValue QProblem::removeConstraint(\tint number,\r\n\t\t\t\t\t\t\t\t\t\tBooleanType updateCholesky\r\n\t\t\t\t\t\t\t\t\t\t)\r\n{\r\n\tint i, j, ii, jj;\r\n\r\n\t/* consistency check */\r\n\tif ( ( getStatus( ) == QPS_NOTINITIALISED ) ||\r\n\t\t ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||\r\n\t\t ( getStatus( ) == QPS_HOMOTOPYQPSOLVED ) ||\r\n \t\t ( getStatus( ) == QPS_SOLVED ) )\r\n\t{\r\n\t\treturn THROWERROR( RET_UNKNOWN_BUG );\r\n\t}\r\n\r\n\t/* some definitions */\r\n\tint nFR = getNFR( );\r\n\tint nAC = getNAC( );\r\n\tint nZ = getNZ( );\r\n\r\n\tint tcol = sizeT - nAC;\r\n\tint number_idx = constraints.getActive( )->getIndex( number );\r\n\r\n\r\n\t/* consistency checks */\r\n\tif ( constraints.getStatus( number ) == ST_INACTIVE )\r\n\t\treturn THROWERROR( RET_CONSTRAINT_NOT_ACTIVE );\r\n\r\n\tif ( ( number_idx < 0 ) || ( number_idx >= nAC ) )\r\n\t\treturn THROWERROR( RET_CONSTRAINT_NOT_ACTIVE );\r\n\r\n\r\n\tint FR_idx[NVMAX];\r\n\tif ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )\r\n\t\treturn THROWERROR( RET_REMOVECONSTRAINT_FAILED );\r\n\r\n\r\n\t/* I) REMOVE th ROW FROM T,\r\n\t * i.e. shift rows number+1 through nAC upwards (instead of the actual\r\n\t * constraint number its corresponding index within matrix A is used). */\r\n\tif ( number_idx < nAC-1 )\r\n\t{\r\n\t\tfor( i=(number_idx+1); i=0; --j )\r\n\t\t{\r\n\t\t\tcomputeGivens( T[(nAC-2-j)*NVMAX + tcol+1+j],T[(nAC-2-j)*NVMAX + tcol+j], T[(nAC-2-j)*NVMAX + tcol+1+j],T[(nAC-2-j)*NVMAX + tcol+j],c,s );\r\n\r\n\t\t\tfor( i=(nAC-j-1); i<(nAC-1); ++i )\r\n\t\t\t\tapplyGivens( c,s,T[i*NVMAX + tcol+1+j],T[i*NVMAX + tcol+j], T[i*NVMAX + tcol+1+j],T[i*NVMAX + tcol+j] );\r\n\r\n\t\t\tfor( i=0; i 0 )\r\n\t\t{\r\n\t\t\treal_t ZHz[NVMAX];\r\n\t\t\tfor ( i=0; i 0.0 )\r\n\t\t\tR[nZ*NVMAX + nZ] = sqrt( rho2 );\r\n\t\telse\r\n\t\t{\r\n\t\t\thessianType = HST_SEMIDEF;\r\n\t\t\treturn THROWERROR( RET_HESSIAN_NOT_SPD );\r\n\t\t}\r\n\t}\r\n\r\n\t/* IV) UPDATE INDICES */\r\n\tif ( constraints.moveActiveToInactive( number ) != SUCCESSFUL_RETURN )\r\n\t\treturn THROWERROR( RET_REMOVECONSTRAINT_FAILED );\r\n\r\n\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n/*\r\n *\tr e m o v e B o u n d\r\n */\r\nreturnValue QProblem::removeBound(\tint number,\r\n\t\t\t\t\t\t\t\t\tBooleanType updateCholesky\r\n\t\t\t\t\t\t\t\t\t)\r\n{\r\n\tint i, j, ii, jj;\r\n\r\n\t/* consistency checks */\r\n\tif ( bounds.getStatus( number ) == ST_INACTIVE )\r\n\t\treturn THROWERROR( RET_BOUND_NOT_ACTIVE );\r\n\r\n\tif ( ( getStatus( ) == QPS_NOTINITIALISED ) ||\r\n\t\t ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||\r\n\t\t ( getStatus( ) == QPS_HOMOTOPYQPSOLVED ) ||\r\n \t\t ( getStatus( ) == QPS_SOLVED ) )\r\n\t{\r\n\t\treturn THROWERROR( RET_UNKNOWN_BUG );\r\n\t}\r\n\r\n\t/* some definitions */\r\n\tint nFR = getNFR( );\r\n\tint nAC = getNAC( );\r\n\tint nZ = getNZ( );\r\n\r\n\tint tcol = sizeT - nAC;\r\n\r\n\r\n\t/* I) UPDATE INDICES */\r\n\tif ( bounds.moveFixedToFree( number ) != SUCCESSFUL_RETURN )\r\n\t\treturn THROWERROR( RET_REMOVEBOUND_FAILED );\r\n\r\n\r\n\tint FR_idx[NVMAX];\r\n\tif ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )\r\n\t\treturn THROWERROR( RET_REMOVEBOUND_FAILED );\r\n\r\n\t/* I) APPEND th UNITY VECOTR TO Q. */\r\n\tint nnFRp1 = FR_idx[nFR];\r\n\tfor( i=0; i 0 )\r\n\t{\r\n\t\t/* store new column a in a temporary vector instead of shifting T one column to the left and appending a */\r\n\t\tint AC_idx[NCMAX_ALLOC];\r\n\t\tif ( constraints.getActive( )->getNumberArray( AC_idx ) != SUCCESSFUL_RETURN )\r\n\t\t\treturn THROWERROR( RET_REMOVEBOUND_FAILED );\r\n\r\n\t\treal_t tmp[NCMAX_ALLOC];\r\n\t\tfor( i=0; i=0; --j )\r\n\t\t{\r\n\t\t\tcomputeGivens( tmp[nAC-1-j],T[(nAC-1-j)*NVMAX + tcol+j],T[(nAC-1-j)*NVMAX + tcol+j],tmp[nAC-1-j],c,s );\r\n\r\n\t\t\tfor( i=(nAC-j); i 0 )\r\n\t\t{\r\n\t\t\treal_t Hz[NVMAX];\r\n\t\t\tfor( i=0; i 0 )\r\n\t\t\t{\r\n\t\t\t\treal_t r[NVMAX];\r\n\t\t\t\treal_t rhs[NVMAX];\r\n\t\t\t\tfor( i=0; i 0.0 )\r\n\t\t\tR[nZ*NVMAX + nZ] = sqrt( rho2 );\r\n\t\telse\r\n\t\t{\r\n\t\t\thessianType = HST_SEMIDEF;\r\n\t\t\treturn THROWERROR( RET_HESSIAN_NOT_SPD );\r\n\t\t}\r\n\t}\r\n\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n/*\r\n *\tb a c k s o l v e R (CODE DUPLICATE OF QProblemB CLASS!!!)\r\n */\r\nreturnValue QProblem::backsolveR(\tconst real_t* const b, BooleanType transposed,\r\n\t\t\t\t\t\t\t\t\treal_t* const a\r\n\t\t\t\t\t\t\t\t\t)\r\n{\r\n\t/* Call standard backsolve procedure (i.e. removingBound == BT_FALSE). */\r\n\treturn backsolveR( b,transposed,BT_FALSE,a );\r\n}\r\n\r\n\r\n/*\r\n *\tb a c k s o l v e R (CODE DUPLICATE OF QProblemB CLASS!!!)\r\n */\r\nreturnValue QProblem::backsolveR(\tconst real_t* const b, BooleanType transposed,\r\n\t\t\t\t\t\t\t\t\tBooleanType removingBound,\r\n\t\t\t\t\t\t\t\t\treal_t* const a\r\n\t\t\t\t\t\t\t\t\t)\r\n{\r\n\tint i, j;\r\n\tint nR = getNZ( );\r\n\r\n\treal_t sum;\r\n\r\n\t/* if backsolve is called while removing a bound, reduce nZ by one. */\r\n\tif ( removingBound == BT_TRUE )\r\n\t\t--nR;\r\n\r\n\t/* nothing to do */\r\n\tif ( nR <= 0 )\r\n\t\treturn SUCCESSFUL_RETURN;\r\n\r\n\r\n\t/* Solve Ra = b, where R might be transposed. */\r\n\tif ( transposed == BT_FALSE )\r\n\t{\r\n\t\t/* solve Ra = b */\r\n\t\tfor( i=(nR-1); i>=0; --i )\r\n\t\t{\r\n\t\t\tsum = b[i];\r\n\t\t\tfor( j=(i+1); j ZERO )\r\n\t\t\t\ta[i] = sum / R[i*NVMAX + i];\r\n\t\t\telse\r\n\t\t\t\treturn THROWERROR( RET_DIV_BY_ZERO );\r\n\t\t}\r\n\t}\r\n\telse\r\n\t{\r\n\t\t/* solve R^T*a = b */\r\n\t\tfor( i=0; i ZERO )\r\n\t\t\t\ta[i] = sum / R[i*NVMAX + i];\r\n\t\t\telse\r\n\t\t\t\treturn THROWERROR( RET_DIV_BY_ZERO );\r\n\t\t}\r\n\t}\r\n\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n\r\n/*\r\n *\tb a c k s o l v e T\r\n */\r\nreturnValue QProblem::backsolveT( const real_t* const b, BooleanType transposed, real_t* const a )\r\n{\r\n\tint i, j;\r\n\tint nT = getNAC( );\r\n\tint tcol = sizeT - nT;\r\n\r\n\treal_t sum;\r\n\r\n\t/* nothing to do */\r\n\tif ( nT <= 0 )\r\n\t\treturn SUCCESSFUL_RETURN;\r\n\r\n\r\n\t/* Solve Ta = b, where T might be transposed. */\r\n\tif ( transposed == BT_FALSE )\r\n\t{\r\n\t\t/* solve Ta = b */\r\n\t\tfor( i=0; i ZERO )\r\n\t\t\t\ta[nT-1-i] = sum / T[i*NVMAX + sizeT-1-i];\r\n\t\t\telse\r\n\t\t\t\treturn THROWERROR( RET_DIV_BY_ZERO );\r\n\t\t}\r\n\t}\r\n\telse\r\n\t{\r\n\t\t/* solve T^T*a = b */\r\n\t\tfor( i=0; i ZERO )\r\n\t\t\t\ta[nT-1-i] = sum / T[(nT-1-i)*NVMAX + tcol+i];\r\n\t\t\telse\r\n\t\t\t\treturn THROWERROR( RET_DIV_BY_ZERO );\r\n\t\t}\r\n\t}\r\n\r\n\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n/*\r\n *\th o t s t a r t _ d e t e r m i n e D a t a S h i f t\r\n */\r\nreturnValue QProblem::hotstart_determineDataShift( const int* const FX_idx, const int* const AC_idx,\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tconst real_t* const g_new, const real_t* const lbA_new, const real_t* const ubA_new,\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tconst real_t* const lb_new, const real_t* const ub_new,\r\n\t\t\t\t\t\t\t\t\t\t\t\t\treal_t* const delta_g, real_t* const delta_lbA, real_t* const delta_ubA,\r\n\t\t\t\t\t\t\t\t\t\t\t\t\treal_t* const delta_lb, real_t* const delta_ub,\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tBooleanType& Delta_bC_isZero, BooleanType& Delta_bB_isZero\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t)\r\n{\r\n\tint i, ii;\r\n\tint nC = getNC( );\r\n\tint nAC = getNAC( );\r\n\r\n\r\n\t/* I) DETERMINE DATA SHIFT FOR BOUNDS */\r\n\tQProblemB::hotstart_determineDataShift( FX_idx,g_new,lb_new,ub_new, delta_g,delta_lb,delta_ub, Delta_bB_isZero );\r\n\r\n\r\n\t/* II) DETERMINE DATA SHIFT FOR CONSTRAINTS */\r\n\t/* 1) Calculate shift directions. */\r\n\tfor( i=0; i EPS ) || ( getAbs( delta_ubA[ii] ) > EPS ) )\r\n\t\t{\r\n\t\t\tDelta_bC_isZero = BT_FALSE;\r\n\t\t\tbreak;\r\n\t\t}\r\n\t}\r\n\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n/*\r\n *\th o t s t a r t _ d e t e r m i n e S t e p D i r e c t i o n\r\n */\r\nreturnValue QProblem::hotstart_determineStepDirection(\tconst int* const FR_idx, const int* const FX_idx, const int* const AC_idx,\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_g, const real_t* const delta_lbA, const real_t* const delta_ubA,\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_lb, const real_t* const delta_ub,\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\tBooleanType Delta_bC_isZero, BooleanType Delta_bB_isZero,\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\treal_t* const delta_xFX, real_t* const delta_xFR,\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\treal_t* const delta_yAC, real_t* const delta_yFX\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t)\r\n{\r\n\tint i, j, ii, jj;\r\n\tint nFR = getNFR( );\r\n\tint nFX = getNFX( );\r\n\tint nAC = getNAC( );\r\n\tint nZ = getNZ( );\r\n\r\n\t/* initialise auxiliary vectors */\r\n\treal_t HMX_delta_xFX[NVMAX];\r\n\treal_t YFR_delta_xFRy[NVMAX];\r\n\treal_t ZFR_delta_xFRz[NVMAX];\r\n\treal_t HFR_YFR_delta_xFRy[NVMAX];\r\n\tfor( i=0; i 0 )\r\n\t{\r\n\t\tfor( i=0; i 0 )\r\n\t{\r\n\t\t/* 1) Determine delta_xFRy. */\r\n\t\tif ( nAC > 0 )\r\n\t\t{\r\n\t\t\tif ( ( Delta_bC_isZero == BT_TRUE ) && ( Delta_bB_isZero == BT_TRUE ) )\r\n\t\t\t{\r\n\t\t\t\tfor( i=0; i 0 )\r\n\t\t\t{\r\n\t\t\t\tfor( i=0; i 0 )\r\n\t\t\t{\r\n\t\t\t\tif ( ( Delta_bC_isZero == BT_FALSE ) || ( Delta_bB_isZero == BT_FALSE ) )\r\n\t\t\t\t{\r\n\t\t\t\t\tfor( i=0; i 0 )\r\n\t\t\t{\r\n\t\t\t\t/* auxiliary variable */\r\n\t\t\t\treal_t delta_xFRz_TMP[NVMAX];\r\n\t\t\t\treal_t delta_xFRz_RHS[NVMAX];\r\n\r\n\r\n\t\t\t\tif ( ( nAC > 0 ) && ( nFX > 0 ) && ( Delta_bB_isZero == BT_FALSE ) )\r\n\t\t\t\t{\r\n\t\t\t\t\tfor( j=0; j 0 ) /* => Delta_bB_isZero == BT_TRUE, as BT_FALSE would imply nFX>0 */\r\n\t\t\t\t\t\t{\r\n\t\t\t\t\t\t\tfor( j=0; j 0 ) /* => ( nFR = nZ + nAC > 0 ) */\r\n\t{\r\n\t\t/* auxiliary variables */\r\n\t\treal_t delta_yAC_TMP[NCMAX_ALLOC];\r\n\t\tfor( i=0; i 0 )\r\n\t\t\t{\r\n\t\t\t\tfor( i=0; i 0 )\r\n\t\t\t{\r\n\t\t\t\tif ( Delta_bB_isZero == BT_FALSE )\r\n\t\t\t\t{\r\n\t\t\t\t\tfor( i=0; i 0 )\r\n\t{\r\n\t\tfor( i=0; i 0.0 )\r\n\t\t\t\t\t\tmaxStepLength[nV+ii] = y[nV+ii] / ( -delta_yAC[i] );\r\n\t\t\t\t\telse\r\n\t\t\t\t\t\tmaxStepLength[nV+ii] = 0.0;\r\n\t\t\t\t}\r\n\t\t\t}\r\n\t\t\telse\r\n\t\t\t{\r\n\t\t\t\t/* active upper constraints' bounds */\r\n\t\t\t\tif ( delta_yAC[i] > ZERO )\r\n\t\t\t\t{\r\n\t\t\t\t\tif ( y[nV+ii] < 0.0 )\r\n\t\t\t\t\t\tmaxStepLength[nV+ii] = y[nV+ii] / ( -delta_yAC[i] );\r\n\t\t\t\t\telse\r\n\t\t\t\t\t\tmaxStepLength[nV+ii] = 0.0;\r\n\t\t\t\t}\r\n\t\t\t}\r\n\t\t}\r\n\t}\r\n\r\n\t/* 2) Ensure that active dual bounds remain valid\r\n\t * (ignoring implicitly fixed variables). */\r\n\tfor( i=0; i 0.0 )\r\n\t\t\t\t\t\tmaxStepLength[ii] = y[ii] / ( -delta_yFX[i] );\r\n\t\t\t\t\telse\r\n\t\t\t\t\t\tmaxStepLength[ii] = 0.0;\r\n\t\t\t\t}\r\n\t\t\t}\r\n\t\t\telse\r\n\t\t\t{\r\n\t\t\t\t/* active upper bounds */\r\n\t\t\t\tif ( delta_yFX[i] > ZERO )\r\n\t\t\t\t{\r\n\t\t\t\t\tif ( y[ii] < 0.0 )\r\n\t\t\t\t\t\tmaxStepLength[ii] = y[ii] / ( -delta_yFX[i] );\r\n\t\t\t\t\telse\r\n\t\t\t\t\t\tmaxStepLength[ii] = 0.0;\r\n\t\t\t\t}\r\n\t\t\t}\r\n\t\t}\r\n\t}\r\n\r\n\r\n\t/* II) DETERMINE MAXIMUM PRIMAL STEPLENGTH */\r\n\t/* 1) Ensure that inactive constraints' bounds remain valid\r\n\t * (ignoring unbounded constraints). */\r\n\treal_t delta_x[NVMAX];\r\n\tfor( j=0; j delta_Ax[ii] )\r\n\t\t\t\t{\r\n\t\t\t\t\tif ( Ax[ii] > lbA[ii] )\r\n\t\t\t\t\t\tmaxStepLength[nV+ii] = ( Ax[ii] - lbA[ii] ) / ( delta_lbA[ii] - delta_Ax[ii] );\r\n\t\t\t\t\telse\r\n\t\t\t\t\t\tmaxStepLength[nV+ii] = 0.0;\r\n\t\t\t\t}\r\n\t\t\t}\r\n\r\n\t\t\t/* inactive upper constraints' bounds */\r\n\t\t\tif ( constraints.isNoUpper( ) == BT_FALSE )\r\n\t\t\t{\r\n\t\t\t\tif ( delta_ubA[ii] < delta_Ax[ii] )\r\n\t\t\t\t{\r\n\t\t\t\t\tif ( Ax[ii] < ubA[ii] )\r\n\t\t\t\t\t\tmaxStepLength[nV+nC+nV+ii] = ( Ax[ii] - ubA[ii] ) / ( delta_ubA[ii] - delta_Ax[ii] );\r\n\t\t\t\t\telse\r\n\t\t\t\t\t\tmaxStepLength[nV+nC+nV+ii] = 0.0;\r\n\t\t\t\t}\r\n\t\t\t}\r\n\t\t}\r\n\t}\r\n\r\n\r\n\t/* 2) Ensure that inactive bounds remain valid\r\n\t * (ignoring unbounded variables). */\r\n\t/* inactive lower bounds */\r\n\tif ( bounds.isNoLower( ) == BT_FALSE )\r\n\t{\r\n\t\tfor( i=0; i delta_xFR[i] )\r\n\t\t\t\t{\r\n\t\t\t\t\tif ( x[ii] > lb[ii] )\r\n\t\t\t\t\t\tmaxStepLength[ii] = ( x[ii] - lb[ii] ) / ( delta_lb[ii] - delta_xFR[i] );\r\n\t\t\t\t\telse\r\n\t\t\t\t\t\tmaxStepLength[ii] = 0.0;\r\n\t\t\t\t}\r\n\t\t}\r\n\t}\r\n\r\n\t/* inactive upper bounds */\r\n\tif ( bounds.isNoUpper( ) == BT_FALSE )\r\n\t{\r\n\t\tfor( i=0; i EPS )\r\n\t\tcyclingManager.clearCyclingData( );\r\n\r\n\r\n\t/* V) SET MAXIMUM HOMOTOPY STEPLENGTH */\r\n\ttau = tau_new;\r\n\r\n\t#ifdef PC_DEBUG\r\n\tif ( printlevel == PL_HIGH )\r\n\t{\r\n\r\n\t \tchar messageString[80];\r\n\r\n\t\tif ( BC_status == ST_UNDEFINED )\r\n\t\t\tsprintf( messageString,\"Stepsize is %.6e!\",tau );\r\n\t\telse\r\n\t\t\tsprintf( messageString,\"Stepsize is %.6e! (BC_idx = %d, BC_isBound = %d, BC_status = %d)\",tau,BC_idx,BC_isBound,BC_status );\r\n\r\n\t\tgetGlobalMessageHandler( )->throwInfo( RET_STEPSIZE_NONPOSITIVE,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );\r\n\t}\r\n\t#endif\r\n\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n/*\r\n *\th o t s t a r t _ p e r f o r m S t e p\r\n */\r\nreturnValue QProblem::hotstart_performStep(\tconst int* const FR_idx, const int* const FX_idx, const int* const AC_idx, const int* const IAC_idx,\r\n\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_g, const real_t* const delta_lbA, const real_t* const delta_ubA,\r\n\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_lb, const real_t* const delta_ub,\r\n\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_xFX, const real_t* const delta_xFR,\r\n\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_yAC, const real_t* const delta_yFX,\r\n\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_Ax, int BC_idx, SubjectToStatus BC_status, BooleanType BC_isBound\r\n\t\t\t\t\t\t\t\t\t\t\t)\r\n{\r\n\tint i, j, ii;\r\n\tint nV = getNV( );\r\n\tint nC = getNC( );\r\n\tint nFR = getNFR( );\r\n\tint nFX = getNFX( );\r\n\tint nAC = getNAC( );\r\n\tint nIAC = getNIAC( );\r\n\r\n\r\n\t/* I) CHECK (CONSTRAINTS') BOUNDS' CONSISTENCY */\r\n\tif ( areBoundsConsistent( delta_lb,delta_ub,delta_lbA,delta_ubA ) == BT_FALSE )\r\n\t{\r\n\t\tinfeasible = BT_TRUE;\r\n\t\ttau = 0.0;\r\n\r\n\t\treturn THROWERROR( RET_QP_INFEASIBLE );\r\n\t}\r\n\r\n\r\n\t/* II) GO TO ACTIVE SET CHANGE */\r\n\tif ( tau > ZERO )\r\n\t{\r\n\t\t/* 1) Perform step in primal und dual space... */\r\n\t\tfor( i=0; ithrowWarning( RET_STEPSIZE,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );\r\n\t\t#endif\r\n\t}\r\n\r\n\r\n\t/* setup output preferences */\r\n\t#ifdef PC_DEBUG\r\n\tchar messageString[80];\r\n\tVisibilityStatus visibilityStatus;\r\n\r\n\tif ( printlevel == PL_HIGH )\r\n\t\tvisibilityStatus = VS_VISIBLE;\r\n\telse\r\n\t\tvisibilityStatus = VS_HIDDEN;\r\n\t#endif\r\n\r\n\t\r\n\t/* III) UPDATE ACTIVE SET */\r\n\tswitch ( BC_status )\r\n\t{\r\n\t\t/* Optimal solution found as no working set change detected. */\r\n\t\tcase ST_UNDEFINED:\r\n\t\t\treturn RET_OPTIMAL_SOLUTION_FOUND;\r\n\r\n\r\n\t\t/* Remove one variable from active set. */\r\n\t\tcase ST_INACTIVE:\r\n\t\t\tif ( BC_isBound == BT_TRUE )\r\n\t\t\t{\r\n\t\t\t\t#ifdef PC_DEBUG\r\n\t\t\t\tsprintf( messageString,\"bound no. %d.\", BC_idx );\r\n\t\t\t\tgetGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus );\r\n\t\t\t\t#endif\r\n\r\n\t\t\t\tif ( removeBound( BC_idx,BT_TRUE ) != SUCCESSFUL_RETURN )\r\n\t\t\t\t\treturn THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );\r\n\r\n\t\t\t\ty[BC_idx] = 0.0;\r\n\t\t\t}\r\n\t\t\telse\r\n\t\t\t{\r\n\t\t\t\t#ifdef PC_DEBUG\r\n\t\t\t\tsprintf( messageString,\"constraint no. %d.\", BC_idx );\r\n\t\t\t\tgetGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus );\r\n\t\t\t\t#endif\r\n\r\n\t\t\t\tif ( removeConstraint( BC_idx,BT_TRUE ) != SUCCESSFUL_RETURN )\r\n\t\t\t\t\treturn THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );\r\n\r\n\t\t\t\ty[nV+BC_idx] = 0.0;\r\n\t\t\t}\r\n\t\t\tbreak;\r\n\r\n\r\n\t\t/* Add one variable to active set. */\r\n\t\tdefault:\r\n\t\t\tif ( BC_isBound == BT_TRUE )\r\n\t\t\t{\r\n\t\t\t\t#ifdef PC_DEBUG\r\n\t\t\t\tif ( BC_status == ST_LOWER )\r\n\t\t\t\t\tsprintf( messageString,\"lower bound no. %d.\", BC_idx );\r\n\t\t\t\telse\r\n\t\t\t\t\tsprintf( messageString,\"upper bound no. %d.\", BC_idx );\r\n\t\t\t\tgetGlobalMessageHandler( )->throwInfo( RET_ADD_TO_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus );\r\n\t\t\t\t#endif\r\n\r\n\t\t\t\tif ( addBound( BC_idx,BC_status,BT_TRUE ) != SUCCESSFUL_RETURN )\r\n\t\t\t\t\treturn THROWERROR( RET_ADD_TO_ACTIVESET_FAILED );\r\n\t\t\t}\r\n\t\t\telse\r\n\t\t\t{\r\n\t\t\t\t#ifdef PC_DEBUG\r\n\t\t\t\tif ( BC_status == ST_LOWER )\r\n\t\t\t\t\tsprintf( messageString,\"lower constraint's bound no. %d.\", BC_idx );\r\n\t\t\t\telse\r\n\t\t\t\t\tsprintf( messageString,\"upper constraint's bound no. %d.\", BC_idx );\r\n\t\t\t\tgetGlobalMessageHandler( )->throwInfo( RET_ADD_TO_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus );\r\n\t\t\t\t#endif\r\n\r\n\t\t\t\tif ( addConstraint( BC_idx,BC_status,BT_TRUE ) != SUCCESSFUL_RETURN )\r\n\t\t\t\t\treturn THROWERROR( RET_ADD_TO_ACTIVESET_FAILED );\r\n\t\t\t}\r\n\t\t\tbreak;\r\n\t}\r\n\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n/*\r\n *\ta r e B o u n d s C o n s i s t e n t\r\n */\r\nBooleanType QProblem::areBoundsConsistent(\tconst real_t* const delta_lb, const real_t* const delta_ub,\r\n\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_lbA, const real_t* const delta_ubA\r\n\t\t\t\t\t\t\t\t\t\t\t) const\r\n{\r\n\tint i;\r\n\r\n\t/* 1) Check bounds' consistency. */\r\n\tif ( QProblemB::areBoundsConsistent( delta_lb,delta_ub ) == BT_FALSE )\r\n\t\treturn BT_FALSE;\r\n\r\n\t/* 2) Check constraints' consistency, i.e.\r\n\t * check if delta_lb[i] is greater than delta_ub[i]\r\n\t * for a component i whose bounds are already (numerically) equal. */\r\n\tfor( i=0; i ubA[i] - BOUNDTOL ) && ( delta_lbA[i] > delta_ubA[i] + EPS ) )\r\n\t\t\treturn BT_FALSE;\r\n\r\n\treturn BT_TRUE;\r\n}\r\n\r\n\r\n/*\r\n *\ts e t u p Q P d a t a\r\n */\r\nreturnValue QProblem::setupQPdata(\tconst real_t* const _H, const real_t* const _R, const real_t* const _g, const real_t* const _A,\r\n\t\t\t\t\t\t\t\t\tconst real_t* const _lb, const real_t* const _ub,\r\n\t\t\t\t\t\t\t\t\tconst real_t* const _lbA, const real_t* const _ubA\r\n\t\t\t\t\t\t\t\t\t)\r\n{\r\n\tint i, j;\r\n\tint nV = getNV( );\r\n\tint nC = getNC( );\r\n\r\n\r\n\t/* 1) Load Hessian matrix as well as lower and upper bounds vectors. */\r\n\tif (QProblemB::setupQPdata(_H, _R, _g, _lb, _ub) != SUCCESSFUL_RETURN)\r\n\t\treturn THROWERROR( RET_INVALID_ARGUMENTS );\r\n\r\n\t/* 2) Load constraint matrix. */\r\n\tif ( ( nC > 0 ) && ( _A == 0 ) )\r\n\t\treturn THROWERROR( RET_INVALID_ARGUMENTS );\r\n\r\n\tif ( nC > 0 )\r\n\t{\r\n\t\tfor( i=0; igetNumberArray( AC_idx );\r\n\r\n\t/* 1) check for Hx + g - [yFX yAC]*[Id A]' = 0. */\r\n\tfor( i=0; i maxKKTviolation )\r\n\t\t\tmaxKKTviolation = getAbs( tmp );\r\n\t}\r\n\r\n\t/* 2) Check for [lb lbA] <= [Id A]*x <= [ub ubA]. */\r\n\t/* lbA <= Ax <= ubA */\r\n\tfor( i=0; i maxKKTviolation )\r\n\t\t\tmaxKKTviolation = lbA[i] - Ax[i];\r\n\r\n\t\tif ( Ax[i] - ubA[i] > maxKKTviolation )\r\n\t\t\tmaxKKTviolation = Ax[i] - ubA[i];\r\n\t}\r\n\r\n\t/* lb <= x <= ub */\r\n\tfor( i=0; i maxKKTviolation )\r\n\t\t\tmaxKKTviolation = lb[i] - x[i];\r\n\r\n\t\tif ( x[i] - ub[i] > maxKKTviolation )\r\n\t\t\tmaxKKTviolation = x[i] - ub[i];\r\n\t}\r\n\r\n\t/* 3) Check for correct sign of y and for complementary slackness. */\r\n\t/* bounds */\r\n\tfor( i=0; i maxKKTviolation )\r\n\t\t\t\t\tmaxKKTviolation = -y[i];\r\n\t\t\t\tif ( getAbs( x[i] - lb[i] ) > maxKKTviolation )\r\n\t\t\t\t\tmaxKKTviolation = getAbs( x[i] - lb[i] );\r\n\t\t\t\tbreak;\r\n\r\n\t\t\tcase ST_UPPER:\r\n\t\t\t\tif ( y[i] > maxKKTviolation )\r\n\t\t\t\t\tmaxKKTviolation = y[i];\r\n\t\t\t\tif ( getAbs( ub[i] - x[i] ) > maxKKTviolation )\r\n\t\t\t\t\tmaxKKTviolation = getAbs( ub[i] - x[i] );\r\n\t\t\t\tbreak;\r\n\r\n\t\t\tdefault: /* inactive */\r\n\t\t\tif ( getAbs( y[i] ) > maxKKTviolation )\r\n\t\t\t\t\tmaxKKTviolation = getAbs( y[i] );\r\n\t\t\t\tbreak;\r\n\t\t}\r\n\t}\r\n\r\n\t/* constraints */\r\n\tfor( i=0; i maxKKTviolation )\r\n\t\t\t\t\tmaxKKTviolation = -y[nV+i];\r\n\t\t\t\tif ( getAbs( Ax[i] - lbA[i] ) > maxKKTviolation )\r\n\t\t\t\t\tmaxKKTviolation = getAbs( Ax[i] - lbA[i] );\r\n\t\t\t\tbreak;\r\n\r\n\t\t\tcase ST_UPPER:\r\n\t\t\t\tif ( y[nV+i] > maxKKTviolation )\r\n\t\t\t\t\tmaxKKTviolation = y[nV+i];\r\n\t\t\t\tif ( getAbs( ubA[i] - Ax[i] ) > maxKKTviolation )\r\n\t\t\t\t\tmaxKKTviolation = getAbs( ubA[i] - Ax[i] );\r\n\t\t\t\tbreak;\r\n\r\n\t\t\tdefault: /* inactive */\r\n\t\t\tif ( getAbs( y[nV+i] ) > maxKKTviolation )\r\n\t\t\t\t\tmaxKKTviolation = getAbs( y[nV+i] );\r\n\t\t\t\tbreak;\r\n\t\t}\r\n\t}\r\n\r\n\tif ( maxKKTviolation > CRITICALACCURACY )\r\n\t\treturn RET_NO_SOLUTION;\r\n\r\n\tif ( maxKKTviolation > DESIREDACCURACY )\r\n\t\treturn RET_INACCURATE_SOLUTION;\r\n\r\n\t#endif /* __PERFORM_KKT_TEST__ */\r\n\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n/*\r\n *\tend of file\r\n */\r\n"
},
{
"path": "externals/qpoases/SRC/QProblem.ipp",
"content": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.\r\n *\r\n *\tqpOASES is free software; you can redistribute it and/or\r\n *\tmodify it under the terms of the GNU Lesser General Public\r\n *\tLicense as published by the Free Software Foundation; either\r\n *\tversion 2.1 of the License, or (at your option) any later version.\r\n *\r\n *\tqpOASES is distributed in the hope that it will be useful,\r\n *\tbut WITHOUT ANY WARRANTY; without even the implied warranty of\r\n *\tMERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU\r\n *\tLesser General Public License for more details.\r\n *\r\n *\tYou should have received a copy of the GNU Lesser General Public\r\n *\tLicense along with qpOASES; if not, write to the Free Software\r\n *\tFoundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA\r\n *\r\n */\r\n\r\n\r\n/**\r\n *\t\\file SRC/QProblem.ipp\r\n *\t\\author Hans Joachim Ferreau\r\n *\t\\version 1.3embedded\r\n *\t\\date 2007-2008\r\n *\r\n *\tImplementation of inlined member functions of the QProblem class which \r\n *\tis able to use the newly developed online active set strategy for \r\n *\tparametric quadratic programming.\r\n */\r\n\r\n\r\n\r\n/*****************************************************************************\r\n * P U B L I C *\r\n *****************************************************************************/\r\n\r\n/*\r\n *\tg e t A\r\n */\r\ninline returnValue QProblem::getA( real_t* const _A ) const\r\n{\r\n\tint i;\r\n\r\n\tfor ( i=0; i= 0 ) && ( number < getNC( ) ) )\r\n\t{\r\n\t\tfor ( int i=0; i= 0 ) && ( number < getNC( ) ) )\r\n\t{\r\n\t\tvalue = lbA[number];\r\n\t\treturn SUCCESSFUL_RETURN;\r\n\t}\r\n\telse\r\n\t\treturn THROWERROR( RET_INDEX_OUT_OF_BOUNDS );\r\n}\r\n\r\n\r\n/*\r\n *\tg e t U B A\r\n */\r\ninline returnValue QProblem::getUBA( real_t* const _ubA ) const\r\n{\r\n\tint i;\r\n\r\n\tfor ( i=0; i= 0 ) && ( number < getNC( ) ) )\r\n\t{\r\n\t\tvalue = ubA[number];\r\n\t\treturn SUCCESSFUL_RETURN;\r\n\t}\r\n\telse\r\n\t\treturn THROWERROR( RET_INDEX_OUT_OF_BOUNDS );\r\n}\r\n\r\n\r\n/*\r\n *\tg e t C o n s t r a i n t s\r\n */\r\ninline returnValue QProblem::getConstraints( Constraints* const _constraints ) const\r\n{\r\n\t*_constraints = constraints;\r\n\t\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n\r\n/*\r\n *\tg e t N C\r\n */\r\ninline int QProblem::getNC( ) const\r\n{\r\n\treturn constraints.getNC( );\r\n}\r\n\r\n\r\n/*\r\n *\tg e t N E C\r\n */\r\ninline int QProblem::getNEC( ) const\r\n{\r\n\treturn constraints.getNEC( );\r\n}\r\n\r\n\r\n/*\r\n *\tg e t N A C\r\n */\r\ninline int QProblem::getNAC( )\r\n{\r\n\treturn constraints.getNAC( );\r\n}\r\n\r\n\r\n/*\r\n *\tg e t N I A C\r\n */\r\ninline int QProblem::getNIAC( )\r\n{\r\n\treturn constraints.getNIAC( );\r\n}\r\n\r\n\r\n\r\n/*****************************************************************************\r\n * P R O T E C T E D *\r\n *****************************************************************************/\r\n \r\n\r\n/*\r\n *\ts e t A\r\n */\r\ninline returnValue QProblem::setA( const real_t* const A_new )\r\n{\r\n\tint i, j;\r\n\tint nV = getNV( );\r\n\tint nC = getNC( );\r\n\r\n\t/* Set constraint matrix AND update member AX. */\r\n\tfor( j=0; j= 0 ) && ( number < getNC( ) ) )\r\n\t{\r\n\t\tAx[number] = 0.0;\r\n\r\n\t\tfor( i=0; i= 0 ) && ( number < getNC( ) ) )\r\n\t{\r\n\t\tlbA[number] = value;\r\n\t\treturn SUCCESSFUL_RETURN;\r\n\t}\r\n\telse\r\n\t\treturn THROWERROR( RET_INDEX_OUT_OF_BOUNDS );\r\n}\r\n\r\n\r\n/*\r\n *\ts e t U B A\r\n */\r\ninline returnValue QProblem::setUBA( const real_t* const ubA_new )\r\n{\r\n\tint i;\r\n\tint nC = getNC();\r\n\r\n\tfor( i=0; i= 0 ) && ( number < getNC( ) ) )\r\n\t{\r\n\t\tubA[number] = value;\r\n\t\treturn SUCCESSFUL_RETURN;\r\n\t}\r\n\telse\r\n\t\treturn THROWERROR( RET_INDEX_OUT_OF_BOUNDS );\r\n}\r\n\r\n\r\n/*\r\n *\tend of file\r\n */\r\n"
},
{
"path": "externals/qpoases/SRC/QProblemB.cpp",
"content": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.\r\n *\r\n *\tqpOASES is free software; you can redistribute it and/or\r\n *\tmodify it under the terms of the GNU Lesser General Public\r\n *\tLicense as published by the Free Software Foundation; either\r\n *\tversion 2.1 of the License, or (at your option) any later version.\r\n *\r\n *\tqpOASES is distributed in the hope that it will be useful,\r\n *\tbut WITHOUT ANY WARRANTY; without even the implied warranty of\r\n *\tMERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU\r\n *\tLesser General Public License for more details.\r\n *\r\n *\tYou should have received a copy of the GNU Lesser General Public\r\n *\tLicense along with qpOASES; if not, write to the Free Software\r\n *\tFoundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA\r\n *\r\n */\r\n\r\n\r\n/**\r\n *\t\\file SRC/QProblemB.cpp\r\n *\t\\author Hans Joachim Ferreau\r\n *\t\\version 1.3embedded\r\n *\t\\date 2007-2008\r\n *\r\n *\tImplementation of the QProblemB class which is able to use the newly\r\n *\tdeveloped online active set strategy for parametric quadratic programming.\r\n */\r\n\r\n\r\n#include \r\n\r\n#include \r\n\r\nvoid printmatrix(char *name, double *A, int m, int n) {\r\n int i, j;\r\n\r\n printf(\"%s = [...\\n\", name);\r\n for (i = 0; i < m; i++) {\r\n for (j = 0; j < n; j++)\r\n printf(\" % 9.4f\", A[i*n+j]);\r\n printf(\",\\n\");\r\n }\r\n printf(\"];\\n\");\r\n}\r\n\r\n\r\n\r\n/*****************************************************************************\r\n * P U B L I C *\r\n *****************************************************************************/\r\n\r\n\r\n/*\r\n *\tQ P r o b l e m B\r\n */\r\nQProblemB::QProblemB( )\r\n{\r\n\t/* reset global message handler */\r\n\tgetGlobalMessageHandler( )->reset( );\r\n\r\n\thasHessian = BT_FALSE;\r\n\r\n\tbounds.init( 0 );\r\n\r\n\thasCholesky = BT_FALSE;\r\n\r\n\ttau = 0.0;\r\n\r\n\thessianType = HST_POSDEF_NULLSPACE; /* Hessian is assumed to be positive definite by default */\r\n\tinfeasible = BT_FALSE;\r\n\tunbounded = BT_FALSE;\r\n\r\n\tstatus = QPS_NOTINITIALISED;\r\n\r\n\t#ifdef PC_DEBUG\r\n\tprintlevel = PL_MEDIUM;\r\n\tsetPrintLevel( PL_MEDIUM );\r\n\t#else\r\n\tprintlevel = QPOASES_PRINTLEVEL;\r\n\t#endif\r\n\r\n\tcount = 0;\r\n}\r\n\r\n\r\n/*\r\n *\tQ P r o b l e m B\r\n */\r\nQProblemB::QProblemB( int _nV )\r\n{\r\n\t/* consistency check */\r\n\tif ( _nV <= 0 )\r\n\t{\r\n\t\t_nV = 1;\r\n\t\tTHROWERROR( RET_INVALID_ARGUMENTS );\r\n\t}\r\n\r\n\thasHessian = BT_FALSE;\r\n\r\n\t/* reset global message handler */\r\n\tgetGlobalMessageHandler( )->reset( );\r\n\r\n\tbounds.init( _nV );\r\n\r\n\thasCholesky = BT_FALSE;\r\n\r\n\ttau = 0.0;\r\n\r\n\thessianType = HST_POSDEF_NULLSPACE; /* Hessian is assumed to be positive definite by default */\r\n\tinfeasible = BT_FALSE;\r\n\tunbounded = BT_FALSE;\r\n\r\n\tstatus = QPS_NOTINITIALISED;\r\n\r\n\t#ifdef PC_DEBUG\r\n\tprintlevel = PL_MEDIUM;\r\n\tsetPrintLevel( PL_MEDIUM );\r\n\t#else\r\n\tprintlevel = QPOASES_PRINTLEVEL;\r\n\t#endif\r\n\r\n\tcount = 0;\r\n}\r\n\r\n\r\n/*\r\n *\tQ P r o b l e m B\r\n */\r\nQProblemB::QProblemB( const QProblemB& rhs )\r\n{\r\n\tint i, j;\r\n\r\n\tint _nV = rhs.bounds.getNV( );\r\n\r\n\tfor( i=0; i<_nV; ++i )\r\n\t\tfor( j=0; j<_nV; ++j )\r\n\t\t\tH[i*NVMAX + j] = rhs.H[i*NVMAX + j];\r\n\r\n\thasHessian = rhs.hasHessian;\r\n\r\n\tfor( i=0; i<_nV; ++i )\r\n\t\tg[i] = rhs.g[i];\r\n\r\n\tfor( i=0; i<_nV; ++i )\r\n\t\tlb[i] = rhs.lb[i];\r\n\r\n\tfor( i=0; i<_nV; ++i )\r\n\t\tub[i] = rhs.ub[i];\r\n\r\n\r\n\tbounds = rhs.bounds;\r\n\r\n\tfor( i=0; i<_nV; ++i )\r\n\t\tfor( j=0; j<_nV; ++j )\r\n\t\t\tR[i*NVMAX + j] = rhs.R[i*NVMAX + j];\r\n\thasCholesky = rhs.hasCholesky;\r\n\r\n\tfor( i=0; i<_nV; ++i )\r\n\t\tx[i] = rhs.x[i];\r\n\r\n\tfor( i=0; i<_nV; ++i )\r\n\t\ty[i] = rhs.y[i];\r\n\r\n\ttau = rhs.tau;\r\n\r\n\thessianType = rhs.hessianType;\r\n\tinfeasible = rhs.infeasible;\r\n\tunbounded = rhs.unbounded;\r\n\r\n\tstatus = rhs.status;\r\n\r\n\tprintlevel = rhs.printlevel;\r\n\r\n\tcount = rhs.count;\r\n}\r\n\r\n\r\n/*\r\n *\t~ Q P r o b l e m B\r\n */\r\nQProblemB::~QProblemB( )\r\n{\r\n}\r\n\r\n\r\n/*\r\n *\to p e r a t o r =\r\n */\r\nQProblemB& QProblemB::operator=( const QProblemB& rhs )\r\n{\r\n\tint i, j;\r\n\r\n\tif ( this != &rhs )\r\n\t{\r\n\t\tint _nV = rhs.bounds.getNV( );\r\n\r\n\t\tfor( i=0; i<_nV; ++i )\r\n\t\t\tfor( j=0; j<_nV; ++j )\r\n\t\t\t\tH[i*NVMAX + j] = rhs.H[i*NVMAX + j];\r\n\r\n\t\thasHessian = rhs.hasHessian;\r\n\r\n\t\tfor( i=0; i<_nV; ++i )\r\n\t\t\tg[i] = rhs.g[i];\r\n\r\n\t\tfor( i=0; i<_nV; ++i )\r\n\t\t\tlb[i] = rhs.lb[i];\r\n\r\n\t\tfor( i=0; i<_nV; ++i )\r\n\t\t\tub[i] = rhs.ub[i];\r\n\r\n\t\tbounds = rhs.bounds;\r\n\r\n\t\tfor( i=0; i<_nV; ++i )\r\n\t\t\tfor( j=0; j<_nV; ++j )\r\n\t\t\t\tR[i*NVMAX + j] = rhs.R[i*NVMAX + j];\r\n\t\thasCholesky = rhs.hasCholesky;\r\n\r\n\r\n\t\tfor( i=0; i<_nV; ++i )\r\n\t\t\tx[i] = rhs.x[i];\r\n\r\n\t\tfor( i=0; i<_nV; ++i )\r\n\t\t\ty[i] = rhs.y[i];\r\n\r\n\t\ttau = rhs.tau;\r\n\r\n\t\thessianType = rhs.hessianType;\r\n\t\tinfeasible = rhs.infeasible;\r\n\t\tunbounded = rhs.unbounded;\r\n\r\n\t\tstatus = rhs.status;\r\n\r\n\t\tprintlevel = rhs.printlevel;\r\n\t\tsetPrintLevel( rhs.printlevel );\r\n\r\n\t\tcount = rhs.count;\r\n\t}\r\n\r\n\treturn *this;\r\n}\r\n\r\n\r\n/*\r\n *\tr e s e t\r\n */\r\nreturnValue QProblemB::reset( )\r\n{\r\n\tint i, j;\r\n\tint nV = getNV( );\r\n\r\n\t/** 0) Reset has Hessian flag. */\r\n\thasHessian = BT_FALSE;\r\n\r\n\t/* 1) Reset bounds. */\r\n\tbounds.init( nV );\r\n\r\n\t/* 2) Reset Cholesky decomposition. */\r\n\tfor( i=0; ithrowInfo( RET_ITERATION_STARTED,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );\r\n\t\t\t#endif\r\n\t\t}\r\n\r\n\t\t/* 1) Setup index arrays. */\r\n\t\tif ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )\r\n\t\t\treturn THROWERROR( RET_HOTSTART_FAILED );\r\n\r\n\t\tif ( bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN )\r\n\t\t\treturn THROWERROR( RET_HOTSTART_FAILED );\r\n\r\n\t\t/* 2) Initialize shift direction of the gradient and the bounds. */\r\n\t\treturnvalue = hotstart_determineDataShift( FX_idx,\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tg_new,lb_new,ub_new,\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tdelta_g,delta_lb,delta_ub,\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tDelta_bB_isZero\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t);\r\n\t\tif ( returnvalue != SUCCESSFUL_RETURN )\r\n\t\t{\r\n\t\t\tnWSR = l;\r\n\t\t\tTHROWERROR( RET_SHIFT_DETERMINATION_FAILED );\r\n\t\t\treturn returnvalue;\r\n\t\t}\r\n\r\n\t\t/* 3) Determination of step direction of X and Y. */\r\n\t\treturnvalue = hotstart_determineStepDirection(\tFR_idx,FX_idx,\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\tdelta_g,delta_lb,delta_ub,\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\tDelta_bB_isZero,\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\tdelta_xFX,delta_xFR,delta_yFX\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t);\r\n\t\tif ( returnvalue != SUCCESSFUL_RETURN )\r\n\t\t{\r\n\t\t\tnWSR = l;\r\n\t\t\tTHROWERROR( RET_STEPDIRECTION_DETERMINATION_FAILED );\r\n\t\t\treturn returnvalue;\r\n\t\t}\r\n\r\n\r\n\t\t/* 4) Determination of step length TAU. */\r\n\t\treturnvalue = hotstart_determineStepLength(\tFR_idx,FX_idx,\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tdelta_lb,delta_ub,\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tdelta_xFR,delta_yFX,\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tBC_idx,BC_status );\r\n\t\tif ( returnvalue != SUCCESSFUL_RETURN )\r\n\t\t{\r\n\t\t\tnWSR = l;\r\n\t\t\tTHROWERROR( RET_STEPLENGTH_DETERMINATION_FAILED );\r\n\t\t\treturn returnvalue;\r\n\t\t}\r\n\r\n\t\t/* 5) Realization of the homotopy step. */\r\n\t\treturnvalue = hotstart_performStep(\tFR_idx,FX_idx,\r\n\t\t\t\t\t\t\t\t\t\t\tdelta_g,delta_lb,delta_ub,\r\n\t\t\t\t\t\t\t\t\t\t\tdelta_xFX,delta_xFR,delta_yFX,\r\n\t\t\t\t\t\t\t\t\t\t\tBC_idx,BC_status\r\n\t\t\t\t\t\t\t\t\t\t\t);\r\n\r\n\r\n\t\tif ( returnvalue != SUCCESSFUL_RETURN )\r\n\t\t{\r\n\t\t\tnWSR = l;\r\n\r\n\t\t\t/* stop runtime measurement */\r\n\t\t\tif ( cputime != 0 )\r\n\t\t\t\t*cputime = getCPUtime( ) - starttime;\r\n\r\n\t\t\t/* optimal solution found? */\r\n\t\t\tif ( returnvalue == RET_OPTIMAL_SOLUTION_FOUND )\r\n\t\t\t{\r\n\t\t\t\tstatus = QPS_SOLVED;\r\n\r\n\t\t\t\tif ( printlevel == PL_HIGH )\r\n\t\t\t\t\tTHROWINFO( RET_OPTIMAL_SOLUTION_FOUND );\r\n\r\n\t\t\t\t#ifdef PC_DEBUG\r\n\t \t\t\tif ( printIteration( l,BC_idx,BC_status ) != SUCCESSFUL_RETURN )\r\n\t\t\t\t\tTHROWERROR( RET_PRINT_ITERATION_FAILED ); /* do not pass this as return value! */\r\n\t\t\t\t#endif\r\n\r\n\t\t\t\t/* check KKT optimality conditions */\r\n\t\t\t\treturn checkKKTconditions( );\r\n\t\t\t}\r\n\t\t\telse\r\n\t\t\t{\r\n\t\t\t\t/* checks for infeasibility... */\r\n\t\t\t\tif ( infeasible == BT_TRUE )\r\n\t\t\t\t{\r\n\t\t\t\t\tstatus = QPS_HOMOTOPYQPSOLVED;\r\n\t\t\t\t\treturn THROWERROR( RET_HOTSTART_STOPPED_INFEASIBILITY );\r\n\t\t\t\t}\r\n\r\n\t\t\t\t/* ...unboundedness... */\r\n\t\t\t\tif ( unbounded == BT_TRUE ) /* not necessary since objective function convex! */\r\n\t\t\t\t\treturn THROWERROR( RET_HOTSTART_STOPPED_UNBOUNDEDNESS );\r\n\r\n\t\t\t\t/* ... and throw unspecific error otherwise */\r\n\t\t\t\tTHROWERROR( RET_HOMOTOPY_STEP_FAILED );\r\n\t\t\t\treturn returnvalue;\r\n\t\t\t}\r\n\t\t}\r\n\r\n\t\t/* 6) Output information of successful QP iteration. */\r\n\t\tstatus = QPS_HOMOTOPYQPSOLVED;\r\n\r\n\t\t#ifdef PC_DEBUG\r\n\t\tif ( printIteration( l,BC_idx,BC_status ) != SUCCESSFUL_RETURN )\r\n\t\t\tTHROWERROR( RET_PRINT_ITERATION_FAILED ); /* do not pass this as return value! */\r\n\t\t#endif\r\n\t}\r\n\r\n\r\n\t/* stop runtime measurement */\r\n\tif ( cputime != 0 )\r\n\t\t*cputime = getCPUtime( ) - starttime;\r\n\r\n\r\n\t/* if programm gets to here, output information that QP could not be solved\r\n\t * within the given maximum numbers of working set changes */\r\n\tif ( printlevel == PL_HIGH )\r\n\t{\r\n\t\t#ifdef PC_DEBUG\r\n\t\tsprintf( messageString,\"(nWSR = %d)\",nWSR );\r\n\t\treturn getGlobalMessageHandler( )->throwWarning( RET_MAX_NWSR_REACHED,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );\r\n\t\t#endif\r\n\t}\r\n\r\n\t/* Finally check KKT optimality conditions. */\r\n\treturnValue returnvalueKKTcheck = checkKKTconditions( );\r\n\r\n\tif ( returnvalueKKTcheck != SUCCESSFUL_RETURN )\r\n\t\treturn returnvalueKKTcheck;\r\n\telse\r\n\t\treturn RET_MAX_NWSR_REACHED;\r\n}\r\n\r\n\r\n/*\r\n *\tg e t N Z\r\n */\r\nint QProblemB::getNZ( )\r\n{\r\n\t/* if no constraints are present: nZ=nFR */\r\n\treturn bounds.getFree( )->getLength( );\r\n}\r\n\r\n\r\n/*\r\n *\tg e t O b j V a l\r\n */\r\nreal_t QProblemB::getObjVal( ) const\r\n{\r\n\treal_t objVal;\r\n\r\n\t/* calculated optimal objective function value\r\n\t * only if current QP has been solved */\r\n\tif ( ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||\r\n\t\t ( getStatus( ) == QPS_HOMOTOPYQPSOLVED ) ||\r\n\t\t ( getStatus( ) == QPS_SOLVED ) )\r\n\t{\r\n\t\tobjVal = getObjVal( x );\r\n\t}\r\n\telse\r\n\t{\r\n\t\tobjVal = INFTY;\r\n\t}\r\n\r\n\treturn objVal;\r\n}\r\n\r\n\r\n/*\r\n *\tg e t O b j V a l\r\n */\r\nreal_t QProblemB::getObjVal( const real_t* const _x ) const\r\n{\r\n\tint i, j;\r\n\tint nV = getNV( );\r\n\r\n\treal_t obj_tmp = 0.0;\r\n\r\n\tfor( i=0; i= PL_MEDIUM ) && ( printlevel != _printlevel ) )\r\n\t\tTHROWINFO( RET_PRINTLEVEL_CHANGED );\r\n\t#endif\r\n\r\n\tprintlevel = _printlevel;\r\n\r\n\t/* update message handler preferences */\r\n \tswitch ( printlevel )\r\n \t{\r\n \t\tcase PL_NONE:\r\n \t\t\tgetGlobalMessageHandler( )->setErrorVisibilityStatus( VS_HIDDEN );\r\n\t\t\tgetGlobalMessageHandler( )->setWarningVisibilityStatus( VS_HIDDEN );\r\n\t\t\tgetGlobalMessageHandler( )->setInfoVisibilityStatus( VS_HIDDEN );\r\n\t\t\tbreak;\r\n\r\n\t\tcase PL_LOW:\r\n \t\t\tgetGlobalMessageHandler( )->setErrorVisibilityStatus( VS_VISIBLE );\r\n\t\t\tgetGlobalMessageHandler( )->setWarningVisibilityStatus( VS_HIDDEN );\r\n\t\t\tgetGlobalMessageHandler( )->setInfoVisibilityStatus( VS_HIDDEN );\r\n\t\t\tbreak;\r\n\r\n \t\tdefault: /* PL_MEDIUM, PL_HIGH */\r\n \t\t\tgetGlobalMessageHandler( )->setErrorVisibilityStatus( VS_VISIBLE );\r\n\t\t\tgetGlobalMessageHandler( )->setWarningVisibilityStatus( VS_VISIBLE );\r\n\t\t\tgetGlobalMessageHandler( )->setInfoVisibilityStatus( VS_VISIBLE );\r\n\t\t\tbreak;\r\n \t}\r\n\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n\r\n/*****************************************************************************\r\n * P R O T E C T E D *\r\n *****************************************************************************/\r\n\r\n/*\r\n *\tc h e c k F o r I d e n t i t y H e s s i a n\r\n */\r\nreturnValue QProblemB::checkForIdentityHessian( )\r\n{\r\n\tint i, j;\r\n\tint nV = getNV( );\r\n\r\n\t/* nothing to do as status flag remains unaltered\r\n\t * if Hessian differs from identity matrix */\r\n\tif ( hessianType == HST_IDENTITY )\r\n\t\treturn SUCCESSFUL_RETURN;\r\n\r\n\t/* 1) If Hessian differs from identity matrix,\r\n\t * return without changing the internal HessianType. */\r\n\tfor ( i=0; i EPS )\r\n\t\t\treturn SUCCESSFUL_RETURN;\r\n\r\n\tfor ( i=0; i EPS ) || ( getAbs( H[j*NVMAX + i] ) > EPS ) )\r\n\t\t\t\treturn SUCCESSFUL_RETURN;\r\n\t}\r\n\r\n\t/* 2) If this point is reached, Hessian equals the idetity matrix. */\r\n\thessianType = HST_IDENTITY;\r\n\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n/*\r\n *\ts e t u p S u b j e c t T o T y p e\r\n */\r\nreturnValue QProblemB::setupSubjectToType( )\r\n{\r\n\tint i;\r\n\tint nV = getNV( );\r\n\r\n\r\n\t/* 1) Check if lower bounds are present. */\r\n\tbounds.setNoLower( BT_TRUE );\r\n\tfor( i=0; i -INFTY )\r\n\t\t{\r\n\t\t\tbounds.setNoLower( BT_FALSE );\r\n\t\t\tbreak;\r\n\t\t}\r\n\r\n\t/* 2) Check if upper bounds are present. */\r\n\tbounds.setNoUpper( BT_TRUE );\r\n\tfor( i=0; i INFTY - BOUNDTOL ) )\r\n\t\t{\r\n\t\t\tbounds.setType( i,ST_UNBOUNDED );\r\n\t\t\t++nUV;\r\n\t\t}\r\n\t\telse\r\n\t\t{\r\n\t\t\tif ( lb[i] > ub[i] - BOUNDTOL )\r\n\t\t\t{\r\n\t\t\t\tbounds.setType( i,ST_EQUALITY );\r\n\t\t\t\t++nFV;\r\n\t\t\t}\r\n\t\t\telse\r\n\t\t\t{\r\n\t\t\t\tbounds.setType( i,ST_BOUNDED );\r\n\t\t\t}\r\n\t\t}\r\n\r\n\t/* 4) Set dimensions of bounds structure. */\r\n\tbounds.setNFV( nFV );\r\n\tbounds.setNUV( nUV );\r\n\tbounds.setNBV( nV - nFV - nUV );\r\n\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n/*\r\n *\tc h o l e s k y D e c o m p o s i t i o n\r\n */\r\nreturnValue QProblemB::setupCholeskyDecomposition( )\r\n{\r\n\tint i, j, k, ii, jj;\r\n\tint nV = getNV( );\r\n\tint nFR = getNFR( );\r\n\r\n\t/* If Hessian flag is false, it means that H & R already contain Cholesky\r\n\t * factorization -- provided from outside. */\r\n\tif (hasHessian == BT_FALSE)\r\n\t\treturn SUCCESSFUL_RETURN;\r\n\r\n\t/* 1) Initialises R with all zeros. */\r\n\tfor( i=0; i 0 )\r\n\t\t{\r\n\t\t\tint FR_idx[NVMAX];\r\n\t\t\tif ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )\r\n\t\t\t\treturn THROWERROR( RET_INDEXLIST_CORRUPTED );\r\n\r\n\t\t\t/* R'*R = H */\r\n\t\t\treal_t sum;\r\n\t\t\treal_t inv;\r\n\r\n\t\t\tfor( i=0; i=0; --k )\r\n\t\t\t\t\tsum -= R[k*NVMAX + i] * R[k*NVMAX + i];\r\n\r\n\t\t\t\tif ( sum > 0.0 )\r\n\t\t\t\t{\r\n\t\t\t\t\tR[i*NVMAX + i] = sqrt( sum );\r\n\t\t\t\t\tinv = 1.0 / R[i*NVMAX + i];\r\n\t\t\t\t}\r\n\t\t\t\telse\r\n\t\t\t\t{\r\n\t\t\t\t\thessianType = HST_SEMIDEF;\r\n\t\t\t\t\treturn THROWERROR( RET_HESSIAN_NOT_SPD );\r\n\t\t\t\t}\r\n\r\n\t\t\t\t/* j > i */\r\n\t\t\t\tfor( j=(i+1); j=0; --k )\r\n\t\t\t\t\t\tsum -= R[k*NVMAX + i] * R[k*NVMAX + j];\r\n\r\n\t\t\t\t\tR[i*NVMAX + j] = sum * inv;\r\n\t\t\t\t}\r\n\t\t\t}\r\n\t\t}\r\n\t}\r\n\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n/*\r\n *\ts o l v e I n i t i a l Q P\r\n */\r\nreturnValue QProblemB::solveInitialQP(\tconst real_t* const xOpt, const real_t* const yOpt,\r\n\t\t\t\t\t\t\t\t\t\tconst Bounds* const guessedBounds,\r\n\t\t\t\t\t\t\t\t\t\tint& nWSR, real_t* const cputime\r\n\t\t\t\t\t\t\t\t\t\t)\r\n{\r\n\tint i, nFR;\r\n\tint nV = getNV( );\r\n\r\n\r\n\t/* start runtime measurement */\r\n\treal_t starttime = 0.0;\r\n\tif ( cputime != 0 )\r\n\t\tstarttime = getCPUtime( );\r\n\r\n\r\n\tstatus = QPS_NOTINITIALISED;\r\n\r\n\t/* I) ANALYSE QP DATA: */\r\n\t/* 1) Check if Hessian happens to be the identity matrix. */\r\n\tif ( checkForIdentityHessian( ) != SUCCESSFUL_RETURN )\r\n\t\treturn THROWERROR( RET_INIT_FAILED );\r\n\r\n\t/* 2) Setup type of bounds (i.e. unbounded, implicitly fixed etc.). */\r\n\tif ( setupSubjectToType( ) != SUCCESSFUL_RETURN )\r\n\t\treturn THROWERROR( RET_INIT_FAILED );\r\n\r\n\tstatus = QPS_PREPARINGAUXILIARYQP;\r\n\r\n\r\n\t/* II) SETUP AUXILIARY QP WITH GIVEN OPTIMAL SOLUTION: */\r\n\t/* 1) Setup bounds data structure. */\r\n\tif ( bounds.setupAllFree( ) != SUCCESSFUL_RETURN )\r\n\t\treturn THROWERROR( RET_INIT_FAILED );\r\n\r\n\t/* 2) Setup optimal primal/dual solution for auxiliary QP. */\r\n\tif ( setupAuxiliaryQPsolution( xOpt,yOpt ) != SUCCESSFUL_RETURN )\r\n\t\treturn THROWERROR( RET_INIT_FAILED );\r\n\r\n\t/* 3) Obtain linear independent working set for auxiliary QP. */\r\n\r\n\tstatic Bounds auxiliaryBounds;\r\n\r\n\tauxiliaryBounds.init( nV );\r\n\r\n\tif ( obtainAuxiliaryWorkingSet( xOpt,yOpt,guessedBounds, &auxiliaryBounds ) != SUCCESSFUL_RETURN )\r\n\t\treturn THROWERROR( RET_INIT_FAILED );\r\n\r\n\t/* 4) Setup working set of auxiliary QP and setup cholesky decomposition. */\r\n\tif ( setupAuxiliaryWorkingSet( &auxiliaryBounds,BT_TRUE ) != SUCCESSFUL_RETURN )\r\n\t\treturn THROWERROR( RET_INIT_FAILED );\r\n\r\n\tnFR = getNFR();\r\n\t/* At the moment we can only provide a Cholesky of the Hessian if\r\n\t * the solver is cold-started. */\r\n\tif (hasCholesky == BT_FALSE || nFR != nV)\r\n\t\tif (setupCholeskyDecomposition() != SUCCESSFUL_RETURN)\r\n\t\t\treturn THROWERROR( RET_INIT_FAILED_CHOLESKY );\r\n\r\n\t/* 5) Store original QP formulation... */\r\n\treal_t g_original[NVMAX];\r\n\treal_t lb_original[NVMAX];\r\n\treal_t ub_original[NVMAX];\r\n\r\n\tfor( i=0; isetupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN )\r\n\t\t\t\t\treturn THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );\r\n\t\t\t}\r\n\t\t\telse\r\n\t\t\t{\r\n\t\t\t\tif ( auxiliaryBounds->setupBound( i,guessedBounds->getStatus( i ) ) != SUCCESSFUL_RETURN )\r\n\t\t\t\t\treturn THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );\r\n\t\t\t}\r\n\t\t}\r\n\t}\r\n\telse\t/* No initial working set specified. */\r\n\t{\r\n\t\tif ( ( xOpt != 0 ) && ( yOpt == 0 ) )\r\n\t\t{\r\n\t\t\t/* Obtain initial working set by \"clipping\". */\r\n\t\t\tfor( i=0; isetupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN )\r\n\t\t\t\t\t\treturn THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );\r\n\t\t\t\t\tcontinue;\r\n\t\t\t\t}\r\n\r\n\t\t\t\tif ( xOpt[i] >= ub[i] - BOUNDTOL )\r\n\t\t\t\t{\r\n\t\t\t\t\tif ( auxiliaryBounds->setupBound( i,ST_UPPER ) != SUCCESSFUL_RETURN )\r\n\t\t\t\t\t\treturn THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );\r\n\t\t\t\t\tcontinue;\r\n\t\t\t\t}\r\n\r\n\t\t\t\t/* Moreover, add all implictly fixed variables if specified. */\r\n\t\t\t\tif ( bounds.getType( i ) == ST_EQUALITY )\r\n\t\t\t\t{\r\n\t\t\t\t\tif ( auxiliaryBounds->setupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN )\r\n\t\t\t\t\t\treturn THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );\r\n\t\t\t\t}\r\n\t\t\t\telse\r\n\t\t\t\t{\r\n\t\t\t\t\tif ( auxiliaryBounds->setupBound( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )\r\n\t\t\t\t\t\treturn THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );\r\n\t\t\t\t}\r\n\t\t\t}\r\n\t\t}\r\n\r\n\t\tif ( ( xOpt == 0 ) && ( yOpt != 0 ) )\r\n\t\t{\r\n\t\t\t/* Obtain initial working set in accordance to sign of dual solution vector. */\r\n\t\t\tfor( i=0; i ZERO )\r\n\t\t\t\t{\r\n\t\t\t\t\tif ( auxiliaryBounds->setupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN )\r\n\t\t\t\t\t\treturn THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );\r\n\t\t\t\t\tcontinue;\r\n\t\t\t\t}\r\n\r\n\t\t\t\tif ( yOpt[i] < -ZERO )\r\n\t\t\t\t{\r\n\t\t\t\t\tif ( auxiliaryBounds->setupBound( i,ST_UPPER ) != SUCCESSFUL_RETURN )\r\n\t\t\t\t\t\treturn THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );\r\n\t\t\t\t\tcontinue;\r\n\t\t\t\t}\r\n\r\n\t\t\t\t/* Moreover, add all implictly fixed variables if specified. */\r\n\t\t\t\tif ( bounds.getType( i ) == ST_EQUALITY )\r\n\t\t\t\t{\r\n\t\t\t\t\tif ( auxiliaryBounds->setupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN )\r\n\t\t\t\t\t\treturn THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );\r\n\t\t\t\t}\r\n\t\t\t\telse\r\n\t\t\t\t{\r\n\t\t\t\t\tif ( auxiliaryBounds->setupBound( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )\r\n\t\t\t\t\t\treturn THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );\r\n\t\t\t\t}\r\n\t\t\t}\r\n\t\t}\r\n\r\n\t\t/* If xOpt and yOpt are null pointer and no initial working is specified,\r\n\t\t * start with empty working set (or implicitly fixed bounds only)\r\n\t\t * for auxiliary QP. */\r\n\t\tif ( ( xOpt == 0 ) && ( yOpt == 0 ) )\r\n\t\t{\r\n\t\t\tfor( i=0; isetupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN )\r\n\t\t\t\t\t\treturn THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );\r\n\t\t\t\t}\r\n\t\t\t\telse\r\n\t\t\t\t{\r\n\t\t\t\t\tif ( auxiliaryBounds->setupBound( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )\r\n\t\t\t\t\t\treturn THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );\r\n\t\t\t\t}\r\n\t\t\t}\r\n\t\t}\r\n\t}\r\n\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n/*\r\n *\ts e t u p A u x i l i a r y W o r k i n g S e t\r\n */\r\nreturnValue QProblemB::setupAuxiliaryWorkingSet( \tconst Bounds* const auxiliaryBounds,\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tBooleanType setupAfresh\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t)\r\n{\r\n\tint i;\r\n\tint nV = getNV( );\r\n\r\n\t/* consistency checks */\r\n\tif ( auxiliaryBounds != 0 )\r\n\t{\r\n\t\tfor( i=0; igetStatus( i ) == ST_UNDEFINED ) )\r\n\t\t\t\treturn THROWERROR( RET_UNKNOWN_BUG );\r\n\t}\r\n\telse\r\n\t{\r\n\t\treturn THROWERROR( RET_INVALID_ARGUMENTS );\r\n\t}\r\n\r\n\r\n\t/* I) SETUP CHOLESKY FLAG:\r\n\t * Cholesky decomposition shall only be updated if working set\r\n\t * shall be updated (i.e. NOT setup afresh!) */\r\n\tBooleanType updateCholesky;\r\n\tif ( setupAfresh == BT_TRUE )\r\n\t\tupdateCholesky = BT_FALSE;\r\n\telse\r\n\t\tupdateCholesky = BT_TRUE;\r\n\r\n\r\n\t/* II) REMOVE FORMERLY ACTIVE BOUNDS (IF NECESSARY): */\r\n\tif ( setupAfresh == BT_FALSE )\r\n\t{\r\n\t\t/* Remove all active bounds that shall be inactive AND\r\n\t\t* all active bounds that are active at the wrong bound. */\r\n\t\tfor( i=0; igetStatus( i ) != ST_LOWER ) )\r\n\t\t\t\tif ( removeBound( i,updateCholesky ) != SUCCESSFUL_RETURN )\r\n\t\t\t\t\treturn THROWERROR( RET_SETUP_WORKINGSET_FAILED );\r\n\r\n\t\t\tif ( ( bounds.getStatus( i ) == ST_UPPER ) && ( auxiliaryBounds->getStatus( i ) != ST_UPPER ) )\r\n\t\t\t\tif ( removeBound( i,updateCholesky ) != SUCCESSFUL_RETURN )\r\n\t\t\t\t\treturn THROWERROR( RET_SETUP_WORKINGSET_FAILED );\r\n\t\t}\r\n\t}\r\n\r\n\r\n\t/* III) ADD NEWLY ACTIVE BOUNDS: */\r\n\t/* Add all inactive bounds that shall be active AND\r\n\t * all formerly active bounds that have been active at the wrong bound. */\r\n\tfor( i=0; igetStatus( i ) != ST_INACTIVE ) )\r\n\t\t{\r\n\t\t\tif ( addBound( i,auxiliaryBounds->getStatus( i ),updateCholesky ) != SUCCESSFUL_RETURN )\r\n\t\t\t\treturn THROWERROR( RET_SETUP_WORKINGSET_FAILED );\r\n\t\t}\r\n\t}\r\n\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n/*\r\n *\ts e t u p A u x i l i a r y Q P s o l u t i o n\r\n */\r\nreturnValue QProblemB::setupAuxiliaryQPsolution(\tconst real_t* const xOpt, const real_t* const yOpt\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t)\r\n{\r\n\tint i;\r\n\tint nV = getNV( );\r\n\r\n\r\n\t/* Setup primal/dual solution vectors for auxiliary initial QP:\r\n\t * if a null pointer is passed, a zero vector is assigned;\r\n\t * old solution vector is kept if pointer to internal solution vector is passed. */\r\n\tif ( xOpt != 0 )\r\n\t{\r\n\t\tif ( xOpt != x )\r\n\t\t\tfor( i=0; igetIndex( number );\r\n\r\n\treal_t c, s;\r\n\r\n\t/* 2) Use row-wise Givens rotations to restore upper triangular form of R. */\r\n\tfor( i=number_idx+1; ith column and ... */\r\n\tfor( i=0; igetNumberArray( FR_idx ) != SUCCESSFUL_RETURN )\r\n\t\treturn THROWERROR( RET_REMOVEBOUND_FAILED );\r\n\r\n\t/* 1) Calculate new column of cholesky decomposition. */\r\n\treal_t rhs[NVMAX];\r\n\treal_t r[NVMAX];\r\n\treal_t r0 = H[number*NVMAX + number];\r\n\r\n\tfor( i=0; i 0.0 )\r\n\t\tR[nFR*NVMAX + nFR] = sqrt( r0 );\r\n\telse\r\n\t{\r\n\t\thessianType = HST_SEMIDEF;\r\n\t\treturn THROWERROR( RET_HESSIAN_NOT_SPD );\r\n\t}\r\n\r\n\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n/*\r\n *\tb a c k s o l v e R (CODE DUPLICATED IN QProblem CLASS!!!)\r\n */\r\nreturnValue QProblemB::backsolveR(\tconst real_t* const b, BooleanType transposed,\r\n\t\t\t\t\t\t\t\t\treal_t* const a\r\n\t\t\t\t\t\t\t\t\t)\r\n{\r\n\t/* Call standard backsolve procedure (i.e. removingBound == BT_FALSE). */\r\n\treturn backsolveR( b,transposed,BT_FALSE,a );\r\n}\r\n\r\n\r\n/*\r\n *\tb a c k s o l v e R (CODE DUPLICATED IN QProblem CLASS!!!)\r\n */\r\nreturnValue QProblemB::backsolveR(\tconst real_t* const b, BooleanType transposed,\r\n\t\t\t\t\t\t\t\t\tBooleanType removingBound,\r\n\t\t\t\t\t\t\t\t\treal_t* const a\r\n\t\t\t\t\t\t\t\t\t)\r\n{\r\n\tint i, j;\r\n\tint nR = getNZ( );\r\n\r\n\treal_t sum;\r\n\r\n\t/* if backsolve is called while removing a bound, reduce nZ by one. */\r\n\tif ( removingBound == BT_TRUE )\r\n\t\t--nR;\r\n\r\n\t/* nothing to do */\r\n\tif ( nR <= 0 )\r\n\t\treturn SUCCESSFUL_RETURN;\r\n\r\n\r\n\t/* Solve Ra = b, where R might be transposed. */\r\n\tif ( transposed == BT_FALSE )\r\n\t{\r\n\t\t/* solve Ra = b */\r\n\t\tfor( i=(nR-1); i>=0; --i )\r\n\t\t{\r\n\t\t\tsum = b[i];\r\n\t\t\tfor( j=(i+1); j ZERO )\r\n\t\t\t\ta[i] = sum / R[i*NVMAX + i];\r\n\t\t\telse\r\n\t\t\t\treturn THROWERROR( RET_DIV_BY_ZERO );\r\n\t\t}\r\n\t}\r\n\telse\r\n\t{\r\n\t\t/* solve R^T*a = b */\r\n\t\tfor( i=0; i ZERO )\r\n\t\t\t\ta[i] = sum / R[i*NVMAX + i];\r\n\t\t\telse\r\n\t\t\t\treturn THROWERROR( RET_DIV_BY_ZERO );\r\n\t\t}\r\n\t}\r\n\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n/*\r\n *\th o t s t a r t _ d e t e r m i n e D a t a S h i f t\r\n */\r\nreturnValue QProblemB::hotstart_determineDataShift(\tconst int* const FX_idx,\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tconst real_t* const g_new, const real_t* const lb_new, const real_t* const ub_new,\r\n\t\t\t\t\t\t\t\t\t\t\t\t\treal_t* const delta_g, real_t* const delta_lb, real_t* const delta_ub,\r\n\t\t\t\t\t\t\t\t\t\t\t\t\tBooleanType& Delta_bB_isZero\r\n\t\t\t\t\t\t\t\t\t\t\t\t\t)\r\n{\r\n\tint i, ii;\r\n\tint nV = getNV( );\r\n\tint nFX = getNFX( );\r\n\r\n\r\n\t/* 1) Calculate shift directions. */\r\n\tfor( i=0; i EPS ) || ( getAbs( delta_ub[ii] ) > EPS ) )\r\n\t\t{\r\n\t\t\tDelta_bB_isZero = BT_FALSE;\r\n\t\t\tbreak;\r\n\t\t}\r\n\t}\r\n\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n/*\r\n *\ta r e B o u n d s C o n s i s t e n t\r\n */\r\nBooleanType QProblemB::areBoundsConsistent(\tconst real_t* const delta_lb, const real_t* const delta_ub\r\n\t\t\t\t\t\t\t\t\t\t\t) const\r\n{\r\n\tint i;\r\n\r\n\t/* Check if delta_lb[i] is greater than delta_ub[i]\r\n\t * for a component i whose bounds are already (numerically) equal. */\r\n\tfor( i=0; i ub[i] - BOUNDTOL ) && ( delta_lb[i] > delta_ub[i] + EPS ) )\r\n\t\t\treturn BT_FALSE;\r\n\r\n\treturn BT_TRUE;\r\n}\r\n\r\n\r\n/*\r\n *\ts e t u p Q P d a t a\r\n */\r\nreturnValue QProblemB::setupQPdata(\tconst real_t* const _H, const real_t* const _R, const real_t* const _g,\r\n\t\t\t\t\t\t\t\t\tconst real_t* const _lb, const real_t* const _ub\r\n\t\t\t\t\t\t\t\t\t)\r\n{\r\n\tint i, j;\r\n\tint nV = getNV( );\r\n\r\n\t/* 1) Setup Hessian matrix and it's Cholesky factorization. */\r\n\tif (_H != 0)\r\n\t{\r\n\t\tfor( i=0; i 0 )\r\n\t{\r\n\t\tfor( i=0; i 0 )\r\n\t{\r\n\t\t/* auxiliary variables */\r\n\t\treal_t delta_xFRz_TMP[NVMAX];\r\n\t\treal_t delta_xFRz_RHS[NVMAX];\r\n\r\n\t\t/* Determine delta_xFRz. */\r\n\t\tif ( Delta_bB_isZero == BT_FALSE )\r\n\t\t{\r\n\t\t\tfor( i=0; i 0 )\r\n\t{\r\n\t\tfor( i=0; i= 0.0 ) )\r\n\t\t\t\t{\r\n\t\t\t\t\ttau_tmp = y[ii] / ( -delta_yFX[i] );\r\n\t\t\t\t\tif ( tau_tmp < tau_new )\r\n\t\t\t\t\t{\r\n\t\t\t\t\t\tif ( tau_tmp >= 0.0 )\r\n\t\t\t\t\t\t{\r\n\t\t\t\t\t\t\ttau_new = tau_tmp;\r\n\t\t\t\t\t\t\tBC_idx = ii;\r\n\t\t\t\t\t\t\tBC_status = ST_INACTIVE;\r\n\t\t\t\t\t\t}\r\n\t\t\t\t\t}\r\n\t\t\t\t}\r\n\t\t\t}\r\n\t\t\telse\r\n\t\t\t{\r\n\t\t\t\t/* 2) Active upper bounds. */\r\n\t\t\t\tif ( ( delta_yFX[i] > ZERO ) && ( y[ii] <= 0.0 ) )\r\n\t\t\t\t{\r\n\t\t\t\t\ttau_tmp = y[ii] / ( -delta_yFX[i] );\r\n\t\t\t\t\tif ( tau_tmp < tau_new )\r\n\t\t\t\t\t{\r\n\t\t\t\t\t\tif ( tau_tmp >= 0.0 )\r\n\t\t\t\t\t\t{\r\n\t\t\t\t\t\t\ttau_new = tau_tmp;\r\n\t\t\t\t\t\t\tBC_idx = ii;\r\n\t\t\t\t\t\t\tBC_status = ST_INACTIVE;\r\n\t\t\t\t\t\t}\r\n\t\t\t\t\t}\r\n\t\t\t\t}\r\n\t\t\t}\r\n\t\t}\r\n\t}\r\n\r\n\r\n\t/* II) DETERMINE MAXIMUM PRIMAL STEPLENGTH, i.e. ensure that\r\n\t * inactive bounds remain valid (ignoring unbounded variables). */\r\n\t/* 1) Inactive lower bounds. */\r\n\tif ( bounds.isNoLower( ) == BT_FALSE )\r\n\t{\r\n\t\tfor( i=0; i delta_xFR[i] )\r\n\t\t\t\t{\r\n\t\t\t\t\tif ( x[ii] > lb[ii] )\r\n\t\t\t\t\t\ttau_tmp = ( x[ii] - lb[ii] ) / ( delta_lb[ii] - delta_xFR[i] );\r\n\t\t\t\t\telse\r\n\t\t\t\t\t\ttau_tmp = 0.0;\r\n\r\n\t\t\t\t\tif ( tau_tmp < tau_new )\r\n\t\t\t\t\t{\r\n\t\t\t\t\t\tif ( tau_tmp >= 0.0 )\r\n\t\t\t\t\t\t{\r\n\t\t\t\t\t\t\ttau_new = tau_tmp;\r\n\t\t\t\t\t\t\tBC_idx = ii;\r\n\t\t\t\t\t\t\tBC_status = ST_LOWER;\r\n\t\t\t\t\t\t}\r\n\t\t\t\t\t}\r\n\t\t\t\t}\r\n\t\t\t}\r\n\t\t}\r\n\t}\r\n\r\n\t/* 2) Inactive upper bounds. */\r\n\tif ( bounds.isNoUpper( ) == BT_FALSE )\r\n\t{\r\n\t\tfor( i=0; i= 0.0 )\r\n\t\t\t\t\t\t{\r\n\t\t\t\t\t\t\ttau_new = tau_tmp;\r\n\t\t\t\t\t\t\tBC_idx = ii;\r\n\t\t\t\t\t\t\tBC_status = ST_UPPER;\r\n\t\t\t\t\t\t}\r\n\t\t\t\t\t}\r\n\t\t\t\t}\r\n\t\t\t}\r\n\t\t}\r\n\t}\r\n\r\n\r\n\t/* III) SET MAXIMUM HOMOTOPY STEPLENGTH */\r\n\ttau = tau_new;\r\n\r\n\tif ( printlevel == PL_HIGH )\r\n\t{\r\n\t\t#ifdef PC_DEBUG\r\n\t\tchar messageString[80];\r\n\r\n\t\tif ( BC_status == ST_UNDEFINED )\r\n\t\t\tsprintf( messageString,\"Stepsize is %.6e!\",tau );\r\n\t\telse\r\n\t\t\tsprintf( messageString,\"Stepsize is %.6e! (BC_idx = %d, BC_status = %d)\",tau,BC_idx,BC_status );\r\n\r\n\t\tgetGlobalMessageHandler( )->throwInfo( RET_STEPSIZE_NONPOSITIVE,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );\r\n\t\t#endif\r\n\t}\r\n\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n/*\r\n *\th o t s t a r t _ p e r f o r m S t e p\r\n */\r\nreturnValue QProblemB::hotstart_performStep(\tconst int* const FR_idx, const int* const FX_idx,\r\n\t\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_g, const real_t* const delta_lb, const real_t* const delta_ub,\r\n\t\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_xFX, const real_t* const delta_xFR,\r\n\t\t\t\t\t\t\t\t\t\t\t\tconst real_t* const delta_yFX,\r\n\t\t\t\t\t\t\t\t\t\t\t\tint BC_idx, SubjectToStatus BC_status\r\n\t\t\t\t\t\t\t\t\t\t\t\t)\r\n{\r\n\tint i, ii;\r\n\tint nV = getNV( );\r\n\tint nFR = getNFR( );\r\n\tint nFX = getNFX( );\r\n\r\n\r\n\t/* I) CHECK BOUNDS' CONSISTENCY */\r\n\tif ( areBoundsConsistent( delta_lb,delta_ub ) == BT_FALSE )\r\n\t{\r\n\t\tinfeasible = BT_TRUE;\r\n\t\ttau = 0.0;\r\n\r\n\t\treturn THROWERROR( RET_QP_INFEASIBLE );\r\n\t}\r\n\r\n\r\n\t/* II) GO TO ACTIVE SET CHANGE */\r\n\tif ( tau > ZERO )\r\n\t{\r\n\t\t/* 1) Perform step in primal und dual space. */\r\n\t\tfor( i=0; ithrowWarning( RET_STEPSIZE,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );\r\n\t\t#endif\r\n\t}\r\n\r\n\r\n\t/* setup output preferences */\r\n\t#ifdef PC_DEBUG\r\n\tchar messageString[80];\r\n \tVisibilityStatus visibilityStatus;\r\n\r\n \tif ( printlevel == PL_HIGH )\r\n\t\tvisibilityStatus = VS_VISIBLE;\r\n\telse\r\n\t\tvisibilityStatus = VS_HIDDEN;\r\n\t#endif\r\n\r\n\r\n\t/* III) UPDATE ACTIVE SET */\r\n\tswitch ( BC_status )\r\n\t{\r\n\t\t/* Optimal solution found as no working set change detected. */\r\n\t\tcase ST_UNDEFINED:\r\n\t\t\treturn RET_OPTIMAL_SOLUTION_FOUND;\r\n\r\n\r\n\t\t/* Remove one variable from active set. */\r\n\t\tcase ST_INACTIVE:\r\n\t\t\t#ifdef PC_DEBUG\r\n\t\t\tsprintf( messageString,\"bound no. %d.\", BC_idx );\r\n\t\t\tgetGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus );\r\n\t\t\t#endif\r\n\r\n\t\t\tif ( removeBound( BC_idx,BT_TRUE ) != SUCCESSFUL_RETURN )\r\n\t\t\t\treturn THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );\r\n\r\n\t\t\ty[BC_idx] = 0.0;\r\n\t\t\tbreak;\r\n\r\n\r\n\t\t/* Add one variable to active set. */\r\n\t\tdefault:\r\n\t\t\t#ifdef PC_DEBUG\r\n\t\t\tif ( BC_status == ST_LOWER )\r\n\t\t\t\tsprintf( messageString,\"lower bound no. %d.\", BC_idx );\r\n\t\t\telse\r\n\t\t\t\tsprintf( messageString,\"upper bound no. %d.\", BC_idx );\r\n\t\t\t\tgetGlobalMessageHandler( )->throwInfo( RET_ADD_TO_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus );\r\n\t\t\t#endif\r\n\r\n\t\t\tif ( addBound( BC_idx,BC_status,BT_TRUE ) != SUCCESSFUL_RETURN )\r\n\t\t\t\treturn THROWERROR( RET_ADD_TO_ACTIVESET_FAILED );\r\n\t\t\tbreak;\r\n\t}\r\n\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n#ifdef PC_DEBUG /* Define print functions only for debugging! */\r\n\r\n/*\r\n *\tp r i n t I t e r a t i o n\r\n */\r\nreturnValue QProblemB::printIteration( \tint iteration,\r\n\t\t\t\t\t\t\t\t\t\tint BC_idx,\tSubjectToStatus BC_status\r\n\t\t \t\t\t\t\t\t\t\t)\r\n{\r\n\tchar myPrintfString[160];\r\n\r\n\t/* consistency check */\r\n\tif ( iteration < 0 )\r\n\t\treturn THROWERROR( RET_INVALID_ARGUMENTS );\r\n\r\n\t/* nothing to do */\r\n\tif ( printlevel != PL_MEDIUM )\r\n\t\treturn SUCCESSFUL_RETURN;\r\n\r\n\r\n\t/* 1) Print header at first iteration. */\r\n \tif ( iteration == 0 )\r\n\t{\r\n\t\tsprintf( myPrintfString,\"\\n############## qpOASES -- QP NO.%4.1d ###############\\n\", count );\r\n\t\tmyPrintf( myPrintfString );\r\n\r\n\t\tsprintf( myPrintfString,\" Iter | StepLength | Info | nFX \\n\" );\r\n\t\tmyPrintf( myPrintfString );\r\n\t}\r\n\r\n\t/* 2) Print iteration line. */\r\n\tif ( BC_status == ST_UNDEFINED )\r\n\t{\r\n\t\tsprintf( myPrintfString,\" %4.1d | %1.5e | QP SOLVED | %4.1d \\n\", iteration,tau,getNFX( ) );\r\n\t\tmyPrintf( myPrintfString );\r\n\t}\r\n\telse\r\n\t{\r\n\t\tchar info[8];\r\n\r\n\t\tif ( BC_status == ST_INACTIVE )\r\n\t\t\tsprintf( info,\"REM BND\" );\r\n\t\telse\r\n\t\t\tsprintf( info,\"ADD BND\" );\r\n\r\n\t\tsprintf( myPrintfString,\" %4.1d | %1.5e | %s%4.1d | %4.1d \\n\", iteration,tau,info,BC_idx,getNFX( ) );\r\n\t\tmyPrintf( myPrintfString );\r\n\t}\r\n\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n#endif /* PC_DEBUG */\r\n\r\n\r\n\r\n/*\r\n *\tc h e c k K K T c o n d i t i o n s\r\n */\r\nreturnValue QProblemB::checkKKTconditions( )\r\n{\r\n\t#ifdef __PERFORM_KKT_TEST__\r\n\r\n\tint i, j;\r\n\tint nV = getNV( );\r\n\r\n\treal_t tmp;\r\n\treal_t maxKKTviolation = 0.0;\r\n\r\n\r\n\t/* 1) Check for Hx + g - y*A' = 0 (here: A = Id). */\r\n\tfor( i=0; i maxKKTviolation )\r\n\t\t\tmaxKKTviolation = getAbs( tmp );\r\n\t}\r\n\r\n\t/* 2) Check for lb <= x <= ub. */\r\n\tfor( i=0; i maxKKTviolation )\r\n\t\t\tmaxKKTviolation = lb[i] - x[i];\r\n\r\n\t\tif ( x[i] - ub[i] > maxKKTviolation )\r\n\t\t\tmaxKKTviolation = x[i] - ub[i];\r\n\t}\r\n\r\n\t/* 3) Check for correct sign of y and for complementary slackness. */\r\n\tfor( i=0; i maxKKTviolation )\r\n\t\t\t\t\tmaxKKTviolation = -y[i];\r\n\t\t\t\tif ( getAbs( ( x[i] - lb[i] ) * y[i] ) > maxKKTviolation )\r\n\t\t\t\t\tmaxKKTviolation = getAbs( ( x[i] - lb[i] ) * y[i] );\r\n\t\t\t\tbreak;\r\n\r\n\t\t\tcase ST_UPPER:\r\n\t\t\t\tif ( y[i] > maxKKTviolation )\r\n\t\t\t\t\tmaxKKTviolation = y[i];\r\n\t\t\t\tif ( getAbs( ( ub[i] - x[i] ) * y[i] ) > maxKKTviolation )\r\n\t\t\t\t\tmaxKKTviolation = getAbs( ( ub[i] - x[i] ) * y[i] );\r\n\t\t\t\tbreak;\r\n\r\n\t\t\tdefault: /* inactive */\r\n\t\t\tif ( getAbs( y[i] ) > maxKKTviolation )\r\n\t\t\t\t\tmaxKKTviolation = getAbs( y[i] );\r\n\t\t\t\tbreak;\r\n\t\t}\r\n\t}\r\n\r\n\tif ( maxKKTviolation > CRITICALACCURACY )\r\n\t\treturn RET_NO_SOLUTION;\r\n\r\n\tif ( maxKKTviolation > DESIREDACCURACY )\r\n\t\treturn RET_INACCURATE_SOLUTION;\r\n\r\n\t#endif /* __PERFORM_KKT_TEST__ */\r\n\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n\r\n/*\r\n *\tend of file\r\n */\r\n"
},
{
"path": "externals/qpoases/SRC/QProblemB.ipp",
"content": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.\r\n *\r\n *\tqpOASES is free software; you can redistribute it and/or\r\n *\tmodify it under the terms of the GNU Lesser General Public\r\n *\tLicense as published by the Free Software Foundation; either\r\n *\tversion 2.1 of the License, or (at your option) any later version.\r\n *\r\n *\tqpOASES is distributed in the hope that it will be useful,\r\n *\tbut WITHOUT ANY WARRANTY; without even the implied warranty of\r\n *\tMERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU\r\n *\tLesser General Public License for more details.\r\n *\r\n *\tYou should have received a copy of the GNU Lesser General Public\r\n *\tLicense along with qpOASES; if not, write to the Free Software\r\n *\tFoundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA\r\n *\r\n */\r\n\r\n\r\n/**\r\n *\t\\file SRC/QProblemB.ipp\r\n *\t\\author Hans Joachim Ferreau\r\n *\t\\version 1.3embedded\r\n *\t\\date 2007-2008\r\n *\r\n *\tImplementation of inlined member functions of the QProblemB class which \r\n *\tis able to use the newly developed online active set strategy for \r\n *\tparametric quadratic programming.\r\n */\r\n\r\n\r\n\r\n#include \r\n\r\n\r\n\r\n/*****************************************************************************\r\n * P U B L I C *\r\n *****************************************************************************/\r\n\r\n/*\r\n *\tg e t H\r\n */\r\ninline returnValue QProblemB::getH( real_t* const _H ) const\r\n{\r\n\tint i;\r\n\r\n\tfor ( i=0; i= 0 ) && ( number < getNV( ) ) )\r\n\t{\r\n\t\tvalue = lb[number];\r\n\t\treturn SUCCESSFUL_RETURN;\r\n\t}\r\n\telse\r\n\t{\r\n\t\treturn THROWERROR( RET_INDEX_OUT_OF_BOUNDS );\r\n\t}\r\n}\r\n\r\n\r\n/*\r\n *\tg e t U B\r\n */\r\ninline returnValue QProblemB::getUB( real_t* const _ub ) const\r\n{\r\n\tint i;\r\n\r\n\tfor ( i=0; i= 0 ) && ( number < getNV( ) ) )\r\n\t{\r\n\t\tvalue = ub[number];\r\n\t\treturn SUCCESSFUL_RETURN;\r\n\t}\r\n\telse\r\n\t{\r\n\t\treturn THROWERROR( RET_INDEX_OUT_OF_BOUNDS );\r\n\t}\r\n}\r\n\r\n\r\n/*\r\n *\tg e t B o u n d s\r\n */\r\ninline returnValue QProblemB::getBounds( Bounds* const _bounds ) const\r\n{\r\n\t*_bounds = bounds;\r\n\t\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n/*\r\n *\tg e t N V\r\n */\r\ninline int QProblemB::getNV( ) const\r\n{\r\n\treturn bounds.getNV( );\r\n}\r\n\r\n\r\n/*\r\n *\tg e t N F R\r\n */\r\ninline int QProblemB::getNFR( )\r\n{\r\n\treturn bounds.getNFR( );\r\n}\r\n\r\n\r\n/*\r\n *\tg e t N F X\r\n */\r\ninline int QProblemB::getNFX( )\r\n{\r\n\treturn bounds.getNFX( );\r\n}\r\n\r\n\r\n/*\r\n *\tg e t N F V\r\n */\r\ninline int QProblemB::getNFV( ) const\r\n{\r\n\treturn bounds.getNFV( );\r\n}\r\n\r\n\r\n/*\r\n *\tg e t S t a t u s\r\n */\r\ninline QProblemStatus QProblemB::getStatus( ) const\r\n{\r\n\treturn status;\r\n}\r\n\r\n\r\n/*\r\n *\ti s I n i t i a l i s e d\r\n */\r\ninline BooleanType QProblemB::isInitialised( ) const\r\n{\r\n\tif ( status == QPS_NOTINITIALISED )\r\n\t\treturn BT_FALSE;\r\n\telse\r\n\t\treturn BT_TRUE;\r\n}\r\n\r\n\r\n/*\r\n *\ti s S o l v e d\r\n */\r\ninline BooleanType QProblemB::isSolved( ) const\r\n{\r\n\tif ( status == QPS_SOLVED )\r\n\t\treturn BT_TRUE;\r\n\telse\r\n\t\treturn BT_FALSE;\r\n}\r\n\r\n\r\n/*\r\n *\ti s I n f e a s i b l e\r\n */\r\ninline BooleanType QProblemB::isInfeasible( ) const\r\n{\r\n\treturn infeasible;\r\n}\r\n\r\n\r\n/*\r\n *\ti s U n b o u n d e d\r\n */\r\ninline BooleanType QProblemB::isUnbounded( ) const\r\n{\r\n\treturn unbounded;\r\n}\r\n\r\n\r\n/*\r\n *\tg e t P r i n t L e v e l\r\n */\r\ninline PrintLevel QProblemB::getPrintLevel( ) const\r\n{\r\n\treturn printlevel;\r\n}\r\n\r\n\r\n/*\r\n *\tg e t H e s s i a n T y p e\r\n */\r\ninline HessianType QProblemB::getHessianType( ) const\r\n{\r\n\treturn hessianType;\r\n}\r\n\r\n\r\n/*\r\n *\ts e t H e s s i a n T y p e\r\n */\r\ninline returnValue QProblemB::setHessianType( HessianType _hessianType )\r\n{\r\n\thessianType = _hessianType;\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n\r\n/*****************************************************************************\r\n * P R O T E C T E D *\r\n *****************************************************************************/\r\n\r\n/*\r\n *\ts e t H\r\n */\r\ninline returnValue QProblemB::setH( const real_t* const H_new )\r\n{\r\n\tint i, j;\r\n\r\n\tint nV = getNV();\r\n\r\n\tfor( i=0; i= 0 ) && ( number < getNV( ) ) )\r\n\t{\r\n\t\tlb[number] = value;\r\n\t\treturn SUCCESSFUL_RETURN;\r\n\t}\r\n\telse\r\n\t{\r\n\t\treturn THROWERROR( RET_INDEX_OUT_OF_BOUNDS );\r\n\t}\r\n}\r\n\r\n\r\n/*\r\n *\ts e t U B\r\n */\r\ninline returnValue QProblemB::setUB( const real_t* const ub_new )\r\n{\r\n\tint i;\r\n\r\n\tint nV = getNV();\r\n\r\n\tfor( i=0; i= 0 ) && ( number < getNV( ) ) )\r\n\t{\r\n\t\tub[number] = value;\r\n\r\n\t\treturn SUCCESSFUL_RETURN;\r\n\t}\r\n\telse\r\n\t{\r\n\t\treturn THROWERROR( RET_INDEX_OUT_OF_BOUNDS );\r\n\t}\r\n}\r\n\r\n\r\n/*\r\n *\tc o m p u t e G i v e n s\r\n */\r\ninline void QProblemB::computeGivens(\treal_t xold, real_t yold, real_t& xnew, real_t& ynew,\r\n\t\t\t\t\t\t\t\t\t\treal_t& c, real_t& s \r\n\t\t\t\t\t\t\t\t\t\t) const\r\n{\r\n if ( getAbs( yold ) <= ZERO )\r\n\t{\r\n c = 1.0;\r\n s = 0.0;\r\n\t\t\r\n\t\txnew = xold;\r\n\t\tynew = yold;\r\n\t}\r\n else\r\n\t{\r\n\t\treal_t t, mu;\r\n\r\n mu = getAbs( xold );\r\n\t\tif ( getAbs( yold ) > mu )\r\n\t\t\tmu = getAbs( yold );\r\n\t\t\r\n t = mu * sqrt( (xold/mu)*(xold/mu) + (yold/mu)*(yold/mu) );\r\n\t\t\r\n\t\tif ( xold < 0.0 )\r\n t = -t;\r\n\t\t\r\n c = xold/t;\r\n s = yold/t;\r\n xnew = t;\r\n ynew = 0.0;\r\n\t}\r\n\t\r\n\treturn;\r\n}\r\n\r\n\t\t\r\n/*\r\n *\ta p p l y G i v e n s\r\n */\r\ninline void QProblemB::applyGivens(\treal_t c, real_t s, real_t xold, real_t yold,\r\n\t\t\t\t\t\t\t\t\treal_t& xnew, real_t& ynew \r\n\t\t\t\t\t\t\t\t\t) const\r\n{\r\n\t/* Usual Givens plane rotation requiring four multiplications. */\r\n\txnew = c*xold + s*yold;\r\n\tynew = -s*xold + c*yold;\r\n// \tdouble nu = s/(1.0+c);\r\n// \r\n// \txnew = xold*c + yold*s;\r\n// \tynew = (xnew+xold)*nu - yold;\r\n\t\r\n\treturn;\r\n}\r\n\r\n\r\n/*\r\n *\tend of file\r\n */\r\n"
},
{
"path": "externals/qpoases/SRC/SubjectTo.cpp",
"content": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.\r\n *\r\n *\tqpOASES is free software; you can redistribute it and/or\r\n *\tmodify it under the terms of the GNU Lesser General Public\r\n *\tLicense as published by the Free Software Foundation; either\r\n *\tversion 2.1 of the License, or (at your option) any later version.\r\n *\r\n *\tqpOASES is distributed in the hope that it will be useful,\r\n *\tbut WITHOUT ANY WARRANTY; without even the implied warranty of\r\n *\tMERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU\r\n *\tLesser General Public License for more details.\r\n *\r\n *\tYou should have received a copy of the GNU Lesser General Public\r\n *\tLicense along with qpOASES; if not, write to the Free Software\r\n *\tFoundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA\r\n *\r\n */\r\n\r\n\r\n/**\r\n *\t\\file SRC/SubjectTo.cpp\r\n *\t\\author Hans Joachim Ferreau\r\n *\t\\version 1.3embedded\r\n *\t\\date 2007-2008\r\n *\r\n *\tImplementation of the SubjectTo class designed to manage working sets of\r\n *\tconstraints and bounds within a QProblem.\r\n */\r\n\r\n\r\n#include \r\n\r\n\r\n/*****************************************************************************\r\n * P U B L I C *\r\n *****************************************************************************/\r\n\r\n\r\n/*\r\n *\tS u b j e c t T o\r\n */\r\nSubjectTo::SubjectTo( ) :\tnoLower( BT_TRUE ),\r\n\t\t\t\t\t\t\tnoUpper( BT_TRUE ),\r\n\t\t\t\t\t\t\tsize( 0 )\r\n{\r\n\tint i;\r\n\r\n\tfor( i=0; iaddNumber( newnumber ) == RET_INDEXLIST_EXCEEDS_MAX_LENGTH )\r\n\t\treturn THROWERROR( RET_ADDINDEX_FAILED );\r\n\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n/*\r\n *\tr e m o v e I n d e x\r\n */\r\nreturnValue SubjectTo::removeIndex(\tIndexlist* const indexlist, \r\n\t\t\t\t\t\t\t\t\tint removenumber\r\n\t\t\t\t\t\t\t\t\t)\r\n{\r\n\tstatus[removenumber] = ST_UNDEFINED;\r\n\r\n\tif ( indexlist->removeNumber( removenumber ) != SUCCESSFUL_RETURN )\r\n\t\treturn THROWERROR( RET_UNKNOWN_BUG );\r\n\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n/*\r\n *\ts w a p I n d e x\r\n */\r\nreturnValue SubjectTo::swapIndex(\tIndexlist* const indexlist,\r\n\t\t\t\t\t\t\t\t\tint number1, int number2\r\n\t\t\t\t\t\t\t\t\t)\r\n{\r\n\t/* consistency checks */\r\n\tif ( status[number1] != status[number2] )\r\n\t\treturn THROWERROR( RET_SWAPINDEX_FAILED );\r\n\r\n\tif ( number1 == number2 )\r\n\t{\r\n\t\tTHROWWARNING( RET_NOTHING_TO_DO );\r\n\t\treturn SUCCESSFUL_RETURN;\r\n\t}\r\n\r\n\tif ( indexlist->swapNumbers( number1,number2 ) != SUCCESSFUL_RETURN )\r\n\t\treturn THROWERROR( RET_SWAPINDEX_FAILED );\r\n\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n/*\r\n *\tend of file\r\n */\r\n"
},
{
"path": "externals/qpoases/SRC/SubjectTo.ipp",
"content": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.\r\n *\r\n *\tqpOASES is free software; you can redistribute it and/or\r\n *\tmodify it under the terms of the GNU Lesser General Public\r\n *\tLicense as published by the Free Software Foundation; either\r\n *\tversion 2.1 of the License, or (at your option) any later version.\r\n *\r\n *\tqpOASES is distributed in the hope that it will be useful,\r\n *\tbut WITHOUT ANY WARRANTY; without even the implied warranty of\r\n *\tMERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU\r\n *\tLesser General Public License for more details.\r\n *\r\n *\tYou should have received a copy of the GNU Lesser General Public\r\n *\tLicense along with qpOASES; if not, write to the Free Software\r\n *\tFoundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA\r\n *\r\n */\r\n\r\n\r\n/**\r\n *\t\\file SRC/SubjectTo.ipp\r\n *\t\\author Hans Joachim Ferreau\r\n *\t\\version 1.3embedded\r\n *\t\\date 2007-2008\r\n *\r\n *\tImplementation of the inlined member functions of the SubjectTo class \r\n *\tdesigned to manage working sets of constraints and bounds within a QProblem.\r\n */\r\n\r\n\r\n/*****************************************************************************\r\n * P U B L I C *\r\n *****************************************************************************/\r\n \r\n\r\n/*\r\n *\tg e t T y p e\r\n */\r\ninline SubjectToType SubjectTo::getType( int i ) const\r\n{\r\n\tif ( ( i >= 0 ) && ( i < size ) )\r\n\t\treturn type[i];\r\n\telse\r\n\t\treturn ST_UNKNOWN;\r\n}\r\n\r\n\r\n/*\r\n *\tg e t S t a t u s\r\n */\r\ninline SubjectToStatus SubjectTo::getStatus( int i ) const\r\n{\r\n\tif ( ( i >= 0 ) && ( i < size ) )\r\n\t\treturn status[i];\r\n\telse\r\n\t\treturn ST_UNDEFINED;\r\n}\r\n\r\n\r\n/*\r\n *\ts e t T y p e\r\n */\r\ninline returnValue SubjectTo::setType( int i, SubjectToType value )\r\n{\r\n\tif ( ( i >= 0 ) && ( i < size ) )\r\n\t{\r\n\t\ttype[i] = value;\r\n\t\treturn SUCCESSFUL_RETURN;\r\n\t}\r\n\telse\r\n\t\treturn THROWERROR( RET_INDEX_OUT_OF_BOUNDS );\r\n}\r\n\r\n\r\n/*\r\n *\ts e t S t a t u s\r\n */\r\ninline returnValue SubjectTo::setStatus( int i, SubjectToStatus value )\r\n{\r\n\tif ( ( i >= 0 ) && ( i < size ) )\r\n\t{\r\n\t\tstatus[i] = value;\r\n\t\treturn SUCCESSFUL_RETURN;\r\n\t}\r\n\telse\r\n\t\treturn THROWERROR( RET_INDEX_OUT_OF_BOUNDS );\r\n}\r\n\r\n\r\n/*\r\n *\ts e t N o L o w e r\r\n */\r\ninline void SubjectTo::setNoLower( BooleanType _status )\r\n{\r\n\tnoLower = _status;\r\n}\r\n \r\n\r\n/*\r\n *\ts e t N o U p p e r\r\n */\r\ninline void SubjectTo::setNoUpper( BooleanType _status )\r\n{\r\n\tnoUpper = _status;\r\n}\r\n\r\n\r\n/*\r\n *\ti s N o L o w e r\r\n */\r\ninline BooleanType SubjectTo::isNoLower( ) const\r\n{\r\n\treturn noLower;\r\n}\r\n\r\n \r\n/*\r\n *\ti s N o L o w e r\r\n */\r\ninline BooleanType SubjectTo::isNoUpper( ) const\r\n{\r\n\treturn noUpper;\r\n}\r\n\r\n\r\n/*\r\n *\tend of file\r\n */\r\n"
},
{
"path": "externals/qpoases/SRC/Utils.cpp",
"content": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.\r\n *\r\n *\tqpOASES is free software; you can redistribute it and/or\r\n *\tmodify it under the terms of the GNU Lesser General Public\r\n *\tLicense as published by the Free Software Foundation; either\r\n *\tversion 2.1 of the License, or (at your option) any later version.\r\n *\r\n *\tqpOASES is distributed in the hope that it will be useful,\r\n *\tbut WITHOUT ANY WARRANTY; without even the implied warranty of\r\n *\tMERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU\r\n *\tLesser General Public License for more details.\r\n *\r\n *\tYou should have received a copy of the GNU Lesser General Public\r\n *\tLicense along with qpOASES; if not, write to the Free Software\r\n *\tFoundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA\r\n *\r\n */\r\n\r\n\r\n/**\r\n *\t\\file SRC/Utils.cpp\r\n *\t\\author Hans Joachim Ferreau, Eckhard Arnold\r\n *\t\\version 1.3embedded\r\n *\t\\date 2007-2008\r\n *\r\n *\tImplementation of some inlined utilities for working with the different QProblem\r\n * classes.\r\n */\r\n\r\n\r\n#include \r\n\r\n#if defined(__WIN32__) || defined(WIN32)\r\n #include \r\n#elif defined(LINUX)\r\n #include \r\n #include \r\n#endif\r\n\r\n#ifdef __MATLAB__\r\n #include \r\n#endif\r\n\r\n\r\n#include \r\n\r\n\r\n\r\n#ifdef PC_DEBUG /* Define print functions only for debugging! */\r\n/*\r\n *\tp r i n t\r\n */\r\nreturnValue print( const real_t* const v, int n )\r\n{\r\n\tint i;\r\n\tchar myPrintfString[160];\r\n\r\n\t/* Print a vector. */\r\n\tmyPrintf( \"[\\t\" );\r\n\tfor( i=0; igetOutputFile( );\r\n\tif ( outputfile == 0 )\r\n\t\treturn THROWERROR( RET_NO_GLOBAL_MESSAGE_OUTPUTFILE );\r\n\r\n\tfprintf( outputfile, \"%s\", s );\r\n\t#endif\r\n\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n/*\r\n *\tp r i n t C o p y r i g h t N o t i c e\r\n */\r\nreturnValue printCopyrightNotice( )\r\n{\r\n\treturn myPrintf( \"\\nqpOASES -- An Implementation of the Online Active Set Strategy.\\nCopyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.\\n\\nqpOASES is distributed under the terms of the \\nGNU Lesser General Public License 2.1 in the hope that it will be \\nuseful, but WITHOUT ANY WARRANTY; without even the implied warranty \\nof MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. \\nSee the GNU Lesser General Public License for more details.\\n\\n\" );\r\n}\r\n\r\n\r\n/*\r\n *\tr e a d F r o m F i l e\r\n */\r\nreturnValue readFromFile(\treal_t* data, int nrow, int ncol,\r\n\t\t\t\t\t\t\tconst char* datafilename\r\n\t\t\t\t\t\t\t)\r\n{\r\n\tint i, j;\r\n\tfloat float_data;\r\n\tmyFILE* datafile;\r\n\r\n\t/* 1) Open file. */\r\n\tif ( ( datafile = fopen( datafilename, \"r\" ) ) == 0 )\r\n\t{\r\n\t\tchar errstr[80];\r\n\t\tsprintf( errstr,\"(%s)\",datafilename );\r\n\t\treturn getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );\r\n\t}\r\n\r\n\t/* 2) Read data from file. */\r\n\tfor( i=0; ithrowError( RET_UNABLE_TO_READ_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );\r\n\t\t\t}\r\n\t\t\tdata[i*ncol + j] = ( (real_t) float_data );\r\n\t\t}\r\n\t}\r\n\r\n\t/* 3) Close file. */\r\n\tfclose( datafile );\r\n\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n/*\r\n *\tr e a d F r o m F i l e\r\n */\r\nreturnValue readFromFile(\treal_t* data, int n,\r\n\t\t\t\t\t\t\tconst char* datafilename\r\n\t\t\t\t\t\t\t)\r\n{\r\n\treturn readFromFile( data, n, 1, datafilename );\r\n}\r\n\r\n\r\n\r\n/*\r\n *\tr e a d F r o m F i l e\r\n */\r\nreturnValue readFromFile(\tint* data, int n,\r\n\t\t\t\t\t\t\tconst char* datafilename\r\n\t\t\t\t\t\t\t)\r\n{\r\n\tint i;\r\n\tmyFILE* datafile;\r\n\r\n\t/* 1) Open file. */\r\n\tif ( ( datafile = fopen( datafilename, \"r\" ) ) == 0 )\r\n\t{\r\n\t\tchar errstr[80];\r\n\t\tsprintf( errstr,\"(%s)\",datafilename );\r\n\t\treturn getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );\r\n\t}\r\n\r\n\t/* 2) Read data from file. */\r\n\tfor( i=0; ithrowError( RET_UNABLE_TO_READ_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );\r\n\t\t}\r\n\t}\r\n\r\n\t/* 3) Close file. */\r\n\tfclose( datafile );\r\n\r\n\treturn SUCCESSFUL_RETURN;\r\n}\r\n\r\n\r\n/*\r\n *\tw r i t e I n t o F i l e\r\n */\r\nreturnValue writeIntoFile(\tconst real_t* const data, int nrow, int ncol,\r\n\t\t\t\t\t\t\tconst char* datafilename, BooleanType append\r\n\t\t\t\t\t\t\t)\r\n{\r\n\tint i, j;\r\n\tmyFILE* datafile;\r\n\r\n\t/* 1) Open file. */\r\n\tif ( append == BT_TRUE )\r\n\t{\r\n\t\t/* append data */\r\n\t\tif ( ( datafile = fopen( datafilename, \"a\" ) ) == 0 )\r\n\t\t{\r\n\t\t\tchar errstr[80];\r\n\t\t\tsprintf( errstr,\"(%s)\",datafilename );\r\n\t\t\treturn getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );\r\n\t\t}\r\n\t}\r\n\telse\r\n\t{\r\n\t\t/* do not append data */\r\n\t\tif ( ( datafile = fopen( datafilename, \"w\" ) ) == 0 )\r\n\t\t{\r\n\t\t\tchar errstr[80];\r\n\t\t\tsprintf( errstr,\"(%s)\",datafilename );\r\n\t\t\treturn getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );\r\n\t\t}\r\n\t}\r\n\r\n\t/* 2) Write data into file. */\r\n\tfor( i=0; ithrowError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );\r\n\t\t}\r\n\t}\r\n\telse\r\n\t{\r\n\t\t/* do not append data */\r\n\t\tif ( ( datafile = fopen( datafilename, \"w\" ) ) == 0 )\r\n\t\t{\r\n\t\t\tchar errstr[80];\r\n\t\t\tsprintf( errstr,\"(%s)\",datafilename );\r\n\t\t\treturn getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );\r\n\t\t}\r\n\t}\r\n\r\n\t/* 2) Write data into file. */\r\n\tfor( i=0; i.\n *\n */\n\n\n#pragma once\n\n#include \n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \"rpg_mpc/mpc_wrapper.h\"\n#include \"rpg_mpc/mpc_params.h\"\n\nnamespace rpg_mpc {\n\nenum STATE {\n kPosX = 0,\n kPosY = 1,\n kPosZ = 2,\n kOriW = 3,\n kOriX = 4,\n kOriY = 5,\n kOriZ = 6,\n kVelX = 7,\n kVelY = 8,\n kVelZ = 9\n};\n\nenum INPUT {\n kThrust = 0,\n kRateX = 1,\n kRateY = 2,\n kRateZ = 3\n};\n\ntemplate\nclass MpcController {\npublic:\n\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n static_assert(kStateSize == 10,\n \"MpcController: Wrong model size. Number of states does not match.\");\n static_assert(kInputSize == 4,\n \"MpcController: Wrong model size. Number of inputs does not match.\");\n\n MpcController(const ros::NodeHandle& nh,\n const ros::NodeHandle& pnh,\n const std::string& topic = \"mpc/trajectory_predicted\");\n\n MpcController() : MpcController(ros::NodeHandle(), ros::NodeHandle(\"~\")) {}\n\n quadrotor_common::ControlCommand off();\n\n quadrotor_common::ControlCommand run(\n const quadrotor_common::QuadStateEstimate& state_estimate,\n const quadrotor_common::Trajectory& reference_trajectory,\n const MpcParams& params);\n\n\nprivate:\n // Internal helper functions.\n\n void pointOfInterestCallback(\n const geometry_msgs::PointStamped::ConstPtr& msg);\n\n void offCallback(const std_msgs::Empty::ConstPtr& msg);\n\n bool setStateEstimate(\n const quadrotor_common::QuadStateEstimate& state_estimate);\n\n bool setReference(const quadrotor_common::Trajectory& reference_trajectory);\n\n quadrotor_common::ControlCommand updateControlCommand(\n const Eigen::Ref> state,\n const Eigen::Ref> input,\n ros::Time& time);\n\n bool publishPrediction(\n const Eigen::Ref> states,\n const Eigen::Ref> inputs,\n ros::Time& time);\n\n void preparationThread();\n\n bool setNewParams(MpcParams& params);\n\n // Handles\n ros::NodeHandle nh_;\n ros::NodeHandle pnh_;\n\n // Subscribers and publisher.\n ros::Subscriber sub_point_of_interest_;\n ros::Subscriber sub_autopilot_off_;\n ros::Publisher pub_predicted_trajectory_;\n\n // Parameters\n MpcParams params_;\n\n // MPC\n MpcWrapper mpc_wrapper_;\n\n // Preparation Thread\n std::thread preparation_thread_;\n\n // Variables\n T timing_feedback_, timing_preparation_;\n bool solve_from_scratch_;\n Eigen::Matrix est_state_;\n Eigen::Matrix reference_states_;\n Eigen::Matrix reference_inputs_;\n Eigen::Matrix predicted_states_;\n Eigen::Matrix predicted_inputs_;\n Eigen::Matrix point_of_interest_;\n};\n\n\n} // namespace MPC\n"
},
{
"path": "include/rpg_mpc/mpc_params.h",
"content": "/* rpg_quadrotor_mpc\n * A model predictive control implementation for quadrotors.\n * Copyright (C) 2017-2018 Philipp Foehn, \n * Robotics and Perception Group, University of Zurich\n * \n * Intended to be used with rpg_quadrotor_control and rpg_quadrotor_common.\n * https://github.com/uzh-rpg/rpg_quadrotor_control\n *\n * This program is free software: you can redistribute it and/or modify\n * it under the terms of the GNU General Public License as published by\n * the Free Software Foundation, either version 3 of the License, or\n * (at your option) any later version.\n *\n * This program is distributed in the hope that it will be useful,\n * but WITHOUT ANY WARRANTY; without even the implied warranty of\n * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n * GNU General Public License for more details.\n *\n * You should have received a copy of the GNU General Public License\n * along with this program. If not, see .\n *\n */\n\n\n#pragma once\n\n#include \n#include \n\n#include \"quadrotor_common/parameter_helper.h\"\n\nnamespace rpg_mpc\n{\n\ntemplate \nclass MpcParams {\n public:\n\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n \n MpcParams() :\n changed_(false),\n print_info_(false),\n state_cost_exponential_(0.0),\n input_cost_exponential_(0.0),\n max_bodyrate_xy_(0.0),\n max_bodyrate_z_(0.0),\n min_thrust_(0.0),\n max_thrust_(0.0),\n p_B_C_(Eigen::Matrix::Zero()),\n q_B_C_(Eigen::Quaternion(1.0, 0.0, 0.0, 0.0)),\n Q_(Eigen::Matrix::Zero()),\n R_(Eigen::Matrix::Zero())\n {\n }\n\n ~MpcParams()\n {\n }\n\n bool loadParameters(ros::NodeHandle& pnh)\n {\n #define GET_PARAM(name) \\\n if (!quadrotor_common::getParam(#name, name, pnh)) \\\n return false\n\n #define GET_PARAM_(name) \\\n if (!quadrotor_common::getParam(#name, name ## _, pnh)) \\\n return false\n\n // Read state costs.\n T Q_pos_xy, Q_pos_z, Q_attitude, Q_velocity, Q_perception;\n GET_PARAM(Q_pos_xy);\n GET_PARAM(Q_pos_z);\n GET_PARAM(Q_attitude);\n GET_PARAM(Q_velocity);\n quadrotor_common::getParam(\"Q_perception\", Q_perception, (T)0.0, pnh);\n\n // Check whether all state costs are positive.\n if(Q_pos_xy <= 0.0 ||\n Q_pos_z <= 0.0 ||\n Q_attitude <= 0.0 ||\n Q_velocity <= 0.0 ||\n Q_perception < 0.0) // Perception cost can be zero to deactivate.\n {\n ROS_ERROR(\"MPC: State cost Q has negative enries!\");\n return false;\n }\n\n // Read input costs.\n T R_thrust, R_pitchroll, R_yaw;\n GET_PARAM(R_thrust);\n GET_PARAM(R_pitchroll);\n GET_PARAM(R_yaw);\n\n // Check whether all input costs are positive.\n if(R_thrust <= 0.0 ||\n R_pitchroll <= 0.0 ||\n R_yaw <= 0.0)\n {\n ROS_ERROR(\"MPC: Input cost R has negative enries!\");\n return false;\n }\n\n // Set state and input cost matrices.\n Q_ = (Eigen::Matrix() <<\n Q_pos_xy, Q_pos_xy, Q_pos_z,\n Q_attitude, Q_attitude, Q_attitude, Q_attitude,\n Q_velocity, Q_velocity, Q_velocity,\n Q_perception, Q_perception).finished().asDiagonal();\n R_ = (Eigen::Matrix() <<\n R_thrust, R_pitchroll, R_pitchroll, R_yaw).finished().asDiagonal();\n\n // Read cost scaling values\n quadrotor_common::getParam(\"state_cost_exponential\",\n state_cost_exponential_, (T)0.0, pnh);\n quadrotor_common::getParam(\"input_cost_exponential\",\n input_cost_exponential_, (T)0.0, pnh);\n\n // Read input limits.\n GET_PARAM_(max_bodyrate_xy);\n GET_PARAM_(max_bodyrate_z);\n GET_PARAM_(min_thrust);\n GET_PARAM_(max_thrust);\n\n // Check whether all input limits are positive.\n if(max_bodyrate_xy_ <= 0.0 ||\n max_bodyrate_z_ <= 0.0 ||\n min_thrust_ <= 0.0 ||\n max_thrust_ <= 0.0)\n {\n ROS_ERROR(\"MPC: All limits must be positive non-zero values!\");\n return false;\n }\n\n // Optional parameters\n std::vector p_B_C(3), q_B_C(4);\n if(!pnh.getParam(\"p_B_C\", p_B_C))\n {\n ROS_WARN(\"MPC: Camera extrinsic translation is not set.\");\n }\n else\n {\n p_B_C_ = Eigen::Matrix(p_B_C[0], p_B_C[1], p_B_C[2]);\n }\n if(!pnh.getParam(\"q_B_C\", q_B_C))\n {\n ROS_WARN(\"MPC: Camera extrinsic rotation is not set.\");\n }\n else\n {\n q_B_C_ = Eigen::Quaternion(q_B_C[0], q_B_C[1], q_B_C[2], q_B_C[3]);\n }\n\n quadrotor_common::getParam(\"print_info\", print_info_, false, pnh);\n if(print_info_) ROS_INFO(\"MPC: Informative printing enabled.\");\n\n changed_ = true;\n\n #undef GET_PARAM\n #undef GET_PARAM_OPT\n #undef GET_PARAM_\n #undef GET_PARAM_OPT_\n\n return true;\n }\n\n bool changed_;\n\n bool print_info_;\n\n T state_cost_exponential_;\n T input_cost_exponential_;\n\n T max_bodyrate_xy_;\n T max_bodyrate_z_;\n T min_thrust_;\n T max_thrust_;\n\n Eigen::Matrix p_B_C_;\n Eigen::Quaternion q_B_C_;\n\n Eigen::Matrix Q_;\n Eigen::Matrix R_;\n};\n\n\n\n} // namespace rpg_mpc"
},
{
"path": "include/rpg_mpc/mpc_wrapper.h",
"content": "/* rpg_quadrotor_mpc\n * A model predictive control implementation for quadrotors.\n * Copyright (C) 2017-2018 Philipp Foehn, \n * Robotics and Perception Group, University of Zurich\n * \n * Intended to be used with rpg_quadrotor_control and rpg_quadrotor_common.\n * https://github.com/uzh-rpg/rpg_quadrotor_control\n *\n * This program is free software: you can redistribute it and/or modify\n * it under the terms of the GNU General Public License as published by\n * the Free Software Foundation, either version 3 of the License, or\n * (at your option) any later version.\n *\n * This program is distributed in the hope that it will be useful,\n * but WITHOUT ANY WARRANTY; without even the implied warranty of\n * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n * GNU General Public License for more details.\n *\n * You should have received a copy of the GNU General Public License\n * along with this program. If not, see .\n *\n */\n\n\n#pragma once\n\n#include \n#include \n\nnamespace rpg_mpc {\n\n#include \"acado_auxiliary_functions.h\"\n#include \"acado_common.h\"\n\nstatic constexpr int kSamples = ACADO_N; // number of samples\nstatic constexpr int kStateSize = ACADO_NX; // number of states\nstatic constexpr int kRefSize = ACADO_NY; // number of reference states\nstatic constexpr int kEndRefSize = ACADO_NYN; // number of end reference states\nstatic constexpr int kInputSize = ACADO_NU; // number of inputs\nstatic constexpr int kCostSize = ACADO_NY - ACADO_NU; // number of state costs\nstatic constexpr int kOdSize = ACADO_NOD; // number of online data\n\nACADOvariables acadoVariables;\nACADOworkspace acadoWorkspace;\n\n\ntemplate \nclass MpcWrapper\n{\n public:\n\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n MpcWrapper();\n MpcWrapper(\n const Eigen::Ref> Q,\n const Eigen::Ref> R);\n\n bool setCosts(\n const Eigen::Ref> Q,\n const Eigen::Ref> R,\n const T state_cost_scaling = 0.0, const T input_cost_scaling = 0.0);\n\n bool setLimits(T min_thrust, T max_thrust,\n T max_rollpitchrate, T max_yawrate);\n bool setCameraParameters(\n const Eigen::Ref>& p_B_C,\n Eigen::Quaternion& q_B_C);\n bool setPointOfInterest(\n const Eigen::Ref>& position);\n\n bool setReferencePose(\n const Eigen::Ref> state);\n bool setTrajectory(\n const Eigen::Ref> states,\n const Eigen::Ref> inputs);\n\n bool solve(const Eigen::Ref> state);\n bool update(const Eigen::Ref> state,\n bool do_preparation = true);\n bool prepare();\n\n void getState(const int node_index,\n Eigen::Ref> return_state);\n void getStates(\n Eigen::Ref> return_states);\n void getInput(const int node_index,\n Eigen::Ref> return_input);\n void getInputs(\n Eigen::Ref> return_input);\n T getTimestep() { return dt_; }\n\n private:\n Eigen::Map>\n acado_reference_states_{acadoVariables.y};\n\n Eigen::Map>\n acado_reference_end_state_{acadoVariables.yN};\n\n Eigen::Map>\n acado_initial_state_{acadoVariables.x0};\n\n Eigen::Map>\n acado_states_{acadoVariables.x};\n\n Eigen::Map>\n acado_inputs_{acadoVariables.u};\n\n Eigen::Map>\n acado_online_data_{acadoVariables.od};\n\n Eigen::Map