Showing preview only (1,116K chars total). Download the full file or copy to clipboard to get everything.
Repository: uzh-rpg/rpg_mpc
Branch: master
Commit: 33263db20910
Files: 62
Total size: 1.1 MB
Directory structure:
gitextract_l80w6cpf/
├── .gitignore
├── CMakeLists.txt
├── COPYING
├── README.md
├── externals/
│ └── qpoases/
│ ├── EXAMPLES/
│ │ ├── example1.cpp
│ │ └── example1b.cpp
│ ├── INCLUDE/
│ │ ├── Bounds.hpp
│ │ ├── Constants.hpp
│ │ ├── Constraints.hpp
│ │ ├── CyclingManager.hpp
│ │ ├── EXTRAS/
│ │ │ └── SolutionAnalysis.hpp
│ │ ├── Indexlist.hpp
│ │ ├── MessageHandling.hpp
│ │ ├── QProblem.hpp
│ │ ├── QProblemB.hpp
│ │ ├── SubjectTo.hpp
│ │ ├── Types.hpp
│ │ └── Utils.hpp
│ ├── LICENSE.txt
│ ├── README.txt
│ ├── SRC/
│ │ ├── Bounds.cpp
│ │ ├── Bounds.ipp
│ │ ├── Constraints.cpp
│ │ ├── Constraints.ipp
│ │ ├── CyclingManager.cpp
│ │ ├── CyclingManager.ipp
│ │ ├── EXTRAS/
│ │ │ └── SolutionAnalysis.cpp
│ │ ├── Indexlist.cpp
│ │ ├── Indexlist.ipp
│ │ ├── MessageHandling.cpp
│ │ ├── MessageHandling.ipp
│ │ ├── QProblem.cpp
│ │ ├── QProblem.ipp
│ │ ├── QProblemB.cpp
│ │ ├── QProblemB.ipp
│ │ ├── SubjectTo.cpp
│ │ ├── SubjectTo.ipp
│ │ ├── Utils.cpp
│ │ └── Utils.ipp
│ └── VERSIONS.txt
├── include/
│ └── rpg_mpc/
│ ├── mpc_controller.h
│ ├── mpc_params.h
│ └── mpc_wrapper.h
├── launch/
│ └── mpc_controller.launch
├── model/
│ ├── CMakeLists.txt
│ ├── FindACADO.cmake
│ ├── README.md
│ ├── quadrotor_model_codegen
│ ├── quadrotor_model_thrustrates.cpp
│ └── quadrotor_mpc_codegen/
│ ├── acado_auxiliary_functions.c
│ ├── acado_auxiliary_functions.h
│ ├── acado_common.h
│ ├── acado_integrator.c
│ ├── acado_qpoases_interface.cpp
│ ├── acado_qpoases_interface.hpp
│ └── acado_solver.c
├── package.xml
├── parameters/
│ └── default.yaml
├── src/
│ ├── autopilot_mpc_instance.cpp
│ ├── mpc_controller.cpp
│ └── mpc_wrapper.cpp
└── test/
└── mpc_node.cpp
================================================
FILE CONTENTS
================================================
================================================
FILE: .gitignore
================================================
# CLion stuff
.idea/
cmake-build-debug/
================================================
FILE: CMakeLists.txt
================================================
# rpg_quadrotor_mpc
# A model predictive control implementation for quadrotors.
# Copyright (C) 2017-2018 Philipp Foehn,
# Robotics and Perception Group, University of Zurich
#
# Intended to be used with rpg_quadrotor_control and rpg_quadrotor_common.
# https://github.com/uzh-rpg/rpg_quadrotor_control
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <http://www.gnu.org/licenses/>.
cmake_minimum_required(VERSION 2.8.3)
project(rpg_mpc)
find_package(catkin_simple REQUIRED)
catkin_simple()
# activate c++ 11
IF(CMAKE_COMPILER_IS_GNUCC)
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
ELSE()
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
ENDIF()
# ARM NEON flags
if("${CMAKE_HOST_SYSTEM_PROCESSOR}" STREQUAL "armv7l")
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -march=armv7-a -mfpu=neon -mfloat-abi=hard -funsafe-math-optimizations")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=armv7-a -mfpu=neon -mfloat-abi=hard -funsafe-math-optimizations")
message("enabling ARM neon optimizations")
endif()
# flags for speed (should already be enabled by default)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra")
set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -g")
set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -fopenmp -O3")
cs_add_library(mpc_solver
externals/qpoases/SRC/Bounds.cpp
externals/qpoases/SRC/Constraints.cpp
externals/qpoases/SRC/CyclingManager.cpp
externals/qpoases/SRC/Indexlist.cpp
externals/qpoases/SRC/MessageHandling.cpp
externals/qpoases/SRC/QProblem.cpp
externals/qpoases/SRC/QProblemB.cpp
externals/qpoases/SRC/SubjectTo.cpp
externals/qpoases/SRC/Utils.cpp
externals/qpoases/SRC/EXTRAS/SolutionAnalysis.cpp
model/quadrotor_mpc_codegen/acado_qpoases_interface.cpp
model/quadrotor_mpc_codegen/acado_integrator.c
model/quadrotor_mpc_codegen/acado_solver.c
model/quadrotor_mpc_codegen/acado_auxiliary_functions.c)
target_include_directories(mpc_solver PUBLIC
model/quadrotor_mpc_codegen/
externals/qpoases
externals/qpoases/INCLUDE
externals/qpoases/SRC)
cs_add_library(mpc_wrapper
src/mpc_wrapper.cpp)
target_link_libraries(mpc_wrapper
mpc_solver)
cs_add_library(mpc_controller
src/mpc_controller.cpp)
target_link_libraries(mpc_controller
mpc_wrapper)
# make an executable
cs_install()
cs_export()
cs_add_executable(mpc_controller_node
test/mpc_node.cpp)
target_link_libraries(mpc_controller_node
mpc_controller
mpc_wrapper
mpc_solver)
if(catkin_LIBRARIES MATCHES "autopilot")
message("Building with MPC Autopilot")
cs_add_executable(autopilot_mpc_instance
src/autopilot_mpc_instance.cpp)
target_link_libraries(autopilot_mpc_instance
mpc_controller
mpc_wrapper
mpc_solver)
endif(catkin_LIBRARIES MATCHES "autopilot")
cs_install()
cs_export()
================================================
FILE: COPYING
================================================
GNU GENERAL PUBLIC LICENSE
Version 3, 29 June 2007
Copyright (C) 2007 Free Software Foundation, Inc. <https://fsf.org/>
Everyone is permitted to copy and distribute verbatim copies
of this license document, but changing it is not allowed.
Preamble
The GNU General Public License is a free, copyleft license for
software and other kinds of works.
The licenses for most software and other practical works are designed
to take away your freedom to share and change the works. By contrast,
the GNU General Public License is intended to guarantee your freedom to
share and change all versions of a program--to make sure it remains free
software for all its users. We, the Free Software Foundation, use the
GNU General Public License for most of our software; it applies also to
any other work released this way by its authors. You can apply it to
your programs, too.
When we speak of free software, we are referring to freedom, not
price. Our General Public Licenses are designed to make sure that you
have the freedom to distribute copies of free software (and charge for
them if you wish), that you receive source code or can get it if you
want it, that you can change the software or use pieces of it in new
free programs, and that you know you can do these things.
To protect your rights, we need to prevent others from denying you
these rights or asking you to surrender the rights. Therefore, you have
certain responsibilities if you distribute copies of the software, or if
you modify it: responsibilities to respect the freedom of others.
For example, if you distribute copies of such a program, whether
gratis or for a fee, you must pass on to the recipients the same
freedoms that you received. You must make sure that they, too, receive
or can get the source code. And you must show them these terms so they
know their rights.
Developers that use the GNU GPL protect your rights with two steps:
(1) assert copyright on the software, and (2) offer you this License
giving you legal permission to copy, distribute and/or modify it.
For the developers' and authors' protection, the GPL clearly explains
that there is no warranty for this free software. For both users' and
authors' sake, the GPL requires that modified versions be marked as
changed, so that their problems will not be attributed erroneously to
authors of previous versions.
Some devices are designed to deny users access to install or run
modified versions of the software inside them, although the manufacturer
can do so. This is fundamentally incompatible with the aim of
protecting users' freedom to change the software. The systematic
pattern of such abuse occurs in the area of products for individuals to
use, which is precisely where it is most unacceptable. Therefore, we
have designed this version of the GPL to prohibit the practice for those
products. If such problems arise substantially in other domains, we
stand ready to extend this provision to those domains in future versions
of the GPL, as needed to protect the freedom of users.
Finally, every program is threatened constantly by software patents.
States should not allow patents to restrict development and use of
software on general-purpose computers, but in those that do, we wish to
avoid the special danger that patents applied to a free program could
make it effectively proprietary. To prevent this, the GPL assures that
patents cannot be used to render the program non-free.
The precise terms and conditions for copying, distribution and
modification follow.
TERMS AND CONDITIONS
0. Definitions.
"This License" refers to version 3 of the GNU General Public License.
"Copyright" also means copyright-like laws that apply to other kinds of
works, such as semiconductor masks.
"The Program" refers to any copyrightable work licensed under this
License. Each licensee is addressed as "you". "Licensees" and
"recipients" may be individuals or organizations.
To "modify" a work means to copy from or adapt all or part of the work
in a fashion requiring copyright permission, other than the making of an
exact copy. The resulting work is called a "modified version" of the
earlier work or a work "based on" the earlier work.
A "covered work" means either the unmodified Program or a work based
on the Program.
To "propagate" a work means to do anything with it that, without
permission, would make you directly or secondarily liable for
infringement under applicable copyright law, except executing it on a
computer or modifying a private copy. Propagation includes copying,
distribution (with or without modification), making available to the
public, and in some countries other activities as well.
To "convey" a work means any kind of propagation that enables other
parties to make or receive copies. Mere interaction with a user through
a computer network, with no transfer of a copy, is not conveying.
An interactive user interface displays "Appropriate Legal Notices"
to the extent that it includes a convenient and prominently visible
feature that (1) displays an appropriate copyright notice, and (2)
tells the user that there is no warranty for the work (except to the
extent that warranties are provided), that licensees may convey the
work under this License, and how to view a copy of this License. If
the interface presents a list of user commands or options, such as a
menu, a prominent item in the list meets this criterion.
1. Source Code.
The "source code" for a work means the preferred form of the work
for making modifications to it. "Object code" means any non-source
form of a work.
A "Standard Interface" means an interface that either is an official
standard defined by a recognized standards body, or, in the case of
interfaces specified for a particular programming language, one that
is widely used among developers working in that language.
The "System Libraries" of an executable work include anything, other
than the work as a whole, that (a) is included in the normal form of
packaging a Major Component, but which is not part of that Major
Component, and (b) serves only to enable use of the work with that
Major Component, or to implement a Standard Interface for which an
implementation is available to the public in source code form. A
"Major Component", in this context, means a major essential component
(kernel, window system, and so on) of the specific operating system
(if any) on which the executable work runs, or a compiler used to
produce the work, or an object code interpreter used to run it.
The "Corresponding Source" for a work in object code form means all
the source code needed to generate, install, and (for an executable
work) run the object code and to modify the work, including scripts to
control those activities. However, it does not include the work's
System Libraries, or general-purpose tools or generally available free
programs which are used unmodified in performing those activities but
which are not part of the work. For example, Corresponding Source
includes interface definition files associated with source files for
the work, and the source code for shared libraries and dynamically
linked subprograms that the work is specifically designed to require,
such as by intimate data communication or control flow between those
subprograms and other parts of the work.
The Corresponding Source need not include anything that users
can regenerate automatically from other parts of the Corresponding
Source.
The Corresponding Source for a work in source code form is that
same work.
2. Basic Permissions.
All rights granted under this License are granted for the term of
copyright on the Program, and are irrevocable provided the stated
conditions are met. This License explicitly affirms your unlimited
permission to run the unmodified Program. The output from running a
covered work is covered by this License only if the output, given its
content, constitutes a covered work. This License acknowledges your
rights of fair use or other equivalent, as provided by copyright law.
You may make, run and propagate covered works that you do not
convey, without conditions so long as your license otherwise remains
in force. You may convey covered works to others for the sole purpose
of having them make modifications exclusively for you, or provide you
with facilities for running those works, provided that you comply with
the terms of this License in conveying all material for which you do
not control copyright. Those thus making or running the covered works
for you must do so exclusively on your behalf, under your direction
and control, on terms that prohibit them from making any copies of
your copyrighted material outside their relationship with you.
Conveying under any other circumstances is permitted solely under
the conditions stated below. Sublicensing is not allowed; section 10
makes it unnecessary.
3. Protecting Users' Legal Rights From Anti-Circumvention Law.
No covered work shall be deemed part of an effective technological
measure under any applicable law fulfilling obligations under article
11 of the WIPO copyright treaty adopted on 20 December 1996, or
similar laws prohibiting or restricting circumvention of such
measures.
When you convey a covered work, you waive any legal power to forbid
circumvention of technological measures to the extent such circumvention
is effected by exercising rights under this License with respect to
the covered work, and you disclaim any intention to limit operation or
modification of the work as a means of enforcing, against the work's
users, your or third parties' legal rights to forbid circumvention of
technological measures.
4. Conveying Verbatim Copies.
You may convey verbatim copies of the Program's source code as you
receive it, in any medium, provided that you conspicuously and
appropriately publish on each copy an appropriate copyright notice;
keep intact all notices stating that this License and any
non-permissive terms added in accord with section 7 apply to the code;
keep intact all notices of the absence of any warranty; and give all
recipients a copy of this License along with the Program.
You may charge any price or no price for each copy that you convey,
and you may offer support or warranty protection for a fee.
5. Conveying Modified Source Versions.
You may convey a work based on the Program, or the modifications to
produce it from the Program, in the form of source code under the
terms of section 4, provided that you also meet all of these conditions:
a) The work must carry prominent notices stating that you modified
it, and giving a relevant date.
b) The work must carry prominent notices stating that it is
released under this License and any conditions added under section
7. This requirement modifies the requirement in section 4 to
"keep intact all notices".
c) You must license the entire work, as a whole, under this
License to anyone who comes into possession of a copy. This
License will therefore apply, along with any applicable section 7
additional terms, to the whole of the work, and all its parts,
regardless of how they are packaged. This License gives no
permission to license the work in any other way, but it does not
invalidate such permission if you have separately received it.
d) If the work has interactive user interfaces, each must display
Appropriate Legal Notices; however, if the Program has interactive
interfaces that do not display Appropriate Legal Notices, your
work need not make them do so.
A compilation of a covered work with other separate and independent
works, which are not by their nature extensions of the covered work,
and which are not combined with it such as to form a larger program,
in or on a volume of a storage or distribution medium, is called an
"aggregate" if the compilation and its resulting copyright are not
used to limit the access or legal rights of the compilation's users
beyond what the individual works permit. Inclusion of a covered work
in an aggregate does not cause this License to apply to the other
parts of the aggregate.
6. Conveying Non-Source Forms.
You may convey a covered work in object code form under the terms
of sections 4 and 5, provided that you also convey the
machine-readable Corresponding Source under the terms of this License,
in one of these ways:
a) Convey the object code in, or embodied in, a physical product
(including a physical distribution medium), accompanied by the
Corresponding Source fixed on a durable physical medium
customarily used for software interchange.
b) Convey the object code in, or embodied in, a physical product
(including a physical distribution medium), accompanied by a
written offer, valid for at least three years and valid for as
long as you offer spare parts or customer support for that product
model, to give anyone who possesses the object code either (1) a
copy of the Corresponding Source for all the software in the
product that is covered by this License, on a durable physical
medium customarily used for software interchange, for a price no
more than your reasonable cost of physically performing this
conveying of source, or (2) access to copy the
Corresponding Source from a network server at no charge.
c) Convey individual copies of the object code with a copy of the
written offer to provide the Corresponding Source. This
alternative is allowed only occasionally and noncommercially, and
only if you received the object code with such an offer, in accord
with subsection 6b.
d) Convey the object code by offering access from a designated
place (gratis or for a charge), and offer equivalent access to the
Corresponding Source in the same way through the same place at no
further charge. You need not require recipients to copy the
Corresponding Source along with the object code. If the place to
copy the object code is a network server, the Corresponding Source
may be on a different server (operated by you or a third party)
that supports equivalent copying facilities, provided you maintain
clear directions next to the object code saying where to find the
Corresponding Source. Regardless of what server hosts the
Corresponding Source, you remain obligated to ensure that it is
available for as long as needed to satisfy these requirements.
e) Convey the object code using peer-to-peer transmission, provided
you inform other peers where the object code and Corresponding
Source of the work are being offered to the general public at no
charge under subsection 6d.
A separable portion of the object code, whose source code is excluded
from the Corresponding Source as a System Library, need not be
included in conveying the object code work.
A "User Product" is either (1) a "consumer product", which means any
tangible personal property which is normally used for personal, family,
or household purposes, or (2) anything designed or sold for incorporation
into a dwelling. In determining whether a product is a consumer product,
doubtful cases shall be resolved in favor of coverage. For a particular
product received by a particular user, "normally used" refers to a
typical or common use of that class of product, regardless of the status
of the particular user or of the way in which the particular user
actually uses, or expects or is expected to use, the product. A product
is a consumer product regardless of whether the product has substantial
commercial, industrial or non-consumer uses, unless such uses represent
the only significant mode of use of the product.
"Installation Information" for a User Product means any methods,
procedures, authorization keys, or other information required to install
and execute modified versions of a covered work in that User Product from
a modified version of its Corresponding Source. The information must
suffice to ensure that the continued functioning of the modified object
code is in no case prevented or interfered with solely because
modification has been made.
If you convey an object code work under this section in, or with, or
specifically for use in, a User Product, and the conveying occurs as
part of a transaction in which the right of possession and use of the
User Product is transferred to the recipient in perpetuity or for a
fixed term (regardless of how the transaction is characterized), the
Corresponding Source conveyed under this section must be accompanied
by the Installation Information. But this requirement does not apply
if neither you nor any third party retains the ability to install
modified object code on the User Product (for example, the work has
been installed in ROM).
The requirement to provide Installation Information does not include a
requirement to continue to provide support service, warranty, or updates
for a work that has been modified or installed by the recipient, or for
the User Product in which it has been modified or installed. Access to a
network may be denied when the modification itself materially and
adversely affects the operation of the network or violates the rules and
protocols for communication across the network.
Corresponding Source conveyed, and Installation Information provided,
in accord with this section must be in a format that is publicly
documented (and with an implementation available to the public in
source code form), and must require no special password or key for
unpacking, reading or copying.
7. Additional Terms.
"Additional permissions" are terms that supplement the terms of this
License by making exceptions from one or more of its conditions.
Additional permissions that are applicable to the entire Program shall
be treated as though they were included in this License, to the extent
that they are valid under applicable law. If additional permissions
apply only to part of the Program, that part may be used separately
under those permissions, but the entire Program remains governed by
this License without regard to the additional permissions.
When you convey a copy of a covered work, you may at your option
remove any additional permissions from that copy, or from any part of
it. (Additional permissions may be written to require their own
removal in certain cases when you modify the work.) You may place
additional permissions on material, added by you to a covered work,
for which you have or can give appropriate copyright permission.
Notwithstanding any other provision of this License, for material you
add to a covered work, you may (if authorized by the copyright holders of
that material) supplement the terms of this License with terms:
a) Disclaiming warranty or limiting liability differently from the
terms of sections 15 and 16 of this License; or
b) Requiring preservation of specified reasonable legal notices or
author attributions in that material or in the Appropriate Legal
Notices displayed by works containing it; or
c) Prohibiting misrepresentation of the origin of that material, or
requiring that modified versions of such material be marked in
reasonable ways as different from the original version; or
d) Limiting the use for publicity purposes of names of licensors or
authors of the material; or
e) Declining to grant rights under trademark law for use of some
trade names, trademarks, or service marks; or
f) Requiring indemnification of licensors and authors of that
material by anyone who conveys the material (or modified versions of
it) with contractual assumptions of liability to the recipient, for
any liability that these contractual assumptions directly impose on
those licensors and authors.
All other non-permissive additional terms are considered "further
restrictions" within the meaning of section 10. If the Program as you
received it, or any part of it, contains a notice stating that it is
governed by this License along with a term that is a further
restriction, you may remove that term. If a license document contains
a further restriction but permits relicensing or conveying under this
License, you may add to a covered work material governed by the terms
of that license document, provided that the further restriction does
not survive such relicensing or conveying.
If you add terms to a covered work in accord with this section, you
must place, in the relevant source files, a statement of the
additional terms that apply to those files, or a notice indicating
where to find the applicable terms.
Additional terms, permissive or non-permissive, may be stated in the
form of a separately written license, or stated as exceptions;
the above requirements apply either way.
8. Termination.
You may not propagate or modify a covered work except as expressly
provided under this License. Any attempt otherwise to propagate or
modify it is void, and will automatically terminate your rights under
this License (including any patent licenses granted under the third
paragraph of section 11).
However, if you cease all violation of this License, then your
license from a particular copyright holder is reinstated (a)
provisionally, unless and until the copyright holder explicitly and
finally terminates your license, and (b) permanently, if the copyright
holder fails to notify you of the violation by some reasonable means
prior to 60 days after the cessation.
Moreover, your license from a particular copyright holder is
reinstated permanently if the copyright holder notifies you of the
violation by some reasonable means, this is the first time you have
received notice of violation of this License (for any work) from that
copyright holder, and you cure the violation prior to 30 days after
your receipt of the notice.
Termination of your rights under this section does not terminate the
licenses of parties who have received copies or rights from you under
this License. If your rights have been terminated and not permanently
reinstated, you do not qualify to receive new licenses for the same
material under section 10.
9. Acceptance Not Required for Having Copies.
You are not required to accept this License in order to receive or
run a copy of the Program. Ancillary propagation of a covered work
occurring solely as a consequence of using peer-to-peer transmission
to receive a copy likewise does not require acceptance. However,
nothing other than this License grants you permission to propagate or
modify any covered work. These actions infringe copyright if you do
not accept this License. Therefore, by modifying or propagating a
covered work, you indicate your acceptance of this License to do so.
10. Automatic Licensing of Downstream Recipients.
Each time you convey a covered work, the recipient automatically
receives a license from the original licensors, to run, modify and
propagate that work, subject to this License. You are not responsible
for enforcing compliance by third parties with this License.
An "entity transaction" is a transaction transferring control of an
organization, or substantially all assets of one, or subdividing an
organization, or merging organizations. If propagation of a covered
work results from an entity transaction, each party to that
transaction who receives a copy of the work also receives whatever
licenses to the work the party's predecessor in interest had or could
give under the previous paragraph, plus a right to possession of the
Corresponding Source of the work from the predecessor in interest, if
the predecessor has it or can get it with reasonable efforts.
You may not impose any further restrictions on the exercise of the
rights granted or affirmed under this License. For example, you may
not impose a license fee, royalty, or other charge for exercise of
rights granted under this License, and you may not initiate litigation
(including a cross-claim or counterclaim in a lawsuit) alleging that
any patent claim is infringed by making, using, selling, offering for
sale, or importing the Program or any portion of it.
11. Patents.
A "contributor" is a copyright holder who authorizes use under this
License of the Program or a work on which the Program is based. The
work thus licensed is called the contributor's "contributor version".
A contributor's "essential patent claims" are all patent claims
owned or controlled by the contributor, whether already acquired or
hereafter acquired, that would be infringed by some manner, permitted
by this License, of making, using, or selling its contributor version,
but do not include claims that would be infringed only as a
consequence of further modification of the contributor version. For
purposes of this definition, "control" includes the right to grant
patent sublicenses in a manner consistent with the requirements of
this License.
Each contributor grants you a non-exclusive, worldwide, royalty-free
patent license under the contributor's essential patent claims, to
make, use, sell, offer for sale, import and otherwise run, modify and
propagate the contents of its contributor version.
In the following three paragraphs, a "patent license" is any express
agreement or commitment, however denominated, not to enforce a patent
(such as an express permission to practice a patent or covenant not to
sue for patent infringement). To "grant" such a patent license to a
party means to make such an agreement or commitment not to enforce a
patent against the party.
If you convey a covered work, knowingly relying on a patent license,
and the Corresponding Source of the work is not available for anyone
to copy, free of charge and under the terms of this License, through a
publicly available network server or other readily accessible means,
then you must either (1) cause the Corresponding Source to be so
available, or (2) arrange to deprive yourself of the benefit of the
patent license for this particular work, or (3) arrange, in a manner
consistent with the requirements of this License, to extend the patent
license to downstream recipients. "Knowingly relying" means you have
actual knowledge that, but for the patent license, your conveying the
covered work in a country, or your recipient's use of the covered work
in a country, would infringe one or more identifiable patents in that
country that you have reason to believe are valid.
If, pursuant to or in connection with a single transaction or
arrangement, you convey, or propagate by procuring conveyance of, a
covered work, and grant a patent license to some of the parties
receiving the covered work authorizing them to use, propagate, modify
or convey a specific copy of the covered work, then the patent license
you grant is automatically extended to all recipients of the covered
work and works based on it.
A patent license is "discriminatory" if it does not include within
the scope of its coverage, prohibits the exercise of, or is
conditioned on the non-exercise of one or more of the rights that are
specifically granted under this License. You may not convey a covered
work if you are a party to an arrangement with a third party that is
in the business of distributing software, under which you make payment
to the third party based on the extent of your activity of conveying
the work, and under which the third party grants, to any of the
parties who would receive the covered work from you, a discriminatory
patent license (a) in connection with copies of the covered work
conveyed by you (or copies made from those copies), or (b) primarily
for and in connection with specific products or compilations that
contain the covered work, unless you entered into that arrangement,
or that patent license was granted, prior to 28 March 2007.
Nothing in this License shall be construed as excluding or limiting
any implied license or other defenses to infringement that may
otherwise be available to you under applicable patent law.
12. No Surrender of Others' Freedom.
If conditions are imposed on you (whether by court order, agreement or
otherwise) that contradict the conditions of this License, they do not
excuse you from the conditions of this License. If you cannot convey a
covered work so as to satisfy simultaneously your obligations under this
License and any other pertinent obligations, then as a consequence you may
not convey it at all. For example, if you agree to terms that obligate you
to collect a royalty for further conveying from those to whom you convey
the Program, the only way you could satisfy both those terms and this
License would be to refrain entirely from conveying the Program.
13. Use with the GNU Affero General Public License.
Notwithstanding any other provision of this License, you have
permission to link or combine any covered work with a work licensed
under version 3 of the GNU Affero General Public License into a single
combined work, and to convey the resulting work. The terms of this
License will continue to apply to the part which is the covered work,
but the special requirements of the GNU Affero General Public License,
section 13, concerning interaction through a network will apply to the
combination as such.
14. Revised Versions of this License.
The Free Software Foundation may publish revised and/or new versions of
the GNU General Public License from time to time. Such new versions will
be similar in spirit to the present version, but may differ in detail to
address new problems or concerns.
Each version is given a distinguishing version number. If the
Program specifies that a certain numbered version of the GNU General
Public License "or any later version" applies to it, you have the
option of following the terms and conditions either of that numbered
version or of any later version published by the Free Software
Foundation. If the Program does not specify a version number of the
GNU General Public License, you may choose any version ever published
by the Free Software Foundation.
If the Program specifies that a proxy can decide which future
versions of the GNU General Public License can be used, that proxy's
public statement of acceptance of a version permanently authorizes you
to choose that version for the Program.
Later license versions may give you additional or different
permissions. However, no additional obligations are imposed on any
author or copyright holder as a result of your choosing to follow a
later version.
15. Disclaimer of Warranty.
THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY
APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT
HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY
OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO,
THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM
IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF
ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
16. Limitation of Liability.
IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS
THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY
GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE
USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF
DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD
PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS),
EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF
SUCH DAMAGES.
17. Interpretation of Sections 15 and 16.
If the disclaimer of warranty and limitation of liability provided
above cannot be given local legal effect according to their terms,
reviewing courts shall apply local law that most closely approximates
an absolute waiver of all civil liability in connection with the
Program, unless a warranty or assumption of liability accompanies a
copy of the Program in return for a fee.
END OF TERMS AND CONDITIONS
How to Apply These Terms to Your New Programs
If you develop a new program, and you want it to be of the greatest
possible use to the public, the best way to achieve this is to make it
free software which everyone can redistribute and change under these terms.
To do so, attach the following notices to the program. It is safest
to attach them to the start of each source file to most effectively
state the exclusion of warranty; and each file should have at least
the "copyright" line and a pointer to where the full notice is found.
<one line to give the program's name and a brief idea of what it does.>
Copyright (C) <year> <name of author>
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <https://www.gnu.org/licenses/>.
Also add information on how to contact you by electronic and paper mail.
If the program does terminal interaction, make it output a short
notice like this when it starts in an interactive mode:
<program> Copyright (C) <year> <name of author>
This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
This is free software, and you are welcome to redistribute it
under certain conditions; type `show c' for details.
The hypothetical commands `show w' and `show c' should show the appropriate
parts of the General Public License. Of course, your program's commands
might be different; for a GUI interface, you would use an "about box".
You should also get your employer (if you work as a programmer) or school,
if any, to sign a "copyright disclaimer" for the program, if necessary.
For more information on this, and how to apply and follow the GNU GPL, see
<https://www.gnu.org/licenses/>.
The GNU General Public License does not permit incorporating your program
into proprietary programs. If your program is a subroutine library, you
may consider it more useful to permit linking proprietary applications with
the library. If this is what you want to do, use the GNU Lesser General
Public License instead of this License. But first, please read
<https://www.gnu.org/licenses/why-not-lgpl.html>.
================================================
FILE: README.md
================================================
# Model Predictive Control for Quadrotors with extension to Perception-Aware MPC
Model 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.
This MPC is intended to be used with https://github.com/uzh-rpg/rpg_quadrotor_control.
It is available with the extension to be used as a "Perception Aware Model Predictive Controller" (**PAMPC**).
[**Check out our YouTube-Video, showing PAMPC in Action**](https://www.youtube.com/watch?v=9vaj829vE18)
[](https://www.youtube.com/watch?v=9vaj829vE18)
## Publication
If you use this code in an academic context, please cite the following [IROS 2018 paper](http://rpg.ifi.uzh.ch/docs/IROS18_Falanga.pdf).
Davide Falanga, Philipp Foehn, Peng Lu, Davide Scaramuzza: **PAMPC: Perception-Aware Model Predictive Control for Quadrotors**, IEEE/RSJ Int. Conf. Intell. Robot. Syst. (IROS), 2018.
```
@InProceedings{Falanga2018
author = {Falanga, Davide and Foehn, Philipp and Lu, Peng and Scaramuzza, Davide},
title = {{PAMPC}: Perception-Aware Model Predictive Control for Quadrotors},
booktitle = {IEEE/RSJ Int. Conf. Intell. Robot. Syst. (IROS)},
year = {2018}
}
```
## License
Copyright (C) 2017-2018 Philipp Foehn, Robotics and Perception Group, University of Zurich
The 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/).
This code has been tested with ROS kinetic on Ubuntu 16.04.
This is research code, expect that it changes often and any fitness for a particular purpose is disclaimed.
For a commercial license, please contact [Davide Scaramuzza](http://rpg.ifi.uzh.ch/people_scaramuzza.html).
```
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
```
This 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.
ACADO uses qpOASES by Hans Joachim Ferreau et al., released under GPL v2.1.
## Installation, Usage, and Documentation
For detailed instructions on installation, usage, and documentation, please head to our [Wiki](../../wiki).
Quick Links:
- [Installation](../../wiki/Installation)
- [Basic Usage](../../wiki/Basic-Usage)
- [Code Structure](../../wiki/Code-Structure)
- [License](../../wiki/License)
## Code Structure Overview
The whole MPC is based on ACADO (http://acado.github.io/).
ACADO'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.
The code is organized as follows:
### Solver `mpc_solver`
The auto-generated model, transcription, and solver is built as a library called `mpc_solver`.
This library consist of purely auto-generated code with nomenclature, code-style and structure as used in ACADO.
### Wrapper `mpc_wrapper`
To wrap this into a standard interface, the library `mpc_wrapper` is used.
ACADO 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.
* It is written to be compatible even with changing model descriptions in the `mpc_solver`.
* 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.
### Controller `mpc_controller`
To 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:
* **Embedded**: The mpc_controller can be included in any oder controller class or copilot by generating an object with the default constructor "MPC::MpcController<T> 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.
* **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<double> controller(ros::NodeHandle(), ros::NodeHandle("~"));` as in `test/control_node.cpp` and `launch/mpc_controller.launch`.
================================================
FILE: externals/qpoases/EXAMPLES/example1.cpp
================================================
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file EXAMPLES/example1.cpp
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*
* Very simple example for testing qpOASES (using QProblem class).
*/
#include <QProblem.hpp>
/** Example for qpOASES main function using the QProblem class. */
int main( )
{
/* Setup data of first QP. */
real_t H[2*2] = { 1.0, 0.0, 0.0, 0.5 };
real_t A[1*2] = { 1.0, 1.0 };
real_t g[2] = { 1.5, 1.0 };
real_t lb[2] = { 0.5, -2.0 };
real_t ub[2] = { 5.0, 2.0 };
real_t lbA[1] = { -1.0 };
real_t ubA[1] = { 2.0 };
/* Setup data of second QP. */
real_t g_new[2] = { 1.0, 1.5 };
real_t lb_new[2] = { 0.0, -1.0 };
real_t ub_new[2] = { 5.0, -0.5 };
real_t lbA_new[1] = { -2.0 };
real_t ubA_new[1] = { 1.0 };
/* Setting up QProblem object. */
QProblem example( 2,1 );
/* Solve first QP. */
int nWSR = 10;
example.init( H,g,A,lb,ub,lbA,ubA, nWSR,0 );
/* Solve second QP. */
nWSR = 10;
example.hotstart( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,0 );
return 0;
}
/*
* end of file
*/
================================================
FILE: externals/qpoases/EXAMPLES/example1b.cpp
================================================
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file EXAMPLES/example1b.cpp
* \author Hans Joachim Ferreau
* \version 1.3
* \date 2007-2008
*
* Very simple example for testing qpOASES using the QProblemB class.
*/
#include <QProblemB.hpp>
/** Example for qpOASES main function using the QProblemB class. */
int main( )
{
/* Setup data of first QP. */
real_t H[2*2] = { 1.0, 0.0, 0.0, 0.5 };
real_t g[2] = { 1.5, 1.0 };
real_t lb[2] = { 0.5, -2.0 };
real_t ub[2] = { 5.0, 2.0 };
/* Setup data of second QP. */
real_t g_new[2] = { 1.0, 1.5 };
real_t lb_new[2] = { 0.0, -1.0 };
real_t ub_new[2] = { 5.0, -0.5 };
/* Setting up QProblemB object. */
QProblemB example( 2 );
/* Solve first QP. */
int nWSR = 10;
example.init( H,g,lb,ub, nWSR,0 );
/* Solve second QP. */
nWSR = 10;
example.hotstart( g_new,lb_new,ub_new, nWSR,0 );
return 0;
}
/*
* end of file
*/
================================================
FILE: externals/qpoases/INCLUDE/Bounds.hpp
================================================
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file INCLUDE/Bounds.hpp
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*
* Declaration of the Bounds class designed to manage working sets of
* bounds within a QProblem.
*/
#ifndef QPOASES_BOUNDS_HPP
#define QPOASES_BOUNDS_HPP
#include <SubjectTo.hpp>
/** This class manages working sets of bounds by storing
* index sets and other status information.
*
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*/
class Bounds : public SubjectTo
{
/*
* PUBLIC MEMBER FUNCTIONS
*/
public:
/** Default constructor. */
Bounds( );
/** Copy constructor (deep copy). */
Bounds( const Bounds& rhs /**< Rhs object. */
);
/** Destructor. */
~Bounds( );
/** Assignment operator (deep copy). */
Bounds& operator=( const Bounds& rhs /**< Rhs object. */
);
/** Pseudo-constructor takes the number of bounds.
* \return SUCCESSFUL_RETURN */
returnValue init( int n /**< Number of bounds. */
);
/** Initially adds number of a new (i.e. not yet in the list) bound to
* given index set.
* \return SUCCESSFUL_RETURN \n
RET_SETUP_BOUND_FAILED \n
RET_INDEX_OUT_OF_BOUNDS \n
RET_INVALID_ARGUMENTS */
returnValue setupBound( int _number, /**< Number of new bound. */
SubjectToStatus _status /**< Status of new bound. */
);
/** Initially adds all numbers of new (i.e. not yet in the list) bounds to
* to the index set of free bounds; the order depends on the SujectToType
* of each index.
* \return SUCCESSFUL_RETURN \n
RET_SETUP_BOUND_FAILED */
returnValue setupAllFree( );
/** Moves index of a bound from index list of fixed to that of free bounds.
* \return SUCCESSFUL_RETURN \n
RET_MOVING_BOUND_FAILED \n
RET_INDEX_OUT_OF_BOUNDS */
returnValue moveFixedToFree( int _number /**< Number of bound to be freed. */
);
/** Moves index of a bound from index list of free to that of fixed bounds.
* \return SUCCESSFUL_RETURN \n
RET_MOVING_BOUND_FAILED \n
RET_INDEX_OUT_OF_BOUNDS */
returnValue moveFreeToFixed( int _number, /**< Number of bound to be fixed. */
SubjectToStatus _status /**< Status of bound to be fixed. */
);
/** Swaps the indices of two free bounds within the index set.
* \return SUCCESSFUL_RETURN \n
RET_SWAPINDEX_FAILED */
returnValue swapFree( int number1, /**< Number of first constraint or bound. */
int number2 /**< Number of second constraint or bound. */
);
/** Returns number of variables.
* \return Number of variables. */
inline int getNV( ) const;
/** Returns number of implicitly fixed variables.
* \return Number of implicitly fixed variables. */
inline int getNFV( ) const;
/** Returns number of bounded (but possibly free) variables.
* \return Number of bounded (but possibly free) variables. */
inline int getNBV( ) const;
/** Returns number of unbounded variables.
* \return Number of unbounded variables. */
inline int getNUV( ) const;
/** Sets number of implicitly fixed variables.
* \return SUCCESSFUL_RETURN */
inline returnValue setNFV( int n /**< Number of implicitly fixed variables. */
);
/** Sets number of bounded (but possibly free) variables.
* \return SUCCESSFUL_RETURN */
inline returnValue setNBV( int n /**< Number of bounded (but possibly free) variables. */
);
/** Sets number of unbounded variables.
* \return SUCCESSFUL_RETURN */
inline returnValue setNUV( int n /**< Number of unbounded variables */
);
/** Returns number of free variables.
* \return Number of free variables. */
inline int getNFR( );
/** Returns number of fixed variables.
* \return Number of fixed variables. */
inline int getNFX( );
/** Returns a pointer to free variables index list.
* \return Pointer to free variables index list. */
inline Indexlist* getFree( );
/** Returns a pointer to fixed variables index list.
* \return Pointer to fixed variables index list. */
inline Indexlist* getFixed( );
/*
* PROTECTED MEMBER VARIABLES
*/
protected:
int nV; /**< Number of variables (nV = nFV + nBV + nUV). */
int nFV; /**< Number of implicitly fixed variables. */
int nBV; /**< Number of bounded (but possibly free) variables. */
int nUV; /**< Number of unbounded variables. */
Indexlist free; /**< Index list of free variables. */
Indexlist fixed; /**< Index list of fixed variables. */
};
#include <Bounds.ipp>
#endif /* QPOASES_BOUNDS_HPP */
/*
* end of file
*/
================================================
FILE: externals/qpoases/INCLUDE/Constants.hpp
================================================
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file INCLUDE/Constants.hpp
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2008
*
* Definition of all global constants.
*/
#ifndef QPOASES_CONSTANTS_HPP
#define QPOASES_CONSTANTS_HPP
#ifndef QPOASES_CUSTOM_INTERFACE
#include "acado_qpoases_interface.hpp"
#else
#define XSTR(x) #x
#define STR(x) XSTR(x)
#include STR(QPOASES_CUSTOM_INTERFACE)
#endif
/** Maximum number of variables within a QP formulation.
Note: this value has to be positive! */
const int NVMAX = QPOASES_NVMAX;
/** Maximum number of constraints within a QP formulation.
Note: this value has to be positive! */
const int NCMAX = QPOASES_NCMAX;
/** Redefinition of NCMAX used for memory allocation, to avoid zero sized arrays
and compiler errors. */
const int NCMAX_ALLOC = (NCMAX == 0) ? 1 : NCMAX;
/**< Maximum number of working set recalculations.
Note: this value has to be positive! */
const int NWSRMAX = QPOASES_NWSRMAX;
/** Desired KKT tolerance of QP solution; a warning RET_INACCURATE_SOLUTION is
* issued if this tolerance is not met.
* Note: this value has to be positive! */
const real_t DESIREDACCURACY = (real_t) 1.0e-3;
/** Critical KKT tolerance of QP solution; an error is issued if this
* tolerance is not met.
* Note: this value has to be positive! */
const real_t CRITICALACCURACY = (real_t) 1.0e-2;
/** Numerical value of machine precision (min eps, s.t. 1+eps > 1).
Note: this value has to be positive! */
const real_t EPS = (real_t) QPOASES_EPS;
/** Numerical value of zero (for situations in which it would be
* unreasonable to compare with 0.0).
* Note: this value has to be positive! */
const real_t ZERO = (real_t) 1.0e-50;
/** Numerical value of infinity (e.g. for non-existing bounds).
* Note: this value has to be positive! */
const real_t INFTY = (real_t) 1.0e12;
/** Lower/upper (constraints') bound tolerance (an inequality constraint
* whose lower and upper bound differ by less than BOUNDTOL is regarded
* to be an equality constraint).
* Note: this value has to be positive! */
const real_t BOUNDTOL = (real_t) 1.0e-10;
/** Offset for relaxing (constraints') bounds at beginning of an initial homotopy.
* Note: this value has to be positive! */
const real_t BOUNDRELAXATION = (real_t) 1.0e3;
/** Factor that determines physical lengths of index lists.
* Note: this value has to be greater than 1! */
const int INDEXLISTFACTOR = 5;
#endif /* QPOASES_CONSTANTS_HPP */
/*
* end of file
*/
================================================
FILE: externals/qpoases/INCLUDE/Constraints.hpp
================================================
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file INCLUDE/Constraints.hpp
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*
* Declaration of the Constraints class designed to manage working sets of
* constraints within a QProblem.
*/
#ifndef QPOASES_CONSTRAINTS_HPP
#define QPOASES_CONSTRAINTS_HPP
#include <SubjectTo.hpp>
/** This class manages working sets of constraints by storing
* index sets and other status information.
*
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*/
class Constraints : public SubjectTo
{
/*
* PUBLIC MEMBER FUNCTIONS
*/
public:
/** Default constructor. */
Constraints( );
/** Copy constructor (deep copy). */
Constraints( const Constraints& rhs /**< Rhs object. */
);
/** Destructor. */
~Constraints( );
/** Assignment operator (deep copy). */
Constraints& operator=( const Constraints& rhs /**< Rhs object. */
);
/** Pseudo-constructor takes the number of constraints.
* \return SUCCESSFUL_RETURN */
returnValue init( int n /**< Number of constraints. */
);
/** Initially adds number of a new (i.e. not yet in the list) constraint to
* a given index set.
* \return SUCCESSFUL_RETURN \n
RET_SETUP_CONSTRAINT_FAILED \n
RET_INDEX_OUT_OF_BOUNDS \n
RET_INVALID_ARGUMENTS */
returnValue setupConstraint( int _number, /**< Number of new constraint. */
SubjectToStatus _status /**< Status of new constraint. */
);
/** Initially adds all enabled numbers of new (i.e. not yet in the list) constraints to
* to the index set of inactive constraints; the order depends on the SujectToType
* of each index. Only disabled constraints are added to index set of disabled constraints!
* \return SUCCESSFUL_RETURN \n
RET_SETUP_CONSTRAINT_FAILED */
returnValue setupAllInactive( );
/** Moves index of a constraint from index list of active to that of inactive constraints.
* \return SUCCESSFUL_RETURN \n
RET_MOVING_CONSTRAINT_FAILED */
returnValue moveActiveToInactive( int _number /**< Number of constraint to become inactive. */
);
/** Moves index of a constraint from index list of inactive to that of active constraints.
* \return SUCCESSFUL_RETURN \n
RET_MOVING_CONSTRAINT_FAILED */
returnValue moveInactiveToActive( int _number, /**< Number of constraint to become active. */
SubjectToStatus _status /**< Status of constraint to become active. */
);
/** Returns the number of constraints.
* \return Number of constraints. */
inline int getNC( ) const;
/** Returns the number of implicit equality constraints.
* \return Number of implicit equality constraints. */
inline int getNEC( ) const;
/** Returns the number of "real" inequality constraints.
* \return Number of "real" inequality constraints. */
inline int getNIC( ) const;
/** Returns the number of unbounded constraints (i.e. without any bounds).
* \return Number of unbounded constraints (i.e. without any bounds). */
inline int getNUC( ) const;
/** Sets number of implicit equality constraints.
* \return SUCCESSFUL_RETURN */
inline returnValue setNEC( int n /**< Number of implicit equality constraints. */
);
/** Sets number of "real" inequality constraints.
* \return SUCCESSFUL_RETURN */
inline returnValue setNIC( int n /**< Number of "real" inequality constraints. */
);
/** Sets number of unbounded constraints (i.e. without any bounds).
* \return SUCCESSFUL_RETURN */
inline returnValue setNUC( int n /**< Number of unbounded constraints (i.e. without any bounds). */
);
/** Returns the number of active constraints.
* \return Number of constraints. */
inline int getNAC( );
/** Returns the number of inactive constraints.
* \return Number of constraints. */
inline int getNIAC( );
/** Returns a pointer to active constraints index list.
* \return Pointer to active constraints index list. */
inline Indexlist* getActive( );
/** Returns a pointer to inactive constraints index list.
* \return Pointer to inactive constraints index list. */
inline Indexlist* getInactive( );
/*
* PROTECTED MEMBER VARIABLES
*/
protected:
int nC; /**< Number of constraints (nC = nEC + nIC + nUC). */
int nEC; /**< Number of implicit equality constraints. */
int nIC; /**< Number of "real" inequality constraints. */
int nUC; /**< Number of unbounded constraints (i.e. without any bounds). */
Indexlist active; /**< Index list of active constraints. */
Indexlist inactive; /**< Index list of inactive constraints. */
};
#include <Constraints.ipp>
#endif /* QPOASES_CONSTRAINTS_HPP */
/*
* end of file
*/
================================================
FILE: externals/qpoases/INCLUDE/CyclingManager.hpp
================================================
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file INCLUDE/CyclingManager.hpp
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*
* Declaration of the CyclingManager class designed to detect
* and handle possible cycling during QP iterations.
*/
#ifndef QPOASES_CYCLINGMANAGER_HPP
#define QPOASES_CYCLINGMANAGER_HPP
#include <Utils.hpp>
/** This class is intended to detect and handle possible cycling during QP iterations.
* As cycling seems to occur quite rarely, this class is NOT FULLY IMPLEMENTED YET!
*
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*/
class CyclingManager
{
/*
* PUBLIC MEMBER FUNCTIONS
*/
public:
/** Default constructor. */
CyclingManager( );
/** Copy constructor (deep copy). */
CyclingManager( const CyclingManager& rhs /**< Rhs object. */
);
/** Destructor. */
~CyclingManager( );
/** Copy asingment operator (deep copy). */
CyclingManager& operator=( const CyclingManager& rhs /**< Rhs object. */
);
/** Pseudo-constructor which takes the number of bounds/constraints.
* \return SUCCESSFUL_RETURN */
returnValue init( int _nV, /**< Number of bounds to be managed. */
int _nC /**< Number of constraints to be managed. */
);
/** Stores index of a bound/constraint that might cause cycling.
* \return SUCCESSFUL_RETURN \n
RET_INDEX_OUT_OF_BOUNDS */
returnValue setCyclingStatus( int number, /**< Number of bound/constraint. */
BooleanType isBound, /**< Flag that indicates if given number corresponds to a
* bound (BT_TRUE) or a constraint (BT_FALSE). */
CyclingStatus _status /**< Cycling status of bound/constraint. */
);
/** Returns if bound/constraint might cause cycling.
* \return BT_TRUE: bound/constraint might cause cycling \n
BT_FALSE: otherwise */
CyclingStatus getCyclingStatus( int number, /**< Number of bound/constraint. */
BooleanType isBound /**< Flag that indicates if given number corresponds to
* a bound (BT_TRUE) or a constraint (BT_FALSE). */
) const;
/** Clears all previous cycling information.
* \return SUCCESSFUL_RETURN */
returnValue clearCyclingData( );
/** Returns if cycling was detected.
* \return BT_TRUE iff cycling was detected. */
inline BooleanType isCyclingDetected( ) const;
/*
* PROTECTED MEMBER VARIABLES
*/
protected:
int nV; /**< Number of managed bounds. */
int nC; /**< Number of managed constraints. */
CyclingStatus status[NVMAX+NCMAX]; /**< Array to store cycling status of all bounds/constraints. */
BooleanType cyclingDetected; /**< Flag if cycling was detected. */
};
#include <CyclingManager.ipp>
#endif /* QPOASES_CYCLINGMANAGER_HPP */
/*
* end of file
*/
================================================
FILE: externals/qpoases/INCLUDE/EXTRAS/SolutionAnalysis.hpp
================================================
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file INCLUDE/EXTRAS/SolutionAnalysis.hpp
* \author Milan Vukov, Boris Houska, Hans Joachim Ferreau
* \version 1.3embedded
* \date 2012
*
* Solution analysis class, based on a class in the standard version of the qpOASES
*/
//
#ifndef QPOASES_SOLUTIONANALYSIS_HPP
#define QPOASES_SOLUTIONANALYSIS_HPP
#include <QProblem.hpp>
/** Enables the computation of variance as is in the standard version of qpOASES */
#define QPOASES_USE_OLD_VERSION 0
#if QPOASES_USE_OLD_VERSION
#define KKT_DIM (2 * NVMAX + NCMAX)
#endif
class SolutionAnalysis
{
public:
/** Default constructor. */
SolutionAnalysis( );
/** Copy constructor (deep copy). */
SolutionAnalysis( const SolutionAnalysis& rhs /**< Rhs object. */
);
/** Destructor. */
~SolutionAnalysis( );
/** Copy asingment operator (deep copy). */
SolutionAnalysis& operator=( const SolutionAnalysis& rhs /**< Rhs object. */
);
/** A routine for computation of inverse of the Hessian matrix. */
returnValue getHessianInverse(
QProblem* qp, /** QP */
real_t* hessianInverse /** Inverse of the Hessian matrix*/
);
/** A routine for computation of inverse of the Hessian matrix. */
returnValue getHessianInverse( QProblemB* qp, /** QP */
real_t* hessianInverse /** Inverse of the Hessian matrix*/
);
#if QPOASES_USE_OLD_VERSION
returnValue getVarianceCovariance(
QProblem* qp,
real_t* g_b_bA_VAR,
real_t* Primal_Dual_VAR
);
#endif
private:
real_t delta_g_cov[ NVMAX ]; /** A covariance-vector of g */
real_t delta_lb_cov[ NVMAX ]; /** A covariance-vector of lb */
real_t delta_ub_cov[ NVMAX ]; /** A covariance-vector of ub */
real_t delta_lbA_cov[ NCMAX_ALLOC ]; /** A covariance-vector of lbA */
real_t delta_ubA_cov[ NCMAX_ALLOC ]; /** A covariance-vector of ubA */
#if QPOASES_USE_OLD_VERSION
real_t K[KKT_DIM * KKT_DIM]; /** A matrix to store an intermediate result */
#endif
int FR_idx[ NVMAX ]; /** Index array for free variables */
int FX_idx[ NVMAX ]; /** Index array for fixed variables */
int AC_idx[ NCMAX_ALLOC ]; /** Index array for active constraints */
real_t delta_xFR[ NVMAX ]; /** QP reaction, primal, w.r.t. free */
real_t delta_xFX[ NVMAX ]; /** QP reaction, primal, w.r.t. fixed */
real_t delta_yAC[ NVMAX ]; /** QP reaction, dual, w.r.t. active */
real_t delta_yFX[ NVMAX ]; /** QP reaction, dual, w.r.t. fixed*/
};
#endif // QPOASES_SOLUTIONANALYSIS_HPP
================================================
FILE: externals/qpoases/INCLUDE/Indexlist.hpp
================================================
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file INCLUDE/Indexlist.hpp
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*
* Declaration of the Indexlist class designed to manage index lists of
* constraints and bounds within a SubjectTo object.
*/
#ifndef QPOASES_INDEXLIST_HPP
#define QPOASES_INDEXLIST_HPP
#include <Utils.hpp>
/** This class manages index lists.
*
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*/
class Indexlist
{
/*
* PUBLIC MEMBER FUNCTIONS
*/
public:
/** Default constructor. */
Indexlist( );
/** Copy constructor (deep copy). */
Indexlist( const Indexlist& rhs /**< Rhs object. */
);
/** Destructor. */
~Indexlist( );
/** Assingment operator (deep copy). */
Indexlist& operator=( const Indexlist& rhs /**< Rhs object. */
);
/** Pseudo-constructor.
* \return SUCCESSFUL_RETURN */
returnValue init( );
/** Creates an array of all numbers within the index set in correct order.
* \return SUCCESSFUL_RETURN \n
RET_INDEXLIST_CORRUPTED */
returnValue getNumberArray( int* const numberarray /**< Output: Array of numbers (NULL on error). */
) const;
/** Determines the index within the index list at with a given number is stored.
* \return >= 0: Index of given number. \n
-1: Number not found. */
int getIndex( int givennumber /**< Number whose index shall be determined. */
) const;
/** Determines the physical index within the index list at with a given number is stored.
* \return >= 0: Index of given number. \n
-1: Number not found. */
int getPhysicalIndex( int givennumber /**< Number whose physical index shall be determined. */
) const;
/** Returns the number stored at a given physical index.
* \return >= 0: Number stored at given physical index. \n
-RET_INDEXLIST_OUTOFBOUNDS */
int getNumber( int physicalindex /**< Physical index of the number to be returned. */
) const;
/** Returns the current length of the index list.
* \return Current length of the index list. */
inline int getLength( );
/** Returns last number within the index list.
* \return Last number within the index list. */
inline int getLastNumber( ) const;
/** Adds number to index list.
* \return SUCCESSFUL_RETURN \n
RET_INDEXLIST_MUST_BE_REORDERD \n
RET_INDEXLIST_EXCEEDS_MAX_LENGTH */
returnValue addNumber( int addnumber /**< Number to be added. */
);
/** Removes number from index list.
* \return SUCCESSFUL_RETURN */
returnValue removeNumber( int removenumber /**< Number to be removed. */
);
/** Swaps two numbers within index list.
* \return SUCCESSFUL_RETURN */
returnValue swapNumbers( int number1,/**< First number for swapping. */
int number2 /**< Second number for swapping. */
);
/** Determines if a given number is contained in the index set.
* \return BT_TRUE iff number is contain in the index set */
inline BooleanType isMember( int _number /**< Number to be tested for membership. */
) const;
/*
* PROTECTED MEMBER VARIABLES
*/
protected:
int number[INDEXLISTFACTOR*(NVMAX+NCMAX)]; /**< Array to store numbers of constraints or bounds. */
int next[INDEXLISTFACTOR*(NVMAX+NCMAX)]; /**< Array to store physical index of successor. */
int previous[INDEXLISTFACTOR*(NVMAX+NCMAX)]; /**< Array to store physical index of predecossor. */
int length; /**< Length of index list. */
int first; /**< Physical index of first element. */
int last; /**< Physical index of last element. */
int lastusedindex; /**< Physical index of last entry in index list. */
int physicallength; /**< Physical length of index list. */
};
#include <Indexlist.ipp>
#endif /* QPOASES_INDEXLIST_HPP */
/*
* end of file
*/
================================================
FILE: externals/qpoases/INCLUDE/MessageHandling.hpp
================================================
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file INCLUDE/MessageHandling.hpp
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*
* Declaration of the MessageHandling class including global return values.
*/
#ifndef QPOASES_MESSAGEHANDLING_HPP
#define QPOASES_MESSAGEHANDLING_HPP
// #define PC_DEBUG
#ifdef PC_DEBUG
#include <stdio.h>
/** Defines an alias for FILE from stdio.h. */
#define myFILE FILE
/** Defines an alias for stderr from stdio.h. */
#define myStderr stderr
/** Defines an alias for stdout from stdio.h. */
#define myStdout stdout
#else
/** Defines an alias for FILE from stdio.h. */
#define myFILE int
/** Defines an alias for stderr from stdio.h. */
#define myStderr 0
/** Defines an alias for stdout from stdio.h. */
#define myStdout 0
#endif
#include <Types.hpp>
#include <Constants.hpp>
/** Defines symbols for global return values. \n
* Important: All return values are assumed to be nonnegative! */
enum returnValue
{
TERMINAL_LIST_ELEMENT = -1, /**< Terminal list element, internal usage only! */
/* miscellaneous */
SUCCESSFUL_RETURN = 0, /**< Successful return. */
RET_DIV_BY_ZERO, /**< Division by zero. */
RET_INDEX_OUT_OF_BOUNDS, /**< Index out of bounds. */
RET_INVALID_ARGUMENTS, /**< At least one of the arguments is invalid. */
RET_ERROR_UNDEFINED, /**< Error number undefined. */
RET_WARNING_UNDEFINED, /**< Warning number undefined. */
RET_INFO_UNDEFINED, /**< Info number undefined. */
RET_EWI_UNDEFINED, /**< Error/warning/info number undefined. */
RET_AVAILABLE_WITH_LINUX_ONLY, /**< This function is available under Linux only. */
RET_UNKNOWN_BUG, /**< The error occured is not yet known. */
RET_PRINTLEVEL_CHANGED, /**< 10 Print level changed. */
RET_NOT_YET_IMPLEMENTED, /**< Requested function is not yet implemented in this version of qpOASES. */
/* Indexlist */
RET_INDEXLIST_MUST_BE_REORDERD, /**< Index list has to be reordered. */
RET_INDEXLIST_EXCEEDS_MAX_LENGTH, /**< Index list exceeds its maximal physical length. */
RET_INDEXLIST_CORRUPTED, /**< Index list corrupted. */
RET_INDEXLIST_OUTOFBOUNDS, /**< Physical index is out of bounds. */
RET_INDEXLIST_ADD_FAILED, /**< Adding indices from another index set failed. */
RET_INDEXLIST_INTERSECT_FAILED, /**< Intersection with another index set failed. */
/* SubjectTo / Bounds / Constraints */
RET_INDEX_ALREADY_OF_DESIRED_STATUS, /**< Index is already of desired status. */
RET_ADDINDEX_FAILED, /**< Cannot swap between different indexsets. */
RET_SWAPINDEX_FAILED, /**< 20 Adding index to index set failed. */
RET_NOTHING_TO_DO, /**< Nothing to do. */
RET_SETUP_BOUND_FAILED, /**< Setting up bound index failed. */
RET_SETUP_CONSTRAINT_FAILED, /**< Setting up constraint index failed. */
RET_MOVING_BOUND_FAILED, /**< Moving bound between index sets failed. */
RET_MOVING_CONSTRAINT_FAILED, /**< Moving constraint between index sets failed. */
/* QProblem */
RET_QP_ALREADY_INITIALISED, /**< QProblem has already been initialised. */
RET_NO_INIT_WITH_STANDARD_SOLVER, /**< Initialisation via extern QP solver is not yet implemented. */
RET_RESET_FAILED, /**< Reset failed. */
RET_INIT_FAILED, /**< Initialisation failed. */
RET_INIT_FAILED_TQ, /**< 30 Initialisation failed due to TQ factorisation. */
RET_INIT_FAILED_CHOLESKY, /**< Initialisation failed due to Cholesky decomposition. */
RET_INIT_FAILED_HOTSTART, /**< Initialisation failed! QP could not be solved! */
RET_INIT_FAILED_INFEASIBILITY, /**< Initial QP could not be solved due to infeasibility! */
RET_INIT_FAILED_UNBOUNDEDNESS, /**< Initial QP could not be solved due to unboundedness! */
RET_INIT_SUCCESSFUL, /**< Initialisation done. */
RET_OBTAINING_WORKINGSET_FAILED, /**< Failed to obtain working set for auxiliary QP. */
RET_SETUP_WORKINGSET_FAILED, /**< Failed to setup working set for auxiliary QP. */
RET_SETUP_AUXILIARYQP_FAILED, /**< Failed to setup auxiliary QP for initialised homotopy. */
RET_NO_EXTERN_SOLVER, /**< No extern QP solver available. */
RET_QP_UNBOUNDED, /**< 40 QP is unbounded. */
RET_QP_INFEASIBLE, /**< QP is infeasible. */
RET_QP_NOT_SOLVED, /**< Problems occured while solving QP with standard solver. */
RET_QP_SOLVED, /**< QP successfully solved. */
RET_UNABLE_TO_SOLVE_QP, /**< Problems occured while solving QP. */
RET_INITIALISATION_STARTED, /**< Starting problem initialisation. */
RET_HOTSTART_FAILED, /**< Unable to perform homotopy due to internal error. */
RET_HOTSTART_FAILED_TO_INIT, /**< Unable to initialise problem. */
RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED, /**< Unable to perform homotopy as previous QP is not solved. */
RET_ITERATION_STARTED, /**< Iteration... */
RET_SHIFT_DETERMINATION_FAILED, /**< 50 Determination of shift of the QP data failed. */
RET_STEPDIRECTION_DETERMINATION_FAILED, /**< Determination of step direction failed. */
RET_STEPLENGTH_DETERMINATION_FAILED, /**< Determination of step direction failed. */
RET_OPTIMAL_SOLUTION_FOUND, /**< Optimal solution of neighbouring QP found. */
RET_HOMOTOPY_STEP_FAILED, /**< Unable to perform homotopy step. */
RET_HOTSTART_STOPPED_INFEASIBILITY, /**< Premature homotopy termination because QP is infeasible. */
RET_HOTSTART_STOPPED_UNBOUNDEDNESS, /**< Premature homotopy termination because QP is unbounded. */
RET_WORKINGSET_UPDATE_FAILED, /**< Unable to update working sets according to initial guesses. */
RET_MAX_NWSR_REACHED, /**< Maximum number of working set recalculations performed. */
RET_CONSTRAINTS_NOT_SPECIFIED, /**< Problem does comprise constraints! You also have to specify new constraints' bounds. */
RET_INVALID_FACTORISATION_FLAG, /**< 60 Invalid factorisation flag. */
RET_UNABLE_TO_SAVE_QPDATA, /**< Unable to save QP data. */
RET_STEPDIRECTION_FAILED_TQ, /**< Abnormal termination due to TQ factorisation. */
RET_STEPDIRECTION_FAILED_CHOLESKY, /**< Abnormal termination due to Cholesky factorisation. */
RET_CYCLING_DETECTED, /**< Cycling detected. */
RET_CYCLING_NOT_RESOLVED, /**< Cycling cannot be resolved, QP probably infeasible. */
RET_CYCLING_RESOLVED, /**< Cycling probably resolved. */
RET_STEPSIZE, /**< For displaying performed stepsize. */
RET_STEPSIZE_NONPOSITIVE, /**< For displaying non-positive stepsize. */
RET_SETUPSUBJECTTOTYPE_FAILED, /**< Setup of SubjectToTypes failed. */
RET_ADDCONSTRAINT_FAILED, /**< 70 Addition of constraint to working set failed. */
RET_ADDCONSTRAINT_FAILED_INFEASIBILITY, /**< Addition of constraint to working set failed (due to QP infeasibility). */
RET_ADDBOUND_FAILED, /**< Addition of bound to working set failed. */
RET_ADDBOUND_FAILED_INFEASIBILITY, /**< Addition of bound to working set failed (due to QP infeasibility). */
RET_REMOVECONSTRAINT_FAILED, /**< Removal of constraint from working set failed. */
RET_REMOVEBOUND_FAILED, /**< Removal of bound from working set failed. */
RET_REMOVE_FROM_ACTIVESET, /**< Removing from active set... */
RET_ADD_TO_ACTIVESET, /**< Adding to active set... */
RET_REMOVE_FROM_ACTIVESET_FAILED, /**< Removing from active set failed. */
RET_ADD_TO_ACTIVESET_FAILED, /**< Adding to active set failed. */
RET_CONSTRAINT_ALREADY_ACTIVE, /**< 80 Constraint is already active. */
RET_ALL_CONSTRAINTS_ACTIVE, /**< All constraints are active, no further constraint can be added. */
RET_LINEARLY_DEPENDENT, /**< New bound/constraint is linearly dependent. */
RET_LINEARLY_INDEPENDENT, /**< New bound/constraint is linearly independent. */
RET_LI_RESOLVED, /**< Linear independence of active contraint matrix successfully resolved. */
RET_ENSURELI_FAILED, /**< Failed to ensure linear indepence of active contraint matrix. */
RET_ENSURELI_FAILED_TQ, /**< Abnormal termination due to TQ factorisation. */
RET_ENSURELI_FAILED_NOINDEX, /**< No index found, QP probably infeasible. */
RET_ENSURELI_FAILED_CYCLING, /**< Cycling detected, QP probably infeasible. */
RET_BOUND_ALREADY_ACTIVE, /**< Bound is already active. */
RET_ALL_BOUNDS_ACTIVE, /**< 90 All bounds are active, no further bound can be added. */
RET_CONSTRAINT_NOT_ACTIVE, /**< Constraint is not active. */
RET_BOUND_NOT_ACTIVE, /**< Bound is not active. */
RET_HESSIAN_NOT_SPD, /**< Projected Hessian matrix not positive definite. */
RET_MATRIX_SHIFT_FAILED, /**< Unable to update matrices or to transform vectors. */
RET_MATRIX_FACTORISATION_FAILED, /**< Unable to calculate new matrix factorisations. */
RET_PRINT_ITERATION_FAILED, /**< Unable to print information on current iteration. */
RET_NO_GLOBAL_MESSAGE_OUTPUTFILE, /**< No global message output file initialised. */
/* Utils */
RET_UNABLE_TO_OPEN_FILE, /**< Unable to open file. */
RET_UNABLE_TO_WRITE_FILE, /**< Unable to write into file. */
RET_UNABLE_TO_READ_FILE, /**< 100 Unable to read from file. */
RET_FILEDATA_INCONSISTENT, /**< File contains inconsistent data. */
/* SolutionAnalysis */
RET_NO_SOLUTION, /**< QP solution does not satisfy KKT optimality conditions. */
RET_INACCURATE_SOLUTION /**< KKT optimality conditions not satisfied to sufficient accuracy. */
};
/** This class handles all kinds of messages (errors, warnings, infos) initiated
* by qpOASES modules and stores the correspoding global preferences.
*
* \author Hans Joachim Ferreau (special thanks to Leonard Wirsching)
* \version 1.3embedded
* \date 2007-2008
*/
class MessageHandling
{
/*
* INTERNAL DATA STRUCTURES
*/
public:
/** Data structure for entries in global message list. */
typedef struct {
returnValue key; /**< Global return value. */
const char* data; /**< Corresponding message. */
VisibilityStatus globalVisibilityStatus; /**< Determines if message can be printed.
* If this value is set to VS_HIDDEN, no message is printed! */
} ReturnValueList;
/*
* PUBLIC MEMBER FUNCTIONS
*/
public:
/** Default constructor. */
MessageHandling( );
/** Constructor which takes the desired output file. */
MessageHandling( myFILE* _outputFile /**< Output file. */
);
/** Constructor which takes the desired visibility states. */
MessageHandling( VisibilityStatus _errorVisibility, /**< Visibility status for error messages. */
VisibilityStatus _warningVisibility,/**< Visibility status for warning messages. */
VisibilityStatus _infoVisibility /**< Visibility status for info messages. */
);
/** Constructor which takes the desired output file and desired visibility states. */
MessageHandling( myFILE* _outputFile, /**< Output file. */
VisibilityStatus _errorVisibility, /**< Visibility status for error messages. */
VisibilityStatus _warningVisibility,/**< Visibility status for warning messages. */
VisibilityStatus _infoVisibility /**< Visibility status for info messages. */
);
/** Copy constructor (deep copy). */
MessageHandling( const MessageHandling& rhs /**< Rhs object. */
);
/** Destructor. */
~MessageHandling( );
/** Assignment operator (deep copy). */
MessageHandling& operator=( const MessageHandling& rhs /**< Rhs object. */
);
/** Prints an error message(a simplified macro THROWERROR is also provided). \n
* Errors are definied as abnormal events which cause an immediate termination of the current (sub) function.
* Errors of a sub function should be commented by the calling function by means of a warning message
* (if this error does not cause an error of the calling function, either)!
* \return Error number returned by sub function call
*/
returnValue throwError(
returnValue Enumber, /**< Error number returned by sub function call. */
const char* additionaltext, /**< Additional error text (0, if none). */
const char* functionname, /**< Name of function which caused the error. */
const char* filename, /**< Name of file which caused the error. */
const unsigned long linenumber, /**< Number of line which caused the error.incompatible binary file */
VisibilityStatus localVisibilityStatus /**< Determines (locally) if error message can be printed to myStderr.
* If GLOBAL visibility status of the message is set to VS_HIDDEN,
* no message is printed, anyway! */
);
/** Prints a warning message (a simplified macro THROWWARNING is also provided).
* Warnings are definied as abnormal events which does NOT cause an immediate termination of the current (sub) function.
* \return Warning number returned by sub function call
*/
returnValue throwWarning(
returnValue Wnumber, /**< Warning number returned by sub function call. */
const char* additionaltext, /**< Additional warning text (0, if none). */
const char* functionname, /**< Name of function which caused the warning. */
const char* filename, /**< Name of file which caused the warning. */
const unsigned long linenumber, /**< Number of line which caused the warning. */
VisibilityStatus localVisibilityStatus /**< Determines (locally) if warning message can be printed to myStderr.
* If GLOBAL visibility status of the message is set to VS_HIDDEN,
* no message is printed, anyway! */
);
/** Prints a info message (a simplified macro THROWINFO is also provided).
* \return Info number returned by sub function call
*/
returnValue throwInfo(
returnValue Inumber, /**< Info number returned by sub function call. */
const char* additionaltext, /**< Additional warning text (0, if none). */
const char* functionname, /**< Name of function which submitted the info. */
const char* filename, /**< Name of file which submitted the info. */
const unsigned long linenumber, /**< Number of line which submitted the info. */
VisibilityStatus localVisibilityStatus /**< Determines (locally) if info message can be printed to myStderr.
* If GLOBAL visibility status of the message is set to VS_HIDDEN,
* no message is printed, anyway! */
);
/** Resets all preferences to default values.
* \return SUCCESSFUL_RETURN */
returnValue reset( );
/** Prints a complete list of all messages to output file.
* \return SUCCESSFUL_RETURN */
returnValue listAllMessages( );
/** Returns visibility status for error messages.
* \return Visibility status for error messages. */
inline VisibilityStatus getErrorVisibilityStatus( ) const;
/** Returns visibility status for warning messages.
* \return Visibility status for warning messages. */
inline VisibilityStatus getWarningVisibilityStatus( ) const;
/** Returns visibility status for info messages.
* \return Visibility status for info messages. */
inline VisibilityStatus getInfoVisibilityStatus( ) const;
/** Returns pointer to output file.
* \return Pointer to output file. */
inline myFILE* getOutputFile( ) const;
/** Returns error count value.
* \return Error count value. */
inline int getErrorCount( ) const;
/** Changes visibility status for error messages. */
inline void setErrorVisibilityStatus( VisibilityStatus _errorVisibility /**< New visibility status for error messages. */
);
/** Changes visibility status for warning messages. */
inline void setWarningVisibilityStatus( VisibilityStatus _warningVisibility /**< New visibility status for warning messages. */
);
/** Changes visibility status for info messages. */
inline void setInfoVisibilityStatus( VisibilityStatus _infoVisibility /**< New visibility status for info messages. */
);
/** Changes output file for messages. */
inline void setOutputFile( myFILE* _outputFile /**< New output file for messages. */
);
/** Changes error count.
* \return SUCCESSFUL_RETURN \n
* RET_INVALID_ARGUMENT */
inline returnValue setErrorCount( int _errorCount /**< New error count value. */
);
/** Return the error code string. */
static const char* getErrorString(int error);
/*
* PROTECTED MEMBER FUNCTIONS
*/
protected:
/** Prints a info message to myStderr (auxiliary function).
* \return Error/warning/info number returned by sub function call
*/
returnValue throwMessage(
returnValue RETnumber, /**< Error/warning/info number returned by sub function call. */
const char* additionaltext, /**< Additional warning text (0, if none). */
const char* functionname, /**< Name of function which caused the error/warning/info. */
const char* filename, /**< Name of file which caused the error/warning/info. */
const unsigned long linenumber, /**< Number of line which caused the error/warning/info. */
VisibilityStatus localVisibilityStatus, /**< Determines (locally) if info message can be printed to myStderr.
* If GLOBAL visibility status of the message is set to VS_HIDDEN,
* no message is printed, anyway! */
const char* RETstring /**< Leading string of error/warning/info message. */
);
/*
* PROTECTED MEMBER VARIABLES
*/
protected:
VisibilityStatus errorVisibility; /**< Error messages visible? */
VisibilityStatus warningVisibility; /**< Warning messages visible? */
VisibilityStatus infoVisibility; /**< Info messages visible? */
myFILE* outputFile; /**< Output file for messages. */
int errorCount; /**< Counts number of errors (for nicer output only). */
};
#ifndef __FUNCTION__
/** Ensures that __FUNCTION__ macro is defined. */
#define __FUNCTION__ 0
#endif
#ifndef __FILE__
/** Ensures that __FILE__ macro is defined. */
#define __FILE__ 0
#endif
#ifndef __LINE__
/** Ensures that __LINE__ macro is defined. */
#define __LINE__ 0
#endif
/** Short version of throwError with default values, only returnValue is needed */
#define THROWERROR(retval) ( getGlobalMessageHandler( )->throwError((retval),0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE) )
/** Short version of throwWarning with default values, only returnValue is needed */
#define THROWWARNING(retval) ( getGlobalMessageHandler( )->throwWarning((retval),0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE) )
/** Short version of throwInfo with default values, only returnValue is needed */
#define THROWINFO(retval) ( getGlobalMessageHandler( )->throwInfo((retval),0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE) )
/** Returns a pointer to global message handler.
* \return Pointer to global message handler.
*/
MessageHandling* getGlobalMessageHandler( );
#include <MessageHandling.ipp>
#endif /* QPOASES_MESSAGEHANDLING_HPP */
/*
* end of file
*/
================================================
FILE: externals/qpoases/INCLUDE/QProblem.hpp
================================================
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file INCLUDE/QProblem.hpp
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*
* Declaration of the QProblem class which is able to use the newly
* developed online active set strategy for parametric quadratic programming.
*/
#ifndef QPOASES_QPROBLEM_HPP
#define QPOASES_QPROBLEM_HPP
#include <QProblemB.hpp>
#include <Constraints.hpp>
#include <CyclingManager.hpp>
/** A class for setting up and solving quadratic programs. The main feature is
* the possibily to use the newly developed online active set strategy for
* parametric quadratic programming.
*
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*/
class QProblem : public QProblemB
{
/* allow SolutionAnalysis class to access private members */
friend class SolutionAnalysis;
/*
* PUBLIC MEMBER FUNCTIONS
*/
public:
/** Default constructor. */
QProblem( );
/** Constructor which takes the QP dimensions only. */
QProblem( int _nV, /**< Number of variables. */
int _nC /**< Number of constraints. */
);
/** Copy constructor (deep copy). */
QProblem( const QProblem& rhs /**< Rhs object. */
);
/** Destructor. */
~QProblem( );
/** Assignment operator (deep copy). */
QProblem& operator=( const QProblem& rhs /**< Rhs object. */
);
/** Clears all data structures of QProblemB except for QP data.
* \return SUCCESSFUL_RETURN \n
RET_RESET_FAILED */
returnValue reset( );
/** Initialises a QProblem with given QP data and solves it
* using an initial homotopy with empty working set (at most nWSR iterations).
* \return SUCCESSFUL_RETURN \n
RET_INIT_FAILED \n
RET_INIT_FAILED_CHOLESKY \n
RET_INIT_FAILED_TQ \n
RET_INIT_FAILED_HOTSTART \n
RET_INIT_FAILED_INFEASIBILITY \n
RET_INIT_FAILED_UNBOUNDEDNESS \n
RET_MAX_NWSR_REACHED \n
RET_INVALID_ARGUMENTS \n
RET_INACCURATE_SOLUTION \n
RET_NO_SOLUTION */
returnValue init( const real_t* const _H, /**< Hessian matrix. */
const real_t* const _g, /**< Gradient vector. */
const real_t* const _A, /**< Constraint matrix. */
const real_t* const _lb, /**< Lower bound vector (on variables). \n
If no lower bounds exist, a NULL pointer can be passed. */
const real_t* const _ub, /**< Upper bound vector (on variables). \n
If no upper bounds exist, a NULL pointer can be passed. */
const real_t* const _lbA, /**< Lower constraints' bound vector. \n
If no lower constraints' bounds exist, a NULL pointer can be passed. */
const real_t* const _ubA, /**< Upper constraints' bound vector. \n
If no lower constraints' bounds exist, a NULL pointer can be passed. */
int& nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy.
Output: Number of performed working set recalculations. */
const real_t* const yOpt = 0, /**< Initial guess for dual solution vector. */
real_t* const cputime = 0 /**< Output: CPU time required to initialise QP. */
);
/** Initialises a QProblem with given QP data and solves it
* using an initial homotopy with empty working set (at most nWSR iterations).
* \return SUCCESSFUL_RETURN \n
RET_INIT_FAILED \n
RET_INIT_FAILED_CHOLESKY \n
RET_INIT_FAILED_TQ \n
RET_INIT_FAILED_HOTSTART \n
RET_INIT_FAILED_INFEASIBILITY \n
RET_INIT_FAILED_UNBOUNDEDNESS \n
RET_MAX_NWSR_REACHED \n
RET_INVALID_ARGUMENTS \n
RET_INACCURATE_SOLUTION \n
RET_NO_SOLUTION */
returnValue init( const real_t* const _H, /**< Hessian matrix. */
const real_t* const _R, /**< Cholesky factorization of the Hessian matrix. */
const real_t* const _g, /**< Gradient vector. */
const real_t* const _A, /**< Constraint matrix. */
const real_t* const _lb, /**< Lower bound vector (on variables). \n
If no lower bounds exist, a NULL pointer can be passed. */
const real_t* const _ub, /**< Upper bound vector (on variables). \n
If no upper bounds exist, a NULL pointer can be passed. */
const real_t* const _lbA, /**< Lower constraints' bound vector. \n
If no lower constraints' bounds exist, a NULL pointer can be passed. */
const real_t* const _ubA, /**< Upper constraints' bound vector. \n
If no lower constraints' bounds exist, a NULL pointer can be passed. */
int& nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy.
Output: Number of performed working set recalculations. */
const real_t* const yOpt = 0, /**< Initial guess for dual solution vector. */
real_t* const cputime = 0 /**< Output: CPU time required to initialise QP. */
);
/** Solves QProblem using online active set strategy.
* \return SUCCESSFUL_RETURN \n
RET_MAX_NWSR_REACHED \n
RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
RET_HOTSTART_FAILED \n
RET_SHIFT_DETERMINATION_FAILED \n
RET_STEPDIRECTION_DETERMINATION_FAILED \n
RET_STEPLENGTH_DETERMINATION_FAILED \n
RET_HOMOTOPY_STEP_FAILED \n
RET_HOTSTART_STOPPED_INFEASIBILITY \n
RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
RET_INACCURATE_SOLUTION \n
RET_NO_SOLUTION */
returnValue hotstart( const real_t* const g_new, /**< Gradient of neighbouring QP to be solved. */
const real_t* const lb_new, /**< Lower bounds of neighbouring QP to be solved. \n
If no lower bounds exist, a NULL pointer can be passed. */
const real_t* const ub_new, /**< Upper bounds of neighbouring QP to be solved. \n
If no upper bounds exist, a NULL pointer can be passed. */
const real_t* const lbA_new, /**< Lower constraints' bounds of neighbouring QP to be solved. \n
If no lower constraints' bounds exist, a NULL pointer can be passed. */
const real_t* const ubA_new, /**< Upper constraints' bounds of neighbouring QP to be solved. \n
If no upper constraints' bounds exist, a NULL pointer can be passed. */
int& nWSR, /**< Input: Maximum number of working set recalculations; \n
Output: Number of performed working set recalculations. */
real_t* const cputime /**< Output: CPU time required to solve QP (or to perform nWSR iterations). */
);
/** Returns constraint matrix of the QP (deep copy).
* \return SUCCESSFUL_RETURN */
inline returnValue getA( real_t* const _A /**< Array of appropriate dimension for copying constraint matrix.*/
) const;
/** Returns a single row of constraint matrix of the QP (deep copy).
* \return SUCCESSFUL_RETURN \n
RET_INDEX_OUT_OF_BOUNDS */
inline returnValue getA( int number, /**< Number of entry to be returned. */
real_t* const row /**< Array of appropriate dimension for copying (number)th constraint. */
) const;
/** Returns lower constraints' bound vector of the QP (deep copy).
* \return SUCCESSFUL_RETURN */
inline returnValue getLBA( real_t* const _lbA /**< Array of appropriate dimension for copying lower constraints' bound vector.*/
) const;
/** Returns single entry of lower constraints' bound vector of the QP.
* \return SUCCESSFUL_RETURN \n
RET_INDEX_OUT_OF_BOUNDS */
inline returnValue getLBA( int number, /**< Number of entry to be returned. */
real_t& value /**< Output: lbA[number].*/
) const;
/** Returns upper constraints' bound vector of the QP (deep copy).
* \return SUCCESSFUL_RETURN */
inline returnValue getUBA( real_t* const _ubA /**< Array of appropriate dimension for copying upper constraints' bound vector.*/
) const;
/** Returns single entry of upper constraints' bound vector of the QP.
* \return SUCCESSFUL_RETURN \n
RET_INDEX_OUT_OF_BOUNDS */
inline returnValue getUBA( int number, /**< Number of entry to be returned. */
real_t& value /**< Output: ubA[number].*/
) const;
/** Returns current constraints object of the QP (deep copy).
* \return SUCCESSFUL_RETURN */
inline returnValue getConstraints( Constraints* const _constraints /** Output: Constraints object. */
) const;
/** Returns the number of constraints.
* \return Number of constraints. */
inline int getNC( ) const;
/** Returns the number of (implicitly defined) equality constraints.
* \return Number of (implicitly defined) equality constraints. */
inline int getNEC( ) const;
/** Returns the number of active constraints.
* \return Number of active constraints. */
inline int getNAC( );
/** Returns the number of inactive constraints.
* \return Number of inactive constraints. */
inline int getNIAC( );
/** Returns the dimension of null space.
* \return Dimension of null space. */
int getNZ( );
/** Returns the dual solution vector (deep copy).
* \return SUCCESSFUL_RETURN \n
RET_QP_NOT_SOLVED */
returnValue getDualSolution( real_t* const yOpt /**< Output: Dual solution vector (if QP has been solved). */
) const;
/*
* PROTECTED MEMBER FUNCTIONS
*/
protected:
/** Determines type of constraints and bounds (i.e. implicitly fixed, unbounded etc.).
* \return SUCCESSFUL_RETURN \n
RET_SETUPSUBJECTTOTYPE_FAILED */
returnValue setupSubjectToType( );
/** Computes the Cholesky decomposition R of the projected Hessian (i.e. R^T*R = Z^T*H*Z).
* \return SUCCESSFUL_RETURN \n
* RET_INDEXLIST_CORRUPTED */
returnValue setupCholeskyDecompositionProjected( );
/** Initialises TQ factorisation of A (i.e. A*Q = [0 T]) if NO constraint is active.
* \return SUCCESSFUL_RETURN \n
RET_INDEXLIST_CORRUPTED */
returnValue setupTQfactorisation( );
/** Solves a QProblem whose QP data is assumed to be stored in the member variables.
* A guess for its primal/dual optimal solution vectors and the corresponding
* working sets of bounds and constraints can be provided.
* \return SUCCESSFUL_RETURN \n
RET_INIT_FAILED \n
RET_INIT_FAILED_CHOLESKY \n
RET_INIT_FAILED_TQ \n
RET_INIT_FAILED_HOTSTART \n
RET_INIT_FAILED_INFEASIBILITY \n
RET_INIT_FAILED_UNBOUNDEDNESS \n
RET_MAX_NWSR_REACHED */
returnValue solveInitialQP( const real_t* const xOpt, /**< Optimal primal solution vector.
* A NULL pointer can be passed. */
const real_t* const yOpt, /**< Optimal dual solution vector.
* A NULL pointer can be passed. */
const Bounds* const guessedBounds, /**< Guessed working set of bounds for solution (xOpt,yOpt).
* A NULL pointer can be passed. */
const Constraints* const guessedConstraints, /**< Optimal working set of constraints for solution (xOpt,yOpt).
* A NULL pointer can be passed. */
int& nWSR, /**< Input: Maximum number of working set recalculations; \n
* Output: Number of performed working set recalculations. */
real_t* const cputime /**< Output: CPU time required to solve QP (or to perform nWSR iterations). */
);
/** Obtains the desired working set for the auxiliary initial QP in
* accordance with the user specifications
* (assumes that member AX has already been initialised!)
* \return SUCCESSFUL_RETURN \n
RET_OBTAINING_WORKINGSET_FAILED \n
RET_INVALID_ARGUMENTS */
returnValue obtainAuxiliaryWorkingSet( const real_t* const xOpt, /**< Optimal primal solution vector.
* If a NULL pointer is passed, all entries are assumed to be zero. */
const real_t* const yOpt, /**< Optimal dual solution vector.
* If a NULL pointer is passed, all entries are assumed to be zero. */
const Bounds* const guessedBounds, /**< Guessed working set of bounds for solution (xOpt,yOpt). */
const Constraints* const guessedConstraints, /**< Guessed working set for solution (xOpt,yOpt). */
Bounds* auxiliaryBounds, /**< Input: Allocated bound object. \n
* Ouput: Working set of constraints for auxiliary QP. */
Constraints* auxiliaryConstraints /**< Input: Allocated bound object. \n
* Ouput: Working set for auxiliary QP. */
) const;
/** Setups bound and constraints data structures according to auxiliaryBounds/Constraints.
* (If the working set shall be setup afresh, make sure that
* bounds and constraints data structure have been resetted
* and the TQ factorisation has been initialised!)
* \return SUCCESSFUL_RETURN \n
RET_SETUP_WORKINGSET_FAILED \n
RET_INVALID_ARGUMENTS \n
RET_UNKNOWN BUG */
returnValue setupAuxiliaryWorkingSet( const Bounds* const auxiliaryBounds, /**< Working set of bounds for auxiliary QP. */
const Constraints* const auxiliaryConstraints, /**< Working set of constraints for auxiliary QP. */
BooleanType setupAfresh /**< Flag indicating if given working set shall be
* setup afresh or by updating the current one. */
);
/** Setups the optimal primal/dual solution of the auxiliary initial QP.
* \return SUCCESSFUL_RETURN */
returnValue setupAuxiliaryQPsolution( const real_t* const xOpt, /**< Optimal primal solution vector.
* If a NULL pointer is passed, all entries are set to zero. */
const real_t* const yOpt /**< Optimal dual solution vector.
* If a NULL pointer is passed, all entries are set to zero. */
);
/** Setups gradient of the auxiliary initial QP for given
* optimal primal/dual solution and given initial working set
* (assumes that members X, Y and BOUNDS, CONSTRAINTS have already been initialised!).
* \return SUCCESSFUL_RETURN */
returnValue setupAuxiliaryQPgradient( );
/** Setups (constraints') bounds of the auxiliary initial QP for given
* optimal primal/dual solution and given initial working set
* (assumes that members X, Y and BOUNDS, CONSTRAINTS have already been initialised!).
* \return SUCCESSFUL_RETURN \n
RET_UNKNOWN BUG */
returnValue setupAuxiliaryQPbounds( const Bounds* const auxiliaryBounds, /**< Working set of bounds for auxiliary QP. */
const Constraints* const auxiliaryConstraints, /**< Working set of constraints for auxiliary QP. */
BooleanType useRelaxation /**< Flag indicating if inactive (constraints') bounds shall be relaxed. */
);
/** Adds a constraint to active set.
* \return SUCCESSFUL_RETURN \n
RET_ADDCONSTRAINT_FAILED \n
RET_ADDCONSTRAINT_FAILED_INFEASIBILITY \n
RET_ENSURELI_FAILED */
returnValue addConstraint( int number, /**< Number of constraint to be added to active set. */
SubjectToStatus C_status, /**< Status of new active constraint. */
BooleanType updateCholesky /**< Flag indicating if Cholesky decomposition shall be updated. */
);
/** Checks if new active constraint to be added is linearly dependent from
* from row of the active constraints matrix.
* \return RET_LINEARLY_DEPENDENT \n
RET_LINEARLY_INDEPENDENT \n
RET_INDEXLIST_CORRUPTED */
returnValue addConstraint_checkLI( int number /**< Number of constraint to be added to active set. */
);
/** Ensures linear independence of constraint matrix when a new constraint is added.
* To this end a bound or constraint is removed simultaneously if necessary.
* \return SUCCESSFUL_RETURN \n
RET_LI_RESOLVED \n
RET_ENSURELI_FAILED \n
RET_ENSURELI_FAILED_TQ \n
RET_ENSURELI_FAILED_NOINDEX \n
RET_REMOVE_FROM_ACTIVESET */
returnValue addConstraint_ensureLI( int number, /**< Number of constraint to be added to active set. */
SubjectToStatus C_status /**< Status of new active bound. */
);
/** Adds a bound to active set.
* \return SUCCESSFUL_RETURN \n
RET_ADDBOUND_FAILED \n
RET_ADDBOUND_FAILED_INFEASIBILITY \n
RET_ENSURELI_FAILED */
returnValue addBound( int number, /**< Number of bound to be added to active set. */
SubjectToStatus B_status, /**< Status of new active bound. */
BooleanType updateCholesky /**< Flag indicating if Cholesky decomposition shall be updated. */
);
/** Checks if new active bound to be added is linearly dependent from
* from row of the active constraints matrix.
* \return RET_LINEARLY_DEPENDENT \n
RET_LINEARLY_INDEPENDENT */
returnValue addBound_checkLI( int number /**< Number of bound to be added to active set. */
);
/** Ensures linear independence of constraint matrix when a new bound is added.
* To this end a bound or constraint is removed simultaneously if necessary.
* \return SUCCESSFUL_RETURN \n
RET_LI_RESOLVED \n
RET_ENSURELI_FAILED \n
RET_ENSURELI_FAILED_TQ \n
RET_ENSURELI_FAILED_NOINDEX \n
RET_REMOVE_FROM_ACTIVESET */
returnValue addBound_ensureLI( int number, /**< Number of bound to be added to active set. */
SubjectToStatus B_status /**< Status of new active bound. */
);
/** Removes a constraint from active set.
* \return SUCCESSFUL_RETURN \n
RET_CONSTRAINT_NOT_ACTIVE \n
RET_REMOVECONSTRAINT_FAILED \n
RET_HESSIAN_NOT_SPD */
returnValue removeConstraint( int number, /**< Number of constraint to be removed from active set. */
BooleanType updateCholesky /**< Flag indicating if Cholesky decomposition shall be updated. */
);
/** Removes a bounds from active set.
* \return SUCCESSFUL_RETURN \n
RET_BOUND_NOT_ACTIVE \n
RET_HESSIAN_NOT_SPD \n
RET_REMOVEBOUND_FAILED */
returnValue removeBound( int number, /**< Number of bound to be removed from active set. */
BooleanType updateCholesky /**< Flag indicating if Cholesky decomposition shall be updated. */
);
/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix.
* \return SUCCESSFUL_RETURN \n
RET_DIV_BY_ZERO */
returnValue backsolveR( const real_t* const b, /**< Right hand side vector. */
BooleanType transposed, /**< Indicates if the transposed system shall be solved. */
real_t* const a /**< Output: Solution vector */
);
/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix. \n
* Special variant for the case that this function is called from within "removeBound()".
* \return SUCCESSFUL_RETURN \n
RET_DIV_BY_ZERO */
returnValue backsolveR( const real_t* const b, /**< Right hand side vector. */
BooleanType transposed, /**< Indicates if the transposed system shall be solved. */
BooleanType removingBound, /**< Indicates if function is called from "removeBound()". */
real_t* const a /**< Output: Solution vector */
);
/** Solves the system Ta = b or T^Ta = b where T is a reverse upper triangular matrix.
* \return SUCCESSFUL_RETURN \n
RET_DIV_BY_ZERO */
returnValue backsolveT( const real_t* const b, /**< Right hand side vector. */
BooleanType transposed, /**< Indicates if the transposed system shall be solved. */
real_t* const a /**< Output: Solution vector */
);
/** Determines step direction of the shift of the QP data.
* \return SUCCESSFUL_RETURN */
returnValue hotstart_determineDataShift(const int* const FX_idx, /**< Index array of fixed variables. */
const int* const AC_idx, /**< Index array of active constraints. */
const real_t* const g_new, /**< New gradient vector. */
const real_t* const lbA_new,/**< New lower constraints' bounds. */
const real_t* const ubA_new,/**< New upper constraints' bounds. */
const real_t* const lb_new, /**< New lower bounds. */
const real_t* const ub_new, /**< New upper bounds. */
real_t* const delta_g, /**< Output: Step direction of gradient vector. */
real_t* const delta_lbA, /**< Output: Step direction of lower constraints' bounds. */
real_t* const delta_ubA, /**< Output: Step direction of upper constraints' bounds. */
real_t* const delta_lb, /**< Output: Step direction of lower bounds. */
real_t* const delta_ub, /**< Output: Step direction of upper bounds. */
BooleanType& Delta_bC_isZero,/**< Output: Indicates if active constraints' bounds are to be shifted. */
BooleanType& Delta_bB_isZero/**< Output: Indicates if active bounds are to be shifted. */
);
/** Determines step direction of the homotopy path.
* \return SUCCESSFUL_RETURN \n
RET_STEPDIRECTION_FAILED_TQ \n
RET_STEPDIRECTION_FAILED_CHOLESKY */
returnValue hotstart_determineStepDirection(const int* const FR_idx, /**< Index array of free variables. */
const int* const FX_idx, /**< Index array of fixed variables. */
const int* const AC_idx, /**< Index array of active constraints. */
const real_t* const delta_g, /**< Step direction of gradient vector. */
const real_t* const delta_lbA, /**< Step direction of lower constraints' bounds. */
const real_t* const delta_ubA, /**< Step direction of upper constraints' bounds. */
const real_t* const delta_lb, /**< Step direction of lower bounds. */
const real_t* const delta_ub, /**< Step direction of upper bounds. */
BooleanType Delta_bC_isZero, /**< Indicates if active constraints' bounds are to be shifted. */
BooleanType Delta_bB_isZero, /**< Indicates if active bounds are to be shifted. */
real_t* const delta_xFX, /**< Output: Primal homotopy step direction of fixed variables. */
real_t* const delta_xFR, /**< Output: Primal homotopy step direction of free variables. */
real_t* const delta_yAC, /**< Output: Dual homotopy step direction of active constraints' multiplier. */
real_t* const delta_yFX /**< Output: Dual homotopy step direction of fixed variables' multiplier. */
);
/** Determines the maximum possible step length along the homotopy path.
* \return SUCCESSFUL_RETURN */
returnValue hotstart_determineStepLength( const int* const FR_idx, /**< Index array of free variables. */
const int* const FX_idx, /**< Index array of fixed variables. */
const int* const AC_idx, /**< Index array of active constraints. */
const int* const IAC_idx, /**< Index array of inactive constraints. */
const real_t* const delta_lbA, /**< Step direction of lower constraints' bounds. */
const real_t* const delta_ubA, /**< Step direction of upper constraints' bounds. */
const real_t* const delta_lb, /**< Step direction of lower bounds. */
const real_t* const delta_ub, /**< Step direction of upper bounds. */
const real_t* const delta_xFX, /**< Primal homotopy step direction of fixed variables. */
const real_t* const delta_xFR, /**< Primal homotopy step direction of free variables. */
const real_t* const delta_yAC, /**< Dual homotopy step direction of active constraints' multiplier. */
const real_t* const delta_yFX, /**< Dual homotopy step direction of fixed variables' multiplier. */
real_t* const delta_Ax, /**< Output: Step in vector Ax. */
int& BC_idx, /**< Output: Index of blocking constraint. */
SubjectToStatus& BC_status, /**< Output: Status of blocking constraint. */
BooleanType& BC_isBound /**< Output: Indicates if blocking constraint is a bound. */
);
/** Performs a step along the homotopy path (and updates active set).
* \return SUCCESSFUL_RETURN \n
RET_OPTIMAL_SOLUTION_FOUND \n
RET_REMOVE_FROM_ACTIVESET_FAILED \n
RET_ADD_TO_ACTIVESET_FAILED \n
RET_QP_INFEASIBLE */
returnValue hotstart_performStep( const int* const FR_idx, /**< Index array of free variables. */
const int* const FX_idx, /**< Index array of fixed variables. */
const int* const AC_idx, /**< Index array of active constraints. */
const int* const IAC_idx, /**< Index array of inactive constraints. */
const real_t* const delta_g, /**< Step direction of gradient vector. */
const real_t* const delta_lbA, /**< Step direction of lower constraints' bounds. */
const real_t* const delta_ubA, /**< Step direction of upper constraints' bounds. */
const real_t* const delta_lb, /**< Step direction of lower bounds. */
const real_t* const delta_ub, /**< Step direction of upper bounds. */
const real_t* const delta_xFX, /**< Primal homotopy step direction of fixed variables. */
const real_t* const delta_xFR, /**< Primal homotopy step direction of free variables. */
const real_t* const delta_yAC, /**< Dual homotopy step direction of active constraints' multiplier. */
const real_t* const delta_yFX, /**< Dual homotopy step direction of fixed variables' multiplier. */
const real_t* const delta_Ax, /**< Step in vector Ax. */
int BC_idx, /**< Index of blocking constraint. */
SubjectToStatus BC_status, /**< Status of blocking constraint. */
BooleanType BC_isBound /**< Indicates if blocking constraint is a bound. */
);
/** Checks if lower/upper (constraints') bounds remain consistent
* (i.e. if lb <= ub and lbA <= ubA ) during the current step.
* \return BT_TRUE iff (constraints") bounds remain consistent
*/
BooleanType areBoundsConsistent( const real_t* const delta_lb, /**< Step direction of lower bounds. */
const real_t* const delta_ub, /**< Step direction of upper bounds. */
const real_t* const delta_lbA, /**< Step direction of lower constraints' bounds. */
const real_t* const delta_ubA /**< Step direction of upper constraints' bounds. */
) const;
/** Setups internal QP data.
* \return SUCCESSFUL_RETURN \n
RET_INVALID_ARGUMENTS */
returnValue setupQPdata( const real_t* const _H, /**< Hessian matrix. */
const real_t* const _R, /**< Cholesky factorization of the Hessian matrix. */
const real_t* const _g, /**< Gradient vector. */
const real_t* const _A, /**< Constraint matrix. */
const real_t* const _lb, /**< Lower bound vector (on variables). \n
If no lower bounds exist, a NULL pointer can be passed. */
const real_t* const _ub, /**< Upper bound vector (on variables). \n
If no upper bounds exist, a NULL pointer can be passed. */
const real_t* const _lbA, /**< Lower constraints' bound vector. \n
If no lower constraints' bounds exist, a NULL pointer can be passed. */
const real_t* const _ubA /**< Upper constraints' bound vector. \n
If no lower constraints' bounds exist, a NULL pointer can be passed. */
);
#ifdef PC_DEBUG /* Define print functions only for debugging! */
/** Prints concise information on the current iteration.
* \return SUCCESSFUL_RETURN \n */
returnValue printIteration( int iteration, /**< Number of current iteration. */
int BC_idx, /**< Index of blocking constraint. */
SubjectToStatus BC_status, /**< Status of blocking constraint. */
BooleanType BC_isBound /**< Indicates if blocking constraint is a bound. */
);
/** Prints concise information on the current iteration.
* NOTE: ONLY DEFINED FOR SUPPRESSING A COMPILER WARNING!!
* \return SUCCESSFUL_RETURN \n */
returnValue printIteration( int iteration, /**< Number of current iteration. */
int BC_idx, /**< Index of blocking bound. */
SubjectToStatus BC_status /**< Status of blocking bound. */
);
#endif /* PC_DEBUG */
/** Determines the maximum violation of the KKT optimality conditions
* of the current iterate within the QProblem object.
* \return SUCCESSFUL_RETURN \n
* RET_INACCURATE_SOLUTION \n
* RET_NO_SOLUTION */
returnValue checkKKTconditions( );
/** Sets constraint matrix of the QP. \n
(Remark: Also internal vector Ax is recomputed!)
* \return SUCCESSFUL_RETURN */
inline returnValue setA( const real_t* const A_new /**< New constraint matrix (with correct dimension!). */
);
/** Changes single row of constraint matrix of the QP. \n
(Remark: Also correponding component of internal vector Ax is recomputed!)
* \return SUCCESSFUL_RETURN \n
RET_INDEX_OUT_OF_BOUNDS */
inline returnValue setA( int number, /**< Number of row to be changed. */
const real_t* const value /**< New (number)th constraint (with correct dimension!). */
);
/** Sets constraints' lower bound vector of the QP.
* \return SUCCESSFUL_RETURN */
inline returnValue setLBA( const real_t* const lbA_new /**< New constraints' lower bound vector (with correct dimension!). */
);
/** Changes single entry of lower constraints' bound vector of the QP.
* \return SUCCESSFUL_RETURN \n
RET_INDEX_OUT_OF_BOUNDS */
inline returnValue setLBA( int number, /**< Number of entry to be changed. */
real_t value /**< New value for entry of lower constraints' bound vector (with correct dimension!). */
);
/** Sets constraints' upper bound vector of the QP.
* \return SUCCESSFUL_RETURN */
inline returnValue setUBA( const real_t* const ubA_new /**< New constraints' upper bound vector (with correct dimension!). */
);
/** Changes single entry of upper constraints' bound vector of the QP.
* \return SUCCESSFUL_RETURN \n
RET_INDEX_OUT_OF_BOUNDS */
inline returnValue setUBA( int number, /**< Number of entry to be changed. */
real_t value /**< New value for entry of upper constraints' bound vector (with correct dimension!). */
);
/*
* PROTECTED MEMBER VARIABLES
*/
protected:
real_t A[NCMAX_ALLOC*NVMAX]; /**< Constraint matrix. */
real_t lbA[NCMAX_ALLOC]; /**< Lower constraints' bound vector. */
real_t ubA[NCMAX_ALLOC]; /**< Upper constraints' bound vector. */
Constraints constraints; /**< Data structure for problem's constraints. */
real_t T[NVMAX*NVMAX]; /**< Reverse triangular matrix, A = [0 T]*Q'. */
real_t Q[NVMAX*NVMAX]; /**< Orthonormal quadratic matrix, A = [0 T]*Q'. */
int sizeT; /**< Matrix T is stored in a (sizeT x sizeT) array. */
real_t Ax[NCMAX_ALLOC]; /**< Stores the current product A*x (for increased efficiency only). */
CyclingManager cyclingManager; /**< Data structure for storing (possible) cycling information (NOT YET IMPLEMENTED!). */
};
#include <QProblem.ipp>
#endif /* QPOASES_QPROBLEM_HPP */
/*
* end of file
*/
================================================
FILE: externals/qpoases/INCLUDE/QProblemB.hpp
================================================
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file INCLUDE/QProblemB.hpp
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*
* Declaration of the QProblemB class which is able to use the newly
* developed online active set strategy for parametric quadratic programming
* for problems with (simple) bounds only.
*/
#ifndef QPOASES_QPROBLEMB_HPP
#define QPOASES_QPROBLEMB_HPP
#include <Bounds.hpp>
class SolutionAnalysis;
/** Class for setting up and solving quadratic programs with (simple) bounds only.
* The main feature is the possibily to use the newly developed online active set strategy
* for parametric quadratic programming.
*
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*/
class QProblemB
{
/* allow SolutionAnalysis class to access private members */
friend class SolutionAnalysis;
/*
* PUBLIC MEMBER FUNCTIONS
*/
public:
/** Default constructor. */
QProblemB( );
/** Constructor which takes the QP dimension only. */
QProblemB( int _nV /**< Number of variables. */
);
/** Copy constructor (deep copy). */
QProblemB( const QProblemB& rhs /**< Rhs object. */
);
/** Destructor. */
~QProblemB( );
/** Assignment operator (deep copy). */
QProblemB& operator=( const QProblemB& rhs /**< Rhs object. */
);
/** Clears all data structures of QProblemB except for QP data.
* \return SUCCESSFUL_RETURN \n
RET_RESET_FAILED */
returnValue reset( );
/** Initialises a QProblemB with given QP data and solves it
* using an initial homotopy with empty working set (at most nWSR iterations).
* \return SUCCESSFUL_RETURN \n
RET_INIT_FAILED \n
RET_INIT_FAILED_CHOLESKY \n
RET_INIT_FAILED_HOTSTART \n
RET_INIT_FAILED_INFEASIBILITY \n
RET_INIT_FAILED_UNBOUNDEDNESS \n
RET_MAX_NWSR_REACHED \n
RET_INVALID_ARGUMENTS \n
RET_INACCURATE_SOLUTION \n
RET_NO_SOLUTION */
returnValue init( const real_t* const _H, /**< Hessian matrix. */
const real_t* const _g, /**< Gradient vector. */
const real_t* const _lb, /**< Lower bounds (on variables). \n
If no lower bounds exist, a NULL pointer can be passed. */
const real_t* const _ub, /**< Upper bounds (on variables). \n
If no upper bounds exist, a NULL pointer can be passed. */
int& nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. \n
Output: Number of performed working set recalculations. */
const real_t* const yOpt = 0, /**< Initial guess for dual solution vector. */
real_t* const cputime = 0 /**< Output: CPU time required to initialise QP. */
);
/** Initialises a QProblemB with given QP data and solves it
* using an initial homotopy with empty working set (at most nWSR iterations).
* \return SUCCESSFUL_RETURN \n
RET_INIT_FAILED \n
RET_INIT_FAILED_CHOLESKY \n
RET_INIT_FAILED_HOTSTART \n
RET_INIT_FAILED_INFEASIBILITY \n
RET_INIT_FAILED_UNBOUNDEDNESS \n
RET_MAX_NWSR_REACHED \n
RET_INVALID_ARGUMENTS \n
RET_INACCURATE_SOLUTION \n
RET_NO_SOLUTION */
returnValue init( const real_t* const _H, /**< Hessian matrix. */
const real_t* const _R, /**< Cholesky factorization of the Hessian matrix. */
const real_t* const _g, /**< Gradient vector. */
const real_t* const _lb, /**< Lower bounds (on variables). \n
If no lower bounds exist, a NULL pointer can be passed. */
const real_t* const _ub, /**< Upper bounds (on variables). \n
If no upper bounds exist, a NULL pointer can be passed. */
int& nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. \n
Output: Number of performed working set recalculations. */
const real_t* const yOpt = 0, /**< Initial guess for dual solution vector. */
real_t* const cputime = 0 /**< Output: CPU time required to initialise QP. */
);
/** Solves an initialised QProblemB using online active set strategy.
* \return SUCCESSFUL_RETURN \n
RET_MAX_NWSR_REACHED \n
RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
RET_HOTSTART_FAILED \n
RET_SHIFT_DETERMINATION_FAILED \n
RET_STEPDIRECTION_DETERMINATION_FAILED \n
RET_STEPLENGTH_DETERMINATION_FAILED \n
RET_HOMOTOPY_STEP_FAILED \n
RET_HOTSTART_STOPPED_INFEASIBILITY \n
RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
RET_INACCURATE_SOLUTION \n
RET_NO_SOLUTION */
returnValue hotstart( const real_t* const g_new, /**< Gradient of neighbouring QP to be solved. */
const real_t* const lb_new, /**< Lower bounds of neighbouring QP to be solved. \n
If no lower bounds exist, a NULL pointer can be passed. */
const real_t* const ub_new, /**< Upper bounds of neighbouring QP to be solved. \n
If no upper bounds exist, a NULL pointer can be passed. */
int& nWSR, /**< Input: Maximum number of working set recalculations; \n
Output: Number of performed working set recalculations. */
real_t* const cputime /**< Output: CPU time required to solve QP (or to perform nWSR iterations). */
);
/** Returns Hessian matrix of the QP (deep copy).
* \return SUCCESSFUL_RETURN */
inline returnValue getH( real_t* const _H /**< Array of appropriate dimension for copying Hessian matrix.*/
) const;
/** Returns gradient vector of the QP (deep copy).
* \return SUCCESSFUL_RETURN */
inline returnValue getG( real_t* const _g /**< Array of appropriate dimension for copying gradient vector.*/
) const;
/** Returns lower bound vector of the QP (deep copy).
* \return SUCCESSFUL_RETURN */
inline returnValue getLB( real_t* const _lb /**< Array of appropriate dimension for copying lower bound vector.*/
) const;
/** Returns single entry of lower bound vector of the QP.
* \return SUCCESSFUL_RETURN \n
RET_INDEX_OUT_OF_BOUNDS */
inline returnValue getLB( int number, /**< Number of entry to be returned. */
real_t& value /**< Output: lb[number].*/
) const;
/** Returns upper bound vector of the QP (deep copy).
* \return SUCCESSFUL_RETURN */
inline returnValue getUB( real_t* const _ub /**< Array of appropriate dimension for copying upper bound vector.*/
) const;
/** Returns single entry of upper bound vector of the QP.
* \return SUCCESSFUL_RETURN \n
RET_INDEX_OUT_OF_BOUNDS */
inline returnValue getUB( int number, /**< Number of entry to be returned. */
real_t& value /**< Output: ub[number].*/
) const;
/** Returns current bounds object of the QP (deep copy).
* \return SUCCESSFUL_RETURN */
inline returnValue getBounds( Bounds* const _bounds /** Output: Bounds object. */
) const;
/** Returns the number of variables.
* \return Number of variables. */
inline int getNV( ) const;
/** Returns the number of free variables.
* \return Number of free variables. */
inline int getNFR( );
/** Returns the number of fixed variables.
* \return Number of fixed variables. */
inline int getNFX( );
/** Returns the number of implicitly fixed variables.
* \return Number of implicitly fixed variables. */
inline int getNFV( ) const;
/** Returns the dimension of null space.
* \return Dimension of null space. */
int getNZ( );
/** Returns the optimal objective function value.
* \return finite value: Optimal objective function value (QP was solved) \n
+infinity: QP was not yet solved */
real_t getObjVal( ) const;
/** Returns the objective function value at an arbitrary point x.
* \return Objective function value at point x */
real_t getObjVal( const real_t* const _x /**< Point at which the objective function shall be evaluated. */
) const;
/** Returns the primal solution vector.
* \return SUCCESSFUL_RETURN \n
RET_QP_NOT_SOLVED */
returnValue getPrimalSolution( real_t* const xOpt /**< Output: Primal solution vector (if QP has been solved). */
) const;
/** Returns the dual solution vector.
* \return SUCCESSFUL_RETURN \n
RET_QP_NOT_SOLVED */
returnValue getDualSolution( real_t* const yOpt /**< Output: Dual solution vector (if QP has been solved). */
) const;
/** Returns status of the solution process.
* \return Status of solution process. */
inline QProblemStatus getStatus( ) const;
/** Returns if the QProblem object is initialised.
* \return BT_TRUE: QProblemB initialised \n
BT_FALSE: QProblemB not initialised */
inline BooleanType isInitialised( ) const;
/** Returns if the QP has been solved.
* \return BT_TRUE: QProblemB solved \n
BT_FALSE: QProblemB not solved */
inline BooleanType isSolved( ) const;
/** Returns if the QP is infeasible.
* \return BT_TRUE: QP infeasible \n
BT_FALSE: QP feasible (or not known to be infeasible!) */
inline BooleanType isInfeasible( ) const;
/** Returns if the QP is unbounded.
* \return BT_TRUE: QP unbounded \n
BT_FALSE: QP unbounded (or not known to be unbounded!) */
inline BooleanType isUnbounded( ) const;
/** Returns the print level.
* \return Print level. */
inline PrintLevel getPrintLevel( ) const;
/** Changes the print level.
* \return SUCCESSFUL_RETURN */
returnValue setPrintLevel( PrintLevel _printlevel /**< New print level. */
);
/** Returns Hessian type flag (type is not determined due to this call!).
* \return Hessian type. */
inline HessianType getHessianType( ) const;
/** Changes the print level.
* \return SUCCESSFUL_RETURN */
inline returnValue setHessianType( HessianType _hessianType /**< New Hessian type. */
);
/*
* PROTECTED MEMBER FUNCTIONS
*/
protected:
/** Checks if Hessian happens to be the identity matrix,
* and sets corresponding status flag (otherwise the flag remains unaltered!).
* \return SUCCESSFUL_RETURN */
returnValue checkForIdentityHessian( );
/** Determines type of constraints and bounds (i.e. implicitly fixed, unbounded etc.).
* \return SUCCESSFUL_RETURN \n
RET_SETUPSUBJECTTOTYPE_FAILED */
returnValue setupSubjectToType( );
/** Computes the Cholesky decomposition R of the (simply projected) Hessian (i.e. R^T*R = Z^T*H*Z).
* It only works in the case where Z is a simple projection matrix!
* \return SUCCESSFUL_RETURN \n
* RET_INDEXLIST_CORRUPTED */
returnValue setupCholeskyDecomposition( );
/** Solves a QProblemB whose QP data is assumed to be stored in the member variables.
* A guess for its primal/dual optimal solution vectors and the corresponding
* optimal working set can be provided.
* \return SUCCESSFUL_RETURN \n
RET_INIT_FAILED \n
RET_INIT_FAILED_CHOLESKY \n
RET_INIT_FAILED_HOTSTART \n
RET_INIT_FAILED_INFEASIBILITY \n
RET_INIT_FAILED_UNBOUNDEDNESS \n
RET_MAX_NWSR_REACHED */
returnValue solveInitialQP( const real_t* const xOpt, /**< Optimal primal solution vector.
* A NULL pointer can be passed. */
const real_t* const yOpt, /**< Optimal dual solution vector.
* A NULL pointer can be passed. */
const Bounds* const guessedBounds, /**< Guessed working set for solution (xOpt,yOpt).
* A NULL pointer can be passed. */
int& nWSR, /**< Input: Maximum number of working set recalculations; \n
* Output: Number of performed working set recalculations. */
real_t* const cputime /**< Output: CPU time required to solve QP (or to perform nWSR iterations). */
);
/** Obtains the desired working set for the auxiliary initial QP in
* accordance with the user specifications
* \return SUCCESSFUL_RETURN \n
RET_OBTAINING_WORKINGSET_FAILED \n
RET_INVALID_ARGUMENTS */
returnValue obtainAuxiliaryWorkingSet( const real_t* const xOpt, /**< Optimal primal solution vector.
* If a NULL pointer is passed, all entries are assumed to be zero. */
const real_t* const yOpt, /**< Optimal dual solution vector.
* If a NULL pointer is passed, all entries are assumed to be zero. */
const Bounds* const guessedBounds, /**< Guessed working set for solution (xOpt,yOpt). */
Bounds* auxiliaryBounds /**< Input: Allocated bound object. \n
* Ouput: Working set for auxiliary QP. */
) const;
/** Setups bound data structure according to auxiliaryBounds.
* (If the working set shall be setup afresh, make sure that
* bounds data structure has been resetted!)
* \return SUCCESSFUL_RETURN \n
RET_SETUP_WORKINGSET_FAILED \n
RET_INVALID_ARGUMENTS \n
RET_UNKNOWN BUG */
returnValue setupAuxiliaryWorkingSet( const Bounds* const auxiliaryBounds, /**< Working set for auxiliary QP. */
BooleanType setupAfresh /**< Flag indicating if given working set shall be
* setup afresh or by updating the current one. */
);
/** Setups the optimal primal/dual solution of the auxiliary initial QP.
* \return SUCCESSFUL_RETURN */
returnValue setupAuxiliaryQPsolution( const real_t* const xOpt, /**< Optimal primal solution vector.
* If a NULL pointer is passed, all entries are set to zero. */
const real_t* const yOpt /**< Optimal dual solution vector.
* If a NULL pointer is passed, all entries are set to zero. */
);
/** Setups gradient of the auxiliary initial QP for given
* optimal primal/dual solution and given initial working set
* (assumes that members X, Y and BOUNDS have already been initialised!).
* \return SUCCESSFUL_RETURN */
returnValue setupAuxiliaryQPgradient( );
/** Setups bounds of the auxiliary initial QP for given
* optimal primal/dual solution and given initial working set
* (assumes that members X, Y and BOUNDS have already been initialised!).
* \return SUCCESSFUL_RETURN \n
RET_UNKNOWN BUG */
returnValue setupAuxiliaryQPbounds( BooleanType useRelaxation /**< Flag indicating if inactive bounds shall be relaxed. */
);
/** Adds a bound to active set (specialised version for the case where no constraints exist).
* \return SUCCESSFUL_RETURN \n
RET_ADDBOUND_FAILED */
returnValue addBound( int number, /**< Number of bound to be added to active set. */
SubjectToStatus B_status, /**< Status of new active bound. */
BooleanType updateCholesky /**< Flag indicating if Cholesky decomposition shall be updated. */
);
/** Removes a bounds from active set (specialised version for the case where no constraints exist).
* \return SUCCESSFUL_RETURN \n
RET_HESSIAN_NOT_SPD \n
RET_REMOVEBOUND_FAILED */
returnValue removeBound( int number, /**< Number of bound to be removed from active set. */
BooleanType updateCholesky /**< Flag indicating if Cholesky decomposition shall be updated. */
);
/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix.
* \return SUCCESSFUL_RETURN \n
RET_DIV_BY_ZERO */
returnValue backsolveR( const real_t* const b, /**< Right hand side vector. */
BooleanType transposed, /**< Indicates if the transposed system shall be solved. */
real_t* const a /**< Output: Solution vector */
);
/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix. \n
* Special variant for the case that this function is called from within "removeBound()".
* \return SUCCESSFUL_RETURN \n
RET_DIV_BY_ZERO */
returnValue backsolveR( const real_t* const b, /**< Right hand side vector. */
BooleanType transposed, /**< Indicates if the transposed system shall be solved. */
BooleanType removingBound, /**< Indicates if function is called from "removeBound()". */
real_t* const a /**< Output: Solution vector */
);
/** Determines step direction of the shift of the QP data.
* \return SUCCESSFUL_RETURN */
returnValue hotstart_determineDataShift(const int* const FX_idx, /**< Index array of fixed variables. */
const real_t* const g_new, /**< New gradient vector. */
const real_t* const lb_new, /**< New lower bounds. */
const real_t* const ub_new, /**< New upper bounds. */
real_t* const delta_g, /**< Output: Step direction of gradient vector. */
real_t* const delta_lb, /**< Output: Step direction of lower bounds. */
real_t* const delta_ub, /**< Output: Step direction of upper bounds. */
BooleanType& Delta_bB_isZero/**< Output: Indicates if active bounds are to be shifted. */
);
/** Checks if lower/upper bounds remain consistent
* (i.e. if lb <= ub) during the current step.
* \return BT_TRUE iff bounds remain consistent
*/
BooleanType areBoundsConsistent( const real_t* const delta_lb, /**< Step direction of lower bounds. */
const real_t* const delta_ub /**< Step direction of upper bounds. */
) const;
/** Setups internal QP data.
* \return SUCCESSFUL_RETURN \n
RET_INVALID_ARGUMENTS */
returnValue setupQPdata( const real_t* const _H, /**< Hessian matrix. */
const real_t* const _R, /**< Cholesky factorization of the Hessian matrix. */
const real_t* const _g, /**< Gradient vector. */
const real_t* const _lb, /**< Lower bounds (on variables). \n
If no lower bounds exist, a NULL pointer can be passed. */
const real_t* const _ub /**< Upper bounds (on variables). \n
If no upper bounds exist, a NULL pointer can be passed. */
);
/** Sets Hessian matrix of the QP.
* \return SUCCESSFUL_RETURN */
inline returnValue setH( const real_t* const H_new /**< New Hessian matrix (with correct dimension!). */
);
/** Changes gradient vector of the QP.
* \return SUCCESSFUL_RETURN */
inline returnValue setG( const real_t* const g_new /**< New gradient vector (with correct dimension!). */
);
/** Changes lower bound vector of the QP.
* \return SUCCESSFUL_RETURN */
inline returnValue setLB( const real_t* const lb_new /**< New lower bound vector (with correct dimension!). */
);
/** Changes single entry of lower bound vector of the QP.
* \return SUCCESSFUL_RETURN \n
RET_INDEX_OUT_OF_BOUNDS */
inline returnValue setLB( int number, /**< Number of entry to be changed. */
real_t value /**< New value for entry of lower bound vector. */
);
/** Changes upper bound vector of the QP.
* \return SUCCESSFUL_RETURN */
inline returnValue setUB( const real_t* const ub_new /**< New upper bound vector (with correct dimension!). */
);
/** Changes single entry of upper bound vector of the QP.
* \return SUCCESSFUL_RETURN \n
RET_INDEX_OUT_OF_BOUNDS */
inline returnValue setUB( int number, /**< Number of entry to be changed. */
real_t value /**< New value for entry of upper bound vector. */
);
/** Computes parameters for the Givens matrix G for which [x,y]*G = [z,0]
* \return SUCCESSFUL_RETURN */
inline void computeGivens( real_t xold, /**< Matrix entry to be normalised. */
real_t yold, /**< Matrix entry to be annihilated. */
real_t& xnew, /**< Output: Normalised matrix entry. */
real_t& ynew, /**< Output: Annihilated matrix entry. */
real_t& c, /**< Output: Cosine entry of Givens matrix. */
real_t& s /**< Output: Sine entry of Givens matrix. */
) const;
/** Applies Givens matrix determined by c and s (cf. computeGivens).
* \return SUCCESSFUL_RETURN */
inline void applyGivens( real_t c, /**< Cosine entry of Givens matrix. */
real_t s, /**< Sine entry of Givens matrix. */
real_t xold, /**< Matrix entry to be transformed corresponding to
* the normalised entry of the original matrix. */
real_t yold, /**< Matrix entry to be transformed corresponding to
* the annihilated entry of the original matrix. */
real_t& xnew, /**< Output: Transformed matrix entry corresponding to
* the normalised entry of the original matrix. */
real_t& ynew /**< Output: Transformed matrix entry corresponding to
* the annihilated entry of the original matrix. */
) const;
/*
* PRIVATE MEMBER FUNCTIONS
*/
private:
/** Determines step direction of the homotopy path.
* \return SUCCESSFUL_RETURN \n
RET_STEPDIRECTION_FAILED_CHOLESKY */
returnValue hotstart_determineStepDirection(const int* const FR_idx, /**< Index array of free variables. */
const int* const FX_idx, /**< Index array of fixed variables. */
const real_t* const delta_g, /**< Step direction of gradient vector. */
const real_t* const delta_lb, /**< Step direction of lower bounds. */
const real_t* const delta_ub, /**< Step direction of upper bounds. */
BooleanType Delta_bB_isZero, /**< Indicates if active bounds are to be shifted. */
real_t* const delta_xFX, /**< Output: Primal homotopy step direction of fixed variables. */
real_t* const delta_xFR, /**< Output: Primal homotopy step direction of free variables. */
real_t* const delta_yFX /**< Output: Dual homotopy step direction of fixed variables' multiplier. */
);
/** Determines the maximum possible step length along the homotopy path.
* \return SUCCESSFUL_RETURN */
returnValue hotstart_determineStepLength( const int* const FR_idx, /**< Index array of free variables. */
const int* const FX_idx, /**< Index array of fixed variables. */
const real_t* const delta_lb, /**< Step direction of lower bounds. */
const real_t* const delta_ub, /**< Step direction of upper bounds. */
const real_t* const delta_xFR, /**< Primal homotopy step direction of free variables. */
const real_t* const delta_yFX, /**< Dual homotopy step direction of fixed variables' multiplier. */
int& BC_idx, /**< Output: Index of blocking constraint. */
SubjectToStatus& BC_status /**< Output: Status of blocking constraint. */
);
/** Performs a step along the homotopy path (and updates active set).
* \return SUCCESSFUL_RETURN \n
RET_OPTIMAL_SOLUTION_FOUND \n
RET_REMOVE_FROM_ACTIVESET_FAILED \n
RET_ADD_TO_ACTIVESET_FAILED \n
RET_QP_INFEASIBLE */
returnValue hotstart_performStep( const int* const FR_idx, /**< Index array of free variables. */
const int* const FX_idx, /**< Index array of fixed variables. */
const real_t* const delta_g, /**< Step direction of gradient vector. */
const real_t* const delta_lb, /**< Step direction of lower bounds. */
const real_t* const delta_ub, /**< Step direction of upper bounds. */
const real_t* const delta_xFX, /**< Primal homotopy step direction of fixed variables. */
const real_t* const delta_xFR, /**< Primal homotopy step direction of free variables. */
const real_t* const delta_yFX, /**< Dual homotopy step direction of fixed variables' multiplier. */
int BC_idx, /**< Index of blocking constraint. */
SubjectToStatus BC_status /**< Status of blocking constraint. */
);
#ifdef PC_DEBUG /* Define print functions only for debugging! */
/** Prints concise information on the current iteration.
* \return SUCCESSFUL_RETURN \n */
returnValue printIteration( int iteration, /**< Number of current iteration. */
int BC_idx, /**< Index of blocking bound. */
SubjectToStatus BC_status /**< Status of blocking bound. */
);
#endif /* PC_DEBUG */
/** Determines the maximum violation of the KKT optimality conditions
* of the current iterate within the QProblemB object.
* \return SUCCESSFUL_RETURN \n
* RET_INACCURATE_SOLUTION \n
* RET_NO_SOLUTION */
returnValue checkKKTconditions( );
/*
* PROTECTED MEMBER VARIABLES
*/
protected:
real_t H[NVMAX*NVMAX]; /**< Hessian matrix. */
BooleanType hasHessian; /**< Flag indicating whether H contains Hessian or corresponding Cholesky factor R; \sa init. */
real_t g[NVMAX]; /**< Gradient. */
real_t lb[NVMAX]; /**< Lower bound vector (on variables). */
real_t ub[NVMAX]; /**< Upper bound vector (on variables). */
Bounds bounds; /**< Data structure for problem's bounds. */
real_t R[NVMAX*NVMAX]; /**< Cholesky decomposition of H (i.e. H = R^T*R). */
BooleanType hasCholesky; /**< Flag indicating whether Cholesky decomposition has already been setup. */
real_t x[NVMAX]; /**< Primal solution vector. */
real_t y[NVMAX+NCMAX]; /**< Dual solution vector. */
real_t tau; /**< Last homotopy step length. */
QProblemStatus status; /**< Current status of the solution process. */
BooleanType infeasible; /**< QP infeasible? */
BooleanType unbounded; /**< QP unbounded? */
HessianType hessianType; /**< Type of Hessian matrix. */
PrintLevel printlevel; /**< Print level. */
int count; /**< Counts the number of hotstart function calls (internal usage only!). */
};
#include <QProblemB.ipp>
#endif /* QPOASES_QPROBLEMB_HPP */
/*
* end of file
*/
================================================
FILE: externals/qpoases/INCLUDE/SubjectTo.hpp
================================================
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file INCLUDE/SubjectTo.hpp
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*
* Declaration of the SubjectTo class designed to manage working sets of
* constraints and bounds within a QProblem.
*/
#ifndef QPOASES_SUBJECTTO_HPP
#define QPOASES_SUBJECTTO_HPP
#include <Indexlist.hpp>
/** This class manages working sets of constraints and bounds by storing
* index sets and other status information.
*
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*/
class SubjectTo
{
/*
* PUBLIC MEMBER FUNCTIONS
*/
public:
/** Default constructor. */
SubjectTo( );
/** Copy constructor (deep copy). */
SubjectTo( const SubjectTo& rhs /**< Rhs object. */
);
/** Destructor. */
~SubjectTo( );
/** Assignment operator (deep copy). */
SubjectTo& operator=( const SubjectTo& rhs /**< Rhs object. */
);
/** Pseudo-constructor takes the number of constraints or bounds.
* \return SUCCESSFUL_RETURN */
returnValue init( int n /**< Number of constraints or bounds. */
);
/** Returns type of (constraints') bound.
* \return Type of (constraints') bound \n
RET_INDEX_OUT_OF_BOUNDS */
inline SubjectToType getType( int i /**< Number of (constraints') bound. */
) const ;
/** Returns status of (constraints') bound.
* \return Status of (constraints') bound \n
ST_UNDEFINED */
inline SubjectToStatus getStatus( int i /**< Number of (constraints') bound. */
) const;
/** Sets type of (constraints') bound.
* \return SUCCESSFUL_RETURN \n
RET_INDEX_OUT_OF_BOUNDS */
inline returnValue setType( int i, /**< Number of (constraints') bound. */
SubjectToType value /**< Type of (constraints') bound. */
);
/** Sets status of (constraints') bound.
* \return SUCCESSFUL_RETURN \n
RET_INDEX_OUT_OF_BOUNDS */
inline returnValue setStatus( int i, /**< Number of (constraints') bound. */
SubjectToStatus value /**< Status of (constraints') bound. */
);
/** Sets status of lower (constraints') bounds. */
inline void setNoLower( BooleanType _status /**< Status of lower (constraints') bounds. */
);
/** Sets status of upper (constraints') bounds. */
inline void setNoUpper( BooleanType _status /**< Status of upper (constraints') bounds. */
);
/** Returns status of lower (constraints') bounds.
* \return BT_TRUE if there is no lower (constraints') bound on any variable. */
inline BooleanType isNoLower( ) const;
/** Returns status of upper bounds.
* \return BT_TRUE if there is no upper (constraints') bound on any variable. */
inline BooleanType isNoUpper( ) const;
/*
* PROTECTED MEMBER FUNCTIONS
*/
protected:
/** Adds the index of a new constraint or bound to index set.
* \return SUCCESSFUL_RETURN \n
RET_ADDINDEX_FAILED */
returnValue addIndex( Indexlist* const indexlist, /**< Index list to which the new index shall be added. */
int newnumber, /**< Number of new constraint or bound. */
SubjectToStatus newstatus /**< Status of new constraint or bound. */
);
/** Removes the index of a constraint or bound from index set.
* \return SUCCESSFUL_RETURN \n
RET_UNKNOWN_BUG */
returnValue removeIndex( Indexlist* const indexlist, /**< Index list from which the new index shall be removed. */
int removenumber /**< Number of constraint or bound to be removed. */
);
/** Swaps the indices of two constraints or bounds within the index set.
* \return SUCCESSFUL_RETURN \n
RET_SWAPINDEX_FAILED */
returnValue swapIndex( Indexlist* const indexlist, /**< Index list in which the indices shold be swapped. */
int number1, /**< Number of first constraint or bound. */
int number2 /**< Number of second constraint or bound. */
);
/*
* PROTECTED MEMBER VARIABLES
*/
protected:
SubjectToType type[NVMAX+NCMAX]; /**< Type of constraints/bounds. */
SubjectToStatus status[NVMAX+NCMAX]; /**< Status of constraints/bounds. */
BooleanType noLower; /**< This flag indicates if there is no lower bound on any variable. */
BooleanType noUpper; /**< This flag indicates if there is no upper bound on any variable. */
/*
* PRIVATE MEMBER VARIABLES
*/
private:
int size;
};
#include <SubjectTo.ipp>
#endif /* QPOASES_SUBJECTTO_HPP */
/*
* end of file
*/
================================================
FILE: externals/qpoases/INCLUDE/Types.hpp
================================================
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file INCLUDE/Types.hpp
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2008
*
* Declaration of all non-built-in types (except for classes).
*/
#ifndef QPOASES_TYPES_HPP
#define QPOASES_TYPES_HPP
/** Define real_t for facilitating switching between double and float. */
// typedef double real_t;
/** Summarises all possible logical values. */
enum BooleanType
{
BT_FALSE, /**< Logical value for "false". */
BT_TRUE /**< Logical value for "true". */
};
/** Summarises all possible print levels. Print levels are used to describe
* the desired amount of output during runtime of qpOASES. */
enum PrintLevel
{
PL_NONE, /**< No output. */
PL_LOW, /**< Print error messages only. */
PL_MEDIUM, /**< Print error and warning messages as well as concise info messages. */
PL_HIGH /**< Print all messages with full details. */
};
/** Defines visibility status of a message. */
enum VisibilityStatus
{
VS_VISIBLE, /**< Message visible. */
VS_HIDDEN /**< Message not visible. */
};
/** Summarises all possible states of the (S)QProblem(B) object during the
solution process of a QP sequence. */
enum QProblemStatus
{
QPS_NOTINITIALISED, /**< QProblem object is freshly instantiated or reset. */
QPS_PREPARINGAUXILIARYQP, /**< An auxiliary problem is currently setup, either at the very beginning
* via an initial homotopy or after changing the QP matrices. */
QPS_AUXILIARYQPSOLVED, /**< An auxilary problem was solved, either at the very beginning
* via an initial homotopy or after changing the QP matrices. */
QPS_PERFORMINGHOMOTOPY, /**< A homotopy according to the main idea of the online active
* set strategy is performed. */
QPS_HOMOTOPYQPSOLVED, /**< An intermediate QP along the homotopy path was solved. */
QPS_SOLVED /**< The solution of the actual QP was found. */
};
/** Summarises all possible types of bounds and constraints. */
enum SubjectToType
{
ST_UNBOUNDED, /**< Bound/constraint is unbounded. */
ST_BOUNDED, /**< Bound/constraint is bounded but not fixed. */
ST_EQUALITY, /**< Bound/constraint is fixed (implicit equality bound/constraint). */
ST_UNKNOWN /**< Type of bound/constraint unknown. */
};
/** Summarises all possible states of bounds and constraints. */
enum SubjectToStatus
{
ST_INACTIVE, /**< Bound/constraint is inactive. */
ST_LOWER, /**< Bound/constraint is at its lower bound. */
ST_UPPER, /**< Bound/constraint is at its upper bound. */
ST_UNDEFINED /**< Status of bound/constraint undefined. */
};
/** Summarises all possible cycling states of bounds and constraints. */
enum CyclingStatus
{
CYC_NOT_INVOLVED, /**< Bound/constraint is not involved in current cycling. */
CYC_PREV_ADDED, /**< Bound/constraint has previously been added during the current cycling. */
CYC_PREV_REMOVED /**< Bound/constraint has previously been removed during the current cycling. */
};
/** Summarises all possible types of the QP's Hessian matrix. */
enum HessianType
{
HST_SEMIDEF, /**< Hessian is positive semi-definite. */
HST_POSDEF_NULLSPACE, /**< Hessian is positive definite on null space of active bounds/constraints. */
HST_POSDEF, /**< Hessian is (strictly) positive definite. */
HST_IDENTITY /**< Hessian is identity matrix. */
};
#endif /* QPOASES_TYPES_HPP */
/*
* end of file
*/
================================================
FILE: externals/qpoases/INCLUDE/Utils.hpp
================================================
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file INCLUDE/Utils.hpp
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*
* Declaration of global utility functions for working with qpOASES.
*/
#ifndef QPOASES_UTILS_HPP
#define QPOASES_UTILS_HPP
#include <MessageHandling.hpp>
#ifdef PC_DEBUG /* Define print functions only for debugging! */
/** Prints a vector.
* \return SUCCESSFUL_RETURN */
returnValue print( const real_t* const v, /**< Vector to be printed. */
int n /**< Length of vector. */
);
/** Prints a permuted vector.
* \return SUCCESSFUL_RETURN */
returnValue print( const real_t* const v, /**< Vector to be printed. */
int n, /**< Length of vector. */
const int* const V_idx /**< Pemutation vector. */
);
/** Prints a named vector.
* \return SUCCESSFUL_RETURN */
returnValue print( const real_t* const v, /**< Vector to be printed. */
int n, /**< Length of vector. */
const char* name /** Name of vector. */
);
/** Prints a matrix.
* \return SUCCESSFUL_RETURN */
returnValue print( const real_t* const M, /**< Matrix to be printed. */
int nrow, /**< Row number of matrix. */
int ncol /**< Column number of matrix. */
);
/** Prints a permuted matrix.
* \return SUCCESSFUL_RETURN */
returnValue print( const real_t* const M, /**< Matrix to be printed. */
int nrow, /**< Row number of matrix. */
int ncol , /**< Column number of matrix. */
const int* const ROW_idx, /**< Row pemutation vector. */
const int* const COL_idx /**< Column pemutation vector. */
);
/** Prints a named matrix.
* \return SUCCESSFUL_RETURN */
returnValue print( const real_t* const M, /**< Matrix to be printed. */
int nrow, /**< Row number of matrix. */
int ncol, /**< Column number of matrix. */
const char* name /** Name of matrix. */
);
/** Prints an index array.
* \return SUCCESSFUL_RETURN */
returnValue print( const int* const index, /**< Index array to be printed. */
int n /**< Length of index array. */
);
/** Prints a named index array.
* \return SUCCESSFUL_RETURN */
returnValue print( const int* const index, /**< Index array to be printed. */
int n, /**< Length of index array. */
const char* name /**< Name of index array. */
);
/** Prints a string to desired output target (useful also for MATLAB output!).
* \return SUCCESSFUL_RETURN */
returnValue myPrintf( const char* s /**< String to be written. */
);
/** Prints qpOASES copyright notice.
* \return SUCCESSFUL_RETURN */
returnValue printCopyrightNotice( );
/** Reads a real_t matrix from file.
* \return SUCCESSFUL_RETURN \n
RET_UNABLE_TO_OPEN_FILE \n
RET_UNABLE_TO_READ_FILE */
returnValue readFromFile( real_t* data, /**< Matrix to be read from file. */
int nrow, /**< Row number of matrix. */
int ncol, /**< Column number of matrix. */
const char* datafilename /**< Data file name. */
);
/** Reads a real_t vector from file.
* \return SUCCESSFUL_RETURN \n
RET_UNABLE_TO_OPEN_FILE \n
RET_UNABLE_TO_READ_FILE */
returnValue readFromFile( real_t* data, /**< Vector to be read from file. */
int n, /**< Length of vector. */
const char* datafilename /**< Data file name. */
);
/** Reads an integer (column) vector from file.
* \return SUCCESSFUL_RETURN \n
RET_UNABLE_TO_OPEN_FILE \n
RET_UNABLE_TO_READ_FILE */
returnValue readFromFile( int* data, /**< Vector to be read from file. */
int n, /**< Length of vector. */
const char* datafilename /**< Data file name. */
);
/** Writes a real_t matrix into a file.
* \return SUCCESSFUL_RETURN \n
RET_UNABLE_TO_OPEN_FILE */
returnValue writeIntoFile( const real_t* const data, /**< Matrix to be written into file. */
int nrow, /**< Row number of matrix. */
int ncol, /**< Column number of matrix. */
const char* datafilename, /**< Data file name. */
BooleanType append /**< Indicates if data shall be appended if the file already exists (otherwise it is overwritten). */
);
/** Writes a real_t vector into a file.
* \return SUCCESSFUL_RETURN \n
RET_UNABLE_TO_OPEN_FILE */
returnValue writeIntoFile( const real_t* const data, /**< Vector to be written into file. */
int n, /**< Length of vector. */
const char* datafilename, /**< Data file name. */
BooleanType append /**< Indicates if data shall be appended if the file already exists (otherwise it is overwritten). */
);
/** Writes an integer (column) vector into a file.
* \return SUCCESSFUL_RETURN \n
RET_UNABLE_TO_OPEN_FILE */
returnValue writeIntoFile( const int* const integer, /**< Integer vector to be written into file. */
int n, /**< Length of vector. */
const char* datafilename, /**< Data file name. */
BooleanType append /**< Indicates if integer shall be appended if the file already exists (otherwise it is overwritten). */
);
#endif /* PC_DEBUG */
/** Returns the current system time.
* \return current system time */
real_t getCPUtime( );
/** Returns the Euclidean norm of a vector.
* \return 0: successful */
real_t getNorm( const real_t* const v, /**< Vector. */
int n /**< Vector's dimension. */
);
/** Returns the absolute value of a real_t.
* \return Absolute value of a real_t */
inline real_t getAbs( real_t x /**< Input argument. */
);
#include <Utils.ipp>
#endif /* QPOASES_UTILS_HPP */
/*
* end of file
*/
================================================
FILE: externals/qpoases/LICENSE.txt
================================================
GNU LESSER GENERAL PUBLIC LICENSE
Version 2.1, February 1999
Copyright (C) 1991, 1999 Free Software Foundation, Inc.
51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
Everyone is permitted to copy and distribute verbatim copies
of this license document, but changing it is not allowed.
[This is the first released version of the Lesser GPL. It also counts
as the successor of the GNU Library Public License, version 2, hence
the version number 2.1.]
Preamble
The licenses for most software are designed to take away your
freedom to share and change it. By contrast, the GNU General Public
Licenses are intended to guarantee your freedom to share and change
free software--to make sure the software is free for all its users.
This license, the Lesser General Public License, applies to some
specially designated software packages--typically libraries--of the
Free Software Foundation and other authors who decide to use it. You
can use it too, but we suggest you first think carefully about whether
this license or the ordinary General Public License is the better
strategy to use in any particular case, based on the explanations below.
When we speak of free software, we are referring to freedom of use,
not price. Our General Public Licenses are designed to make sure that
you have the freedom to distribute copies of free software (and charge
for this service if you wish); that you receive source code or can get
it if you want it; that you can change the software and use pieces of
it in new free programs; and that you are informed that you can do
these things.
To protect your rights, we need to make restrictions that forbid
distributors to deny you these rights or to ask you to surrender these
rights. These restrictions translate to certain responsibilities for
you if you distribute copies of the library or if you modify it.
For example, if you distribute copies of the library, whether gratis
or for a fee, you must give the recipients all the rights that we gave
you. You must make sure that they, too, receive or can get the source
code. If you link other code with the library, you must provide
complete object files to the recipients, so that they can relink them
with the library after making changes to the library and recompiling
it. And you must show them these terms so they know their rights.
We protect your rights with a two-step method: (1) we copyright the
library, and (2) we offer you this license, which gives you legal
permission to copy, distribute and/or modify the library.
To protect each distributor, we want to make it very clear that
there is no warranty for the free library. Also, if the library is
modified by someone else and passed on, the recipients should know
that what they have is not the original version, so that the original
author's reputation will not be affected by problems that might be
introduced by others.
Finally, software patents pose a constant threat to the existence of
any free program. We wish to make sure that a company cannot
effectively restrict the users of a free program by obtaining a
restrictive license from a patent holder. Therefore, we insist that
any patent license obtained for a version of the library must be
consistent with the full freedom of use specified in this license.
Most GNU software, including some libraries, is covered by the
ordinary GNU General Public License. This license, the GNU Lesser
General Public License, applies to certain designated libraries, and
is quite different from the ordinary General Public License. We use
this license for certain libraries in order to permit linking those
libraries into non-free programs.
When a program is linked with a library, whether statically or using
a shared library, the combination of the two is legally speaking a
combined work, a derivative of the original library. The ordinary
General Public License therefore permits such linking only if the
entire combination fits its criteria of freedom. The Lesser General
Public License permits more lax criteria for linking other code with
the library.
We call this license the "Lesser" General Public License because it
does Less to protect the user's freedom than the ordinary General
Public License. It also provides other free software developers Less
of an advantage over competing non-free programs. These disadvantages
are the reason we use the ordinary General Public License for many
libraries. However, the Lesser license provides advantages in certain
special circumstances.
For example, on rare occasions, there may be a special need to
encourage the widest possible use of a certain library, so that it becomes
a de-facto standard. To achieve this, non-free programs must be
allowed to use the library. A more frequent case is that a free
library does the same job as widely used non-free libraries. In this
case, there is little to gain by limiting the free library to free
software only, so we use the Lesser General Public License.
In other cases, permission to use a particular library in non-free
programs enables a greater number of people to use a large body of
free software. For example, permission to use the GNU C Library in
non-free programs enables many more people to use the whole GNU
operating system, as well as its variant, the GNU/Linux operating
system.
Although the Lesser General Public License is Less protective of the
users' freedom, it does ensure that the user of a program that is
linked with the Library has the freedom and the wherewithal to run
that program using a modified version of the Library.
The precise terms and conditions for copying, distribution and
modification follow. Pay close attention to the difference between a
"work based on the library" and a "work that uses the library". The
former contains code derived from the library, whereas the latter must
be combined with the library in order to run.
GNU LESSER GENERAL PUBLIC LICENSE
TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
0. This License Agreement applies to any software library or other
program which contains a notice placed by the copyright holder or
other authorized party saying it may be distributed under the terms of
this Lesser General Public License (also called "this License").
Each licensee is addressed as "you".
A "library" means a collection of software functions and/or data
prepared so as to be conveniently linked with application programs
(which use some of those functions and data) to form executables.
The "Library", below, refers to any such software library or work
which has been distributed under these terms. A "work based on the
Library" means either the Library or any derivative work under
copyright law: that is to say, a work containing the Library or a
portion of it, either verbatim or with modifications and/or translated
straightforwardly into another language. (Hereinafter, translation is
included without limitation in the term "modification".)
"Source code" for a work means the preferred form of the work for
making modifications to it. For a library, complete source code means
all the source code for all modules it contains, plus any associated
interface definition files, plus the scripts used to control compilation
and installation of the library.
Activities other than copying, distribution and modification are not
covered by this License; they are outside its scope. The act of
running a program using the Library is not restricted, and output from
such a program is covered only if its contents constitute a work based
on the Library (independent of the use of the Library in a tool for
writing it). Whether that is true depends on what the Library does
and what the program that uses the Library does.
1. You may copy and distribute verbatim copies of the Library's
complete source code as you receive it, in any medium, provided that
you conspicuously and appropriately publish on each copy an
appropriate copyright notice and disclaimer of warranty; keep intact
all the notices that refer to this License and to the absence of any
warranty; and distribute a copy of this License along with the
Library.
You may charge a fee for the physical act of transferring a copy,
and you may at your option offer warranty protection in exchange for a
fee.
2. You may modify your copy or copies of the Library or any portion
of it, thus forming a work based on the Library, and copy and
distribute such modifications or work under the terms of Section 1
above, provided that you also meet all of these conditions:
a) The modified work must itself be a software library.
b) You must cause the files modified to carry prominent notices
stating that you changed the files and the date of any change.
c) You must cause the whole of the work to be licensed at no
charge to all third parties under the terms of this License.
d) If a facility in the modified Library refers to a function or a
table of data to be supplied by an application program that uses
the facility, other than as an argument passed when the facility
is invoked, then you must make a good faith effort to ensure that,
in the event an application does not supply such function or
table, the facility still operates, and performs whatever part of
its purpose remains meaningful.
(For example, a function in a library to compute square roots has
a purpose that is entirely well-defined independent of the
application. Therefore, Subsection 2d requires that any
application-supplied function or table used by this function must
be optional: if the application does not supply it, the square
root function must still compute square roots.)
These requirements apply to the modified work as a whole. If
identifiable sections of that work are not derived from the Library,
and can be reasonably considered independent and separate works in
themselves, then this License, and its terms, do not apply to those
sections when you distribute them as separate works. But when you
distribute the same sections as part of a whole which is a work based
on the Library, the distribution of the whole must be on the terms of
this License, whose permissions for other licensees extend to the
entire whole, and thus to each and every part regardless of who wrote
it.
Thus, it is not the intent of this section to claim rights or contest
your rights to work written entirely by you; rather, the intent is to
exercise the right to control the distribution of derivative or
collective works based on the Library.
In addition, mere aggregation of another work not based on the Library
with the Library (or with a work based on the Library) on a volume of
a storage or distribution medium does not bring the other work under
the scope of this License.
3. You may opt to apply the terms of the ordinary GNU General Public
License instead of this License to a given copy of the Library. To do
this, you must alter all the notices that refer to this License, so
that they refer to the ordinary GNU General Public License, version 2,
instead of to this License. (If a newer version than version 2 of the
ordinary GNU General Public License has appeared, then you can specify
that version instead if you wish.) Do not make any other change in
these notices.
Once this change is made in a given copy, it is irreversible for
that copy, so the ordinary GNU General Public License applies to all
subsequent copies and derivative works made from that copy.
This option is useful when you wish to copy part of the code of
the Library into a program that is not a library.
4. You may copy and distribute the Library (or a portion or
derivative of it, under Section 2) in object code or executable form
under the terms of Sections 1 and 2 above provided that you accompany
it with the complete corresponding machine-readable source code, which
must be distributed under the terms of Sections 1 and 2 above on a
medium customarily used for software interchange.
If distribution of object code is made by offering access to copy
from a designated place, then offering equivalent access to copy the
source code from the same place satisfies the requirement to
distribute the source code, even though third parties are not
compelled to copy the source along with the object code.
5. A program that contains no derivative of any portion of the
Library, but is designed to work with the Library by being compiled or
linked with it, is called a "work that uses the Library". Such a
work, in isolation, is not a derivative work of the Library, and
therefore falls outside the scope of this License.
However, linking a "work that uses the Library" with the Library
creates an executable that is a derivative of the Library (because it
contains portions of the Library), rather than a "work that uses the
library". The executable is therefore covered by this License.
Section 6 states terms for distribution of such executables.
When a "work that uses the Library" uses material from a header file
that is part of the Library, the object code for the work may be a
derivative work of the Library even though the source code is not.
Whether this is true is especially significant if the work can be
linked without the Library, or if the work is itself a library. The
threshold for this to be true is not precisely defined by law.
If such an object file uses only numerical parameters, data
structure layouts and accessors, and small macros and small inline
functions (ten lines or less in length), then the use of the object
file is unrestricted, regardless of whether it is legally a derivative
work. (Executables containing this object code plus portions of the
Library will still fall under Section 6.)
Otherwise, if the work is a derivative of the Library, you may
distribute the object code for the work under the terms of Section 6.
Any executables containing that work also fall under Section 6,
whether or not they are linked directly with the Library itself.
6. As an exception to the Sections above, you may also combine or
link a "work that uses the Library" with the Library to produce a
work containing portions of the Library, and distribute that work
under terms of your choice, provided that the terms permit
modification of the work for the customer's own use and reverse
engineering for debugging such modifications.
You must give prominent notice with each copy of the work that the
Library is used in it and that the Library and its use are covered by
this License. You must supply a copy of this License. If the work
during execution displays copyright notices, you must include the
copyright notice for the Library among them, as well as a reference
directing the user to the copy of this License. Also, you must do one
of these things:
a) Accompany the work with the complete corresponding
machine-readable source code for the Library including whatever
changes were used in the work (which must be distributed under
Sections 1 and 2 above); and, if the work is an executable linked
with the Library, with the complete machine-readable "work that
uses the Library", as object code and/or source code, so that the
user can modify the Library and then relink to produce a modified
executable containing the modified Library. (It is understood
that the user who changes the contents of definitions files in the
Library will not necessarily be able to recompile the application
to use the modified definitions.)
b) Use a suitable shared library mechanism for linking with the
Library. A suitable mechanism is one that (1) uses at run time a
copy of the library already present on the user's computer system,
rather than copying library functions into the executable, and (2)
will operate properly with a modified version of the library, if
the user installs one, as long as the modified version is
interface-compatible with the version that the work was made with.
c) Accompany the work with a written offer, valid for at
least three years, to give the same user the materials
specified in Subsection 6a, above, for a charge no more
than the cost of performing this distribution.
d) If distribution of the work is made by offering access to copy
from a designated place, offer equivalent access to copy the above
specified materials from the same place.
e) Verify that the user has already received a copy of these
materials or that you have already sent this user a copy.
For an executable, the required form of the "work that uses the
Library" must include any data and utility programs needed for
reproducing the executable from it. However, as a special exception,
the materials to be distributed need not include anything that is
normally distributed (in either source or binary form) with the major
components (compiler, kernel, and so on) of the operating system on
which the executable runs, unless that component itself accompanies
the executable.
It may happen that this requirement contradicts the license
restrictions of other proprietary libraries that do not normally
accompany the operating system. Such a contradiction means you cannot
use both them and the Library together in an executable that you
distribute.
7. You may place library facilities that are a work based on the
Library side-by-side in a single library together with other library
facilities not covered by this License, and distribute such a combined
library, provided that the separate distribution of the work based on
the Library and of the other library facilities is otherwise
permitted, and provided that you do these two things:
a) Accompany the combined library with a copy of the same work
based on the Library, uncombined with any other library
facilities. This must be distributed under the terms of the
Sections above.
b) Give prominent notice with the combined library of the fact
that part of it is a work based on the Library, and explaining
where to find the accompanying uncombined form of the same work.
8. You may not copy, modify, sublicense, link with, or distribute
the Library except as expressly provided under this License. Any
attempt otherwise to copy, modify, sublicense, link with, or
distribute the Library is void, and will automatically terminate your
rights under this License. However, parties who have received copies,
or rights, from you under this License will not have their licenses
terminated so long as such parties remain in full compliance.
9. You are not required to accept this License, since you have not
signed it. However, nothing else grants you permission to modify or
distribute the Library or its derivative works. These actions are
prohibited by law if you do not accept this License. Therefore, by
modifying or distributing the Library (or any work based on the
Library), you indicate your acceptance of this License to do so, and
all its terms and conditions for copying, distributing or modifying
the Library or works based on it.
10. Each time you redistribute the Library (or any work based on the
Library), the recipient automatically receives a license from the
original licensor to copy, distribute, link with or modify the Library
subject to these terms and conditions. You may not impose any further
restrictions on the recipients' exercise of the rights granted herein.
You are not responsible for enforcing compliance by third parties with
this License.
11. If, as a consequence of a court judgment or allegation of patent
infringement or for any other reason (not limited to patent issues),
conditions are imposed on you (whether by court order, agreement or
otherwise) that contradict the conditions of this License, they do not
excuse you from the conditions of this License. If you cannot
distribute so as to satisfy simultaneously your obligations under this
License and any other pertinent obligations, then as a consequence you
may not distribute the Library at all. For example, if a patent
license would not permit royalty-free redistribution of the Library by
all those who receive copies directly or indirectly through you, then
the only way you could satisfy both it and this License would be to
refrain entirely from distribution of the Library.
If any portion of this section is held invalid or unenforceable under any
particular circumstance, the balance of the section is intended to apply,
and the section as a whole is intended to apply in other circumstances.
It is not the purpose of this section to induce you to infringe any
patents or other property right claims or to contest validity of any
such claims; this section has the sole purpose of protecting the
integrity of the free software distribution system which is
implemented by public license practices. Many people have made
generous contributions to the wide range of software distributed
through that system in reliance on consistent application of that
system; it is up to the author/donor to decide if he or she is willing
to distribute software through any other system and a licensee cannot
impose that choice.
This section is intended to make thoroughly clear what is believed to
be a consequence of the rest of this License.
12. If the distribution and/or use of the Library is restricted in
certain countries either by patents or by copyrighted interfaces, the
original copyright holder who places the Library under this License may add
an explicit geographical distribution limitation excluding those countries,
so that distribution is permitted only in or among countries not thus
excluded. In such case, this License incorporates the limitation as if
written in the body of this License.
13. The Free Software Foundation may publish revised and/or new
versions of the Lesser General Public License from time to time.
Such new versions will be similar in spirit to the present version,
but may differ in detail to address new problems or concerns.
Each version is given a distinguishing version number. If the Library
specifies a version number of this License which applies to it and
"any later version", you have the option of following the terms and
conditions either of that version or of any later version published by
the Free Software Foundation. If the Library does not specify a
license version number, you may choose any version ever published by
the Free Software Foundation.
14. If you wish to incorporate parts of the Library into other free
programs whose distribution conditions are incompatible with these,
write to the author to ask for permission. For software which is
copyrighted by the Free Software Foundation, write to the Free
Software Foundation; we sometimes make exceptions for this. Our
decision will be guided by the two goals of preserving the free status
of all derivatives of our free software and of promoting the sharing
and reuse of software generally.
NO WARRANTY
15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO
WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.
EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR
OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY
KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE
LIBRARY IS WITH YOU. SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME
THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN
WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO
gitextract_l80w6cpf/
├── .gitignore
├── CMakeLists.txt
├── COPYING
├── README.md
├── externals/
│ └── qpoases/
│ ├── EXAMPLES/
│ │ ├── example1.cpp
│ │ └── example1b.cpp
│ ├── INCLUDE/
│ │ ├── Bounds.hpp
│ │ ├── Constants.hpp
│ │ ├── Constraints.hpp
│ │ ├── CyclingManager.hpp
│ │ ├── EXTRAS/
│ │ │ └── SolutionAnalysis.hpp
│ │ ├── Indexlist.hpp
│ │ ├── MessageHandling.hpp
│ │ ├── QProblem.hpp
│ │ ├── QProblemB.hpp
│ │ ├── SubjectTo.hpp
│ │ ├── Types.hpp
│ │ └── Utils.hpp
│ ├── LICENSE.txt
│ ├── README.txt
│ ├── SRC/
│ │ ├── Bounds.cpp
│ │ ├── Bounds.ipp
│ │ ├── Constraints.cpp
│ │ ├── Constraints.ipp
│ │ ├── CyclingManager.cpp
│ │ ├── CyclingManager.ipp
│ │ ├── EXTRAS/
│ │ │ └── SolutionAnalysis.cpp
│ │ ├── Indexlist.cpp
│ │ ├── Indexlist.ipp
│ │ ├── MessageHandling.cpp
│ │ ├── MessageHandling.ipp
│ │ ├── QProblem.cpp
│ │ ├── QProblem.ipp
│ │ ├── QProblemB.cpp
│ │ ├── QProblemB.ipp
│ │ ├── SubjectTo.cpp
│ │ ├── SubjectTo.ipp
│ │ ├── Utils.cpp
│ │ └── Utils.ipp
│ └── VERSIONS.txt
├── include/
│ └── rpg_mpc/
│ ├── mpc_controller.h
│ ├── mpc_params.h
│ └── mpc_wrapper.h
├── launch/
│ └── mpc_controller.launch
├── model/
│ ├── CMakeLists.txt
│ ├── FindACADO.cmake
│ ├── README.md
│ ├── quadrotor_model_codegen
│ ├── quadrotor_model_thrustrates.cpp
│ └── quadrotor_mpc_codegen/
│ ├── acado_auxiliary_functions.c
│ ├── acado_auxiliary_functions.h
│ ├── acado_common.h
│ ├── acado_integrator.c
│ ├── acado_qpoases_interface.cpp
│ ├── acado_qpoases_interface.hpp
│ └── acado_solver.c
├── package.xml
├── parameters/
│ └── default.yaml
├── src/
│ ├── autopilot_mpc_instance.cpp
│ ├── mpc_controller.cpp
│ └── mpc_wrapper.cpp
└── test/
└── mpc_node.cpp
SYMBOL INDEX (226 symbols across 36 files)
FILE: externals/qpoases/EXAMPLES/example1.cpp
function main (line 38) | int main( )
FILE: externals/qpoases/EXAMPLES/example1b.cpp
function main (line 38) | int main( )
FILE: externals/qpoases/INCLUDE/Bounds.hpp
class Bounds (line 50) | class Bounds : public SubjectTo
FILE: externals/qpoases/INCLUDE/Constraints.hpp
class Constraints (line 50) | class Constraints : public SubjectTo
FILE: externals/qpoases/INCLUDE/CyclingManager.hpp
class CyclingManager (line 50) | class CyclingManager
FILE: externals/qpoases/INCLUDE/EXTRAS/SolutionAnalysis.hpp
class SolutionAnalysis (line 48) | class SolutionAnalysis
FILE: externals/qpoases/INCLUDE/Indexlist.hpp
class Indexlist (line 48) | class Indexlist
FILE: externals/qpoases/INCLUDE/MessageHandling.hpp
type returnValue (line 64) | enum returnValue
class MessageHandling (line 188) | class MessageHandling
FILE: externals/qpoases/INCLUDE/QProblem.hpp
class QProblem (line 53) | class QProblem : public QProblemB
FILE: externals/qpoases/INCLUDE/QProblemB.hpp
class SolutionAnalysis (line 45) | class SolutionAnalysis
class QProblemB (line 55) | class QProblemB
FILE: externals/qpoases/INCLUDE/SubjectTo.hpp
class SubjectTo (line 50) | class SubjectTo
FILE: externals/qpoases/INCLUDE/Types.hpp
type BooleanType (line 44) | enum BooleanType
type PrintLevel (line 53) | enum PrintLevel
type VisibilityStatus (line 63) | enum VisibilityStatus
type QProblemStatus (line 72) | enum QProblemStatus
type SubjectToType (line 87) | enum SubjectToType
type SubjectToStatus (line 97) | enum SubjectToStatus
type CyclingStatus (line 107) | enum CyclingStatus
type HessianType (line 116) | enum HessianType
FILE: externals/qpoases/SRC/Bounds.cpp
function Bounds (line 81) | Bounds& Bounds::operator=( const Bounds& rhs )
function returnValue (line 103) | returnValue Bounds::init( int n )
function returnValue (line 120) | returnValue Bounds::setupBound( int _number, SubjectToStatus _status
function returnValue (line 156) | returnValue Bounds::setupAllFree( )
function returnValue (line 197) | returnValue Bounds::moveFixedToFree( int _number )
function returnValue (line 217) | returnValue Bounds::moveFreeToFixed( int _number, SubjectToStatus _status
function returnValue (line 238) | returnValue Bounds::swapFree( int number1, int number2
FILE: externals/qpoases/SRC/Constraints.cpp
function Constraints (line 80) | Constraints& Constraints::operator=( const Constraints& rhs )
function returnValue (line 102) | returnValue Constraints::init( int n )
function returnValue (line 119) | returnValue Constraints::setupConstraint( int _number, SubjectToStatus _...
function returnValue (line 155) | returnValue Constraints::setupAllInactive( )
function returnValue (line 207) | returnValue Constraints::moveActiveToInactive( int _number )
function returnValue (line 227) | returnValue Constraints::moveInactiveToActive( int _number, SubjectToSta...
FILE: externals/qpoases/SRC/CyclingManager.cpp
function CyclingManager (line 79) | CyclingManager& CyclingManager::operator=( const CyclingManager& rhs )
function returnValue (line 102) | returnValue CyclingManager::init( int _nV, int _nC )
function returnValue (line 117) | returnValue CyclingManager::setCyclingStatus( int number,
function CyclingStatus (line 149) | CyclingStatus CyclingManager::getCyclingStatus( int number, BooleanType ...
function returnValue (line 171) | returnValue CyclingManager::clearCyclingData( )
FILE: externals/qpoases/SRC/EXTRAS/SolutionAnalysis.cpp
function SolutionAnalysis (line 62) | SolutionAnalysis& SolutionAnalysis::operator=( const SolutionAnalysis& r...
function returnValue (line 75) | returnValue SolutionAnalysis::getHessianInverse( QProblem* qp, real_t* h...
function returnValue (line 176) | returnValue SolutionAnalysis::getHessianInverse( QProblemB* qp, real_t* ...
function returnValue (line 270) | returnValue SolutionAnalysis::getVarianceCovariance( QProblem* qp, real_...
FILE: externals/qpoases/SRC/Indexlist.cpp
function Indexlist (line 94) | Indexlist& Indexlist::operator=( const Indexlist& rhs )
function returnValue (line 121) | returnValue Indexlist::init( )
function returnValue (line 145) | returnValue Indexlist::getNumberArray( int* const numberarray ) const
function returnValue (line 218) | returnValue Indexlist::addNumber( int addnumber )
function returnValue (line 290) | returnValue Indexlist::removeNumber( int removenumber )
function returnValue (line 323) | returnValue Indexlist::swapNumbers( int number1, int number2 )
FILE: externals/qpoases/SRC/MessageHandling.cpp
function MessageHandling (line 249) | MessageHandling& MessageHandling::operator=( const MessageHandling& rhs )
function returnValue (line 267) | returnValue MessageHandling::throwError(
function returnValue (line 291) | returnValue MessageHandling::throwWarning(
function returnValue (line 315) | returnValue MessageHandling::throwInfo(
function returnValue (line 339) | returnValue MessageHandling::reset( )
function returnValue (line 355) | returnValue MessageHandling::listAllMessages( )
function returnValue (line 386) | returnValue MessageHandling::throwMessage(
function returnValue (line 475) | returnValue MessageHandling::throwMessage(
function MessageHandling (line 515) | MessageHandling* getGlobalMessageHandler( )
FILE: externals/qpoases/SRC/QProblem.cpp
function printmatrix2 (line 39) | void printmatrix2(char *name, double *A, int m, int n) {
function QProblem (line 152) | QProblem& QProblem::operator=( const QProblem& rhs )
function returnValue (line 203) | returnValue QProblem::reset( )
function returnValue (line 237) | returnValue QProblem::init( const real_t* const _H, const real_t* const ...
function returnValue (line 251) | returnValue QProblem::init( const real_t* const _H, const real_t* const ...
function returnValue (line 269) | returnValue QProblem::hotstart( const real_t* const g_new, const real_t*...
function returnValue (line 491) | returnValue QProblem::getDualSolution( real_t* const yOpt ) const
function returnValue (line 521) | returnValue QProblem::setupSubjectToType( )
function returnValue (line 592) | returnValue QProblem::setupCholeskyDecompositionProjected( )
function returnValue (line 763) | returnValue QProblem::setupTQfactorisation( )
function returnValue (line 796) | returnValue QProblem::solveInitialQP( const real_t* const xOpt, const re...
function returnValue (line 944) | returnValue QProblem::obtainAuxiliaryWorkingSet( const real_t* const xOp...
function returnValue (line 1087) | returnValue QProblem::setupAuxiliaryWorkingSet( const Bounds* const auxi...
function returnValue (line 1203) | returnValue QProblem::setupAuxiliaryQPsolution( const real_t* const xOpt...
function returnValue (line 1256) | returnValue QProblem::setupAuxiliaryQPgradient( )
function returnValue (line 1285) | returnValue QProblem::setupAuxiliaryQPbounds( const Bounds* const auxili...
function returnValue (line 1426) | returnValue QProblem::addConstraint( int number, SubjectToStatus C_status,
function returnValue (line 1580) | returnValue QProblem::addConstraint_checkLI( int number )
function returnValue (line 1614) | returnValue QProblem::addConstraint_ensureLI( int number, SubjectToStatu...
function returnValue (line 1874) | returnValue QProblem::addBound( int number, SubjectToStatus B_status,
function returnValue (line 2040) | returnValue QProblem::addBound_checkLI( int number )
function returnValue (line 2062) | returnValue QProblem::addBound_ensureLI( int number, SubjectToStatus B_s...
function returnValue (line 2288) | returnValue QProblem::removeConstraint( int number,
function returnValue (line 2438) | returnValue QProblem::removeBound( int number,
function returnValue (line 2601) | returnValue QProblem::backsolveR( const real_t* const b, BooleanType tra...
function returnValue (line 2613) | returnValue QProblem::backsolveR( const real_t* const b, BooleanType tra...
function returnValue (line 2673) | returnValue QProblem::backsolveT( const real_t* const b, BooleanType tra...
function returnValue (line 2726) | returnValue QProblem::hotstart_determineDataShift( const int* const FX_...
function returnValue (line 2784) | returnValue QProblem::hotstart_determineStepDirection( const int* const ...
function returnValue (line 3139) | returnValue QProblem::hotstart_determineStepLength( const int* const FR_...
function returnValue (line 3405) | returnValue QProblem::hotstart_performStep( const int* const FR_idx, con...
function BooleanType (line 3583) | BooleanType QProblem::areBoundsConsistent( const real_t* const delta_lb,...
function returnValue (line 3607) | returnValue QProblem::setupQPdata( const real_t* const _H, const real_t*...
function returnValue (line 3673) | returnValue QProblem::printIteration( int iteration,
function returnValue (line 3729) | returnValue QProblem::printIteration( int iteration,
function returnValue (line 3743) | returnValue QProblem::checkKKTconditions( )
FILE: externals/qpoases/SRC/QProblemB.cpp
function printmatrix (line 39) | void printmatrix(char *name, double *A, int m, int n) {
function QProblemB (line 194) | QProblemB& QProblemB::operator=( const QProblemB& rhs )
function returnValue (line 252) | returnValue QProblemB::reset( )
function returnValue (line 285) | returnValue QProblemB::init( const real_t* const _H, const real_t* const...
function returnValue (line 298) | returnValue QProblemB::init( const real_t* const _H, const real_t* const...
function returnValue (line 315) | returnValue QProblemB::hotstart( const real_t* const g_new, const real_t...
function real_t (line 522) | real_t QProblemB::getObjVal( ) const
function real_t (line 546) | real_t QProblemB::getObjVal( const real_t* const _x ) const
function returnValue (line 568) | returnValue QProblemB::getPrimalSolution( real_t* const xOpt ) const
function returnValue (line 593) | returnValue QProblemB::getDualSolution( real_t* const yOpt ) const
function returnValue (line 618) | returnValue QProblemB::setPrintLevel( PrintLevel _printlevel )
function returnValue (line 661) | returnValue QProblemB::checkForIdentityHessian( )
function returnValue (line 694) | returnValue QProblemB::setupSubjectToType( )
function returnValue (line 753) | returnValue QProblemB::setupCholeskyDecomposition( )
function returnValue (line 830) | returnValue QProblemB::solveInitialQP( const real_t* const xOpt, const r...
function returnValue (line 943) | returnValue QProblemB::obtainAuxiliaryWorkingSet( const real_t* const xO...
function returnValue (line 1072) | returnValue QProblemB::setupAuxiliaryWorkingSet( const Bounds* const au...
function returnValue (line 1139) | returnValue QProblemB::setupAuxiliaryQPsolution( const real_t* const xOp...
function returnValue (line 1180) | returnValue QProblemB::setupAuxiliaryQPgradient( )
function returnValue (line 1204) | returnValue QProblemB::setupAuxiliaryQPbounds( BooleanType useRelaxation )
function returnValue (line 1269) | returnValue QProblemB::addBound( int number, SubjectToStatus B_status,
function returnValue (line 1330) | returnValue QProblemB::removeBound( int number,
function returnValue (line 1399) | returnValue QProblemB::backsolveR( const real_t* const b, BooleanType tr...
function returnValue (line 1411) | returnValue QProblemB::backsolveR( const real_t* const b, BooleanType tr...
function returnValue (line 1470) | returnValue QProblemB::hotstart_determineDataShift( const int* const FX_...
function BooleanType (line 1530) | BooleanType QProblemB::areBoundsConsistent( const real_t* const delta_lb...
function returnValue (line 1548) | returnValue QProblemB::setupQPdata( const real_t* const _H, const real_t...
function returnValue (line 1637) | returnValue QProblemB::hotstart_determineStepDirection( const int* const...
function returnValue (line 1753) | returnValue QProblemB::hotstart_determineStepLength( const int* const FR...
function returnValue (line 1904) | returnValue QProblemB::hotstart_performStep( const int* const FR_idx, co...
function returnValue (line 2021) | returnValue QProblemB::printIteration( int iteration,
function returnValue (line 2075) | returnValue QProblemB::checkKKTconditions( )
FILE: externals/qpoases/SRC/SubjectTo.cpp
function SubjectTo (line 88) | SubjectTo& SubjectTo::operator=( const SubjectTo& rhs )
function returnValue (line 114) | returnValue SubjectTo::init( int n )
function returnValue (line 141) | returnValue SubjectTo::addIndex( Indexlist* const indexlist,
function returnValue (line 161) | returnValue SubjectTo::removeIndex( Indexlist* const indexlist,
function returnValue (line 177) | returnValue SubjectTo::swapIndex( Indexlist* const indexlist,
FILE: externals/qpoases/SRC/Utils.cpp
function returnValue (line 57) | returnValue print( const real_t* const v, int n )
function returnValue (line 78) | returnValue print( const real_t* const v, int n,
function returnValue (line 101) | returnValue print( const real_t* const v, int n,
function returnValue (line 119) | returnValue print( const real_t* const M, int nrow, int ncol )
function returnValue (line 135) | returnValue print( const real_t* const M, int nrow, int ncol,
function returnValue (line 153) | returnValue print( const real_t* const M, int nrow, int ncol,
function returnValue (line 171) | returnValue print( const int* const index, int n )
function returnValue (line 192) | returnValue print( const int* const index, int n,
function returnValue (line 210) | returnValue myPrintf( const char* s )
function returnValue (line 229) | returnValue printCopyrightNotice( )
function returnValue (line 238) | returnValue readFromFile( real_t* data, int nrow, int ncol,
function returnValue (line 280) | returnValue readFromFile( real_t* data, int n,
function returnValue (line 292) | returnValue readFromFile( int* data, int n,
function returnValue (line 329) | returnValue writeIntoFile( const real_t* const data, int nrow, int ncol,
function returnValue (line 377) | returnValue writeIntoFile( const real_t* const data, int n,
function returnValue (line 388) | returnValue writeIntoFile( const int* const data, int n,
function real_t (line 433) | real_t getCPUtime( )
function real_t (line 455) | real_t getNorm( const real_t* const v, int n )
FILE: include/rpg_mpc/mpc_controller.h
function namespace (line 45) | namespace rpg_mpc {
FILE: include/rpg_mpc/mpc_params.h
function namespace (line 32) | namespace rpg_mpc
FILE: include/rpg_mpc/mpc_wrapper.h
function T (line 91) | T getTimestep() { return dt_; }
function acado_is_prepared_ (line 134) | bool acado_is_prepared_{false};
FILE: model/quadrotor_model_thrustrates.cpp
function main (line 33) | int main( ){
FILE: model/quadrotor_mpc_codegen/acado_auxiliary_functions.c
function real_t (line 24) | real_t* acado_getVariablesX( )
function real_t (line 29) | real_t* acado_getVariablesU( )
function real_t (line 35) | real_t* acado_getVariablesY( )
function real_t (line 42) | real_t* acado_getVariablesYN( )
function real_t (line 48) | real_t* acado_getVariablesX0( )
function acado_printDifferentialVariables (line 58) | void acado_printDifferentialVariables( )
function acado_printControlVariables (line 72) | void acado_printControlVariables( )
function acado_printHeader (line 86) | void acado_printHeader( )
function acado_tic (line 108) | void acado_tic( acado_timer* t )
function real_t (line 114) | real_t acado_toc( acado_timer* t )
function acado_tic (line 123) | void acado_tic( acado_timer* t )
function real_t (line 129) | real_t acado_toc( acado_timer* t )
function acado_tic (line 151) | void acado_tic( acado_timer* t )
function real_t (line 157) | real_t acado_toc( acado_timer* t )
function acado_tic (line 181) | void acado_tic( acado_timer* t )
function real_t (line 188) | real_t acado_toc( acado_timer* t )
FILE: model/quadrotor_mpc_codegen/acado_auxiliary_functions.h
type acado_timer (line 72) | typedef struct acado_timer_
type acado_timer (line 86) | typedef struct acado_timer_
type acado_timer (line 104) | typedef struct acado_timer_
type acado_timer (line 114) | typedef struct acado_timer_
FILE: model/quadrotor_mpc_codegen/acado_common.h
type ACADOvariables (line 104) | typedef struct ACADOvariables_
type ACADOworkspace (line 170) | typedef struct ACADOworkspace_
FILE: model/quadrotor_mpc_codegen/acado_integrator.c
function acado_rhs (line 59) | void acado_rhs(const real_t* in, real_t* out)
function acado_diffs (line 79) | void acado_diffs(const real_t* in, real_t* out)
function acado_solve_dim20_triangular (line 229) | void acado_solve_dim20_triangular( real_t* const A, real_t* const b )
function real_t (line 444) | real_t acado_solve_dim20_system( real_t* const A, real_t* const b, int* ...
function acado_solve_dim20_system_reuse (line 504) | void acado_solve_dim20_system_reuse( real_t* const A, real_t* const b, i...
function acado_integrate (line 769) | int acado_integrate( real_t* const rk_eta, int resetIntegrator )
FILE: model/quadrotor_mpc_codegen/acado_qpoases_interface.cpp
function acado_solve (line 39) | int acado_solve( void )
function acado_getNWSR (line 62) | int acado_getNWSR( void )
FILE: model/quadrotor_mpc_codegen/acado_solver.c
function acado_modelSimulation (line 35) | int acado_modelSimulation( )
function acado_evaluateLSQ (line 228) | void acado_evaluateLSQ(const real_t* in, real_t* out)
function acado_evaluateLSQEndTerm (line 452) | void acado_evaluateLSQEndTerm(const real_t* in, real_t* out)
function acado_setObjQ1Q2 (line 631) | void acado_setObjQ1Q2( real_t* const tmpFx, real_t* const tmpObjS, real_...
function acado_setObjR1R2 (line 895) | void acado_setObjR1R2( real_t* const tmpObjS, real_t* const tmpR1, real_...
function acado_setObjQN1QN2 (line 979) | void acado_setObjQN1QN2( real_t* const tmpFx, real_t* const tmpObjSEndTe...
function acado_evaluateObjective (line 1203) | void acado_evaluateObjective( )
function acado_multGxGu (line 1295) | void acado_multGxGu( real_t* const Gx1, real_t* const Gu1, real_t* const...
function acado_moveGuE (line 1339) | void acado_moveGuE( real_t* const Gu1, real_t* const Gu2 )
function acado_multBTW1 (line 1383) | void acado_multBTW1( real_t* const Gu1, real_t* const Gu2, int iRow, int...
function acado_multBTW1_R1 (line 1403) | void acado_multBTW1_R1( real_t* const R11, real_t* const Gu1, real_t* co...
function acado_multGxTGu (line 1423) | void acado_multGxTGu( real_t* const Gx1, real_t* const Gu1, real_t* cons...
function acado_multQEW2 (line 1467) | void acado_multQEW2( real_t* const Q11, real_t* const Gu1, real_t* const...
function acado_macATw1QDy (line 1511) | void acado_macATw1QDy( real_t* const Gx1, real_t* const w11, real_t* con...
function acado_macBTw1 (line 1525) | void acado_macBTw1( real_t* const Gu1, real_t* const w11, real_t* const ...
function acado_macQSbarW2 (line 1533) | void acado_macQSbarW2( real_t* const Q11, real_t* const w11, real_t* con...
function acado_macASbar (line 1547) | void acado_macASbar( real_t* const Gx1, real_t* const w11, real_t* const...
function acado_expansionStep (line 1561) | void acado_expansionStep( real_t* const Gx1, real_t* const Gu1, real_t* ...
function acado_copyHTH (line 1585) | void acado_copyHTH( int iRow, int iCol )
function acado_multRDy (line 1605) | void acado_multRDy( real_t* const R2, real_t* const Dy1, real_t* const R...
function acado_multQDy (line 1613) | void acado_multQDy( real_t* const Q2, real_t* const Dy1, real_t* const Q...
function acado_condensePrep (line 1627) | void acado_condensePrep( )
function acado_condenseFdb (line 1848) | void acado_condenseFdb( )
function acado_expand (line 2193) | void acado_expand( )
function acado_preparationStep (line 2314) | int acado_preparationStep( )
function acado_feedbackStep (line 2324) | int acado_feedbackStep( )
function acado_initializeSolver (line 2336) | int acado_initializeSolver( )
function acado_initializeNodesByForwardSimulation (line 2509) | void acado_initializeNodesByForwardSimulation( )
function acado_shiftStates (line 2554) | void acado_shiftStates( int strategy, real_t* const xEnd, real_t* const ...
function acado_shiftControls (line 2636) | void acado_shiftControls( real_t* const uEnd )
function real_t (line 2656) | real_t acado_getKKT( )
function real_t (line 2676) | real_t acado_getObjective( )
FILE: src/autopilot_mpc_instance.cpp
class autopilot::AutoPilot<rpg_mpc::MpcController<float>,
rpg_mpc::MpcParams<float>> (line 30) | class autopilot::AutoPilot<rpg_mpc::MpcController<float>,
class autopilot::AutoPilot<rpg_mpc::MpcController<double>,
rpg_mpc::MpcParams<double>> (line 32) | class autopilot::AutoPilot<rpg_mpc::MpcController<double>,
function main (line 35) | int main(int argc, char **argv)
FILE: src/mpc_controller.cpp
type rpg_mpc (line 29) | namespace rpg_mpc {
class MpcController<float> (line 310) | class MpcController<float>
class MpcController<double> (line 313) | class MpcController<double>
FILE: src/mpc_wrapper.cpp
type rpg_mpc (line 28) | namespace rpg_mpc {
class MpcWrapper<float> (line 350) | class MpcWrapper<float>
class MpcWrapper<double> (line 351) | class MpcWrapper<double>
FILE: test/mpc_node.cpp
function main (line 29) | int main(int argc, char **argv)
Condensed preview — 62 files, each showing path, character count, and a content snippet. Download the .json file or copy for the full structured content (1,171K chars).
[
{
"path": ".gitignore",
"chars": 40,
"preview": "# CLion stuff\n.idea/\ncmake-build-debug/\n"
},
{
"path": "CMakeLists.txt",
"chars": 3412,
"preview": "# rpg_quadrotor_mpc\n# A model predictive control implementation for quadrotors.\n# Copyright (C) 2017-2018 Ph"
},
{
"path": "COPYING",
"chars": 35148,
"preview": " GNU GENERAL PUBLIC LICENSE\n Version 3, 29 June 2007\n\n Copyright (C) 2007 Free "
},
{
"path": "README.md",
"chars": 5856,
"preview": "# Model Predictive Control for Quadrotors with extension to Perception-Aware MPC\nModel Predictive Control for Quadrotors"
},
{
"path": "externals/qpoases/EXAMPLES/example1.cpp",
"chars": 1949,
"preview": "/*\n *\tThis file is part of qpOASES.\n *\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\n *\tCopyright ("
},
{
"path": "externals/qpoases/EXAMPLES/example1b.cpp",
"chars": 1776,
"preview": "/*\n *\tThis file is part of qpOASES.\n *\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\n *\tCopyright ("
},
{
"path": "externals/qpoases/INCLUDE/Bounds.hpp",
"chars": 5743,
"preview": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyrig"
},
{
"path": "externals/qpoases/INCLUDE/Constants.hpp",
"chars": 3501,
"preview": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyrig"
},
{
"path": "externals/qpoases/INCLUDE/Constraints.hpp",
"chars": 5864,
"preview": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyrig"
},
{
"path": "externals/qpoases/INCLUDE/CyclingManager.hpp",
"chars": 3861,
"preview": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyrig"
},
{
"path": "externals/qpoases/INCLUDE/EXTRAS/SolutionAnalysis.hpp",
"chars": 3435,
"preview": "/*\n *\tThis file is part of qpOASES.\n *\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\n *\tCopyright ("
},
{
"path": "externals/qpoases/INCLUDE/Indexlist.hpp",
"chars": 4928,
"preview": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyrig"
},
{
"path": "externals/qpoases/INCLUDE/MessageHandling.hpp",
"chars": 20367,
"preview": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyrig"
},
{
"path": "externals/qpoases/INCLUDE/QProblem.hpp",
"chars": 33063,
"preview": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyrig"
},
{
"path": "externals/qpoases/INCLUDE/QProblemB.hpp",
"chars": 27333,
"preview": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyrig"
},
{
"path": "externals/qpoases/INCLUDE/SubjectTo.hpp",
"chars": 5572,
"preview": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyrig"
},
{
"path": "externals/qpoases/INCLUDE/Types.hpp",
"chars": 4467,
"preview": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyrig"
},
{
"path": "externals/qpoases/INCLUDE/Utils.hpp",
"chars": 6713,
"preview": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyrig"
},
{
"path": "externals/qpoases/LICENSE.txt",
"chars": 26940,
"preview": "\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 Fo"
},
{
"path": "externals/qpoases/README.txt",
"chars": 3177,
"preview": "##\r\n##\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n##\tCopyright (C) 2007-2008 by Hans Joachim Ferre"
},
{
"path": "externals/qpoases/SRC/Bounds.cpp",
"chars": 5901,
"preview": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyrig"
},
{
"path": "externals/qpoases/SRC/Bounds.ipp",
"chars": 2545,
"preview": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyrig"
},
{
"path": "externals/qpoases/SRC/Constraints.cpp",
"chars": 6102,
"preview": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyrig"
},
{
"path": "externals/qpoases/SRC/Constraints.ipp",
"chars": 2641,
"preview": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyrig"
},
{
"path": "externals/qpoases/SRC/CyclingManager.cpp",
"chars": 3949,
"preview": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyrig"
},
{
"path": "externals/qpoases/SRC/CyclingManager.ipp",
"chars": 1643,
"preview": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyrig"
},
{
"path": "externals/qpoases/SRC/EXTRAS/SolutionAnalysis.cpp",
"chars": 12434,
"preview": "/*\n *\tThis file is part of qpOASES.\n *\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\n *\tCopyright ("
},
{
"path": "externals/qpoases/SRC/Indexlist.cpp",
"chars": 6642,
"preview": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyrig"
},
{
"path": "externals/qpoases/SRC/Indexlist.ipp",
"chars": 2163,
"preview": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyrig"
},
{
"path": "externals/qpoases/SRC/MessageHandling.cpp",
"chars": 19449,
"preview": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyrig"
},
{
"path": "externals/qpoases/SRC/MessageHandling.ipp",
"chars": 3215,
"preview": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyrig"
},
{
"path": "externals/qpoases/SRC/QProblem.cpp",
"chars": 96202,
"preview": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyrig"
},
{
"path": "externals/qpoases/SRC/QProblem.ipp",
"chars": 5667,
"preview": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyrig"
},
{
"path": "externals/qpoases/SRC/QProblemB.cpp",
"chars": 51089,
"preview": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyrig"
},
{
"path": "externals/qpoases/SRC/QProblemB.ipp",
"chars": 7273,
"preview": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyrig"
},
{
"path": "externals/qpoases/SRC/SubjectTo.cpp",
"chars": 4244,
"preview": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyrig"
},
{
"path": "externals/qpoases/SRC/SubjectTo.ipp",
"chars": 2847,
"preview": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyrig"
},
{
"path": "externals/qpoases/SRC/Utils.cpp",
"chars": 10228,
"preview": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyrig"
},
{
"path": "externals/qpoases/SRC/Utils.ipp",
"chars": 1315,
"preview": "/*\r\n *\tThis file is part of qpOASES.\r\n *\r\n *\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n *\tCopyrig"
},
{
"path": "externals/qpoases/VERSIONS.txt",
"chars": 3061,
"preview": "##\r\n##\tqpOASES -- An Implementation of the Online Active Set Strategy.\r\n##\tCopyright (C) 2007-2008 by Hans Joachim Ferre"
},
{
"path": "include/rpg_mpc/mpc_controller.h",
"chars": 4292,
"preview": "/* rpg_quadrotor_mpc\n * A model predictive control implementation for quadrotors.\n * Copyright (C) 2017-2018 Ph"
},
{
"path": "include/rpg_mpc/mpc_params.h",
"chars": 5150,
"preview": "/* rpg_quadrotor_mpc\n * A model predictive control implementation for quadrotors.\n * Copyright (C) 2017-2018 Ph"
},
{
"path": "include/rpg_mpc/mpc_wrapper.h",
"chars": 5164,
"preview": "/* rpg_quadrotor_mpc\n * A model predictive control implementation for quadrotors.\n * Copyright (C) 2017-2018 Ph"
},
{
"path": "launch/mpc_controller.launch",
"chars": 1280,
"preview": "<!-- rpg_quadrotor_mpc\n A model predictive control implementation for quadrotors.\n Copyright (C) 2017-2018 Ph"
},
{
"path": "model/CMakeLists.txt",
"chars": 1844,
"preview": "# rpg_quadrotor_mpc\n# A model predictive control implementation for quadrotors.\n# Copyright (C) 2017-2018 Ph"
},
{
"path": "model/FindACADO.cmake",
"chars": 4538,
"preview": "################################################################################\n#\n# Description:\n# ACADO Toolkit packag"
},
{
"path": "model/README.md",
"chars": 339,
"preview": "# How to compile the MPC code generation and analysis\n\n1. Install ACADO as described in http://acado.github.io/install_l"
},
{
"path": "model/quadrotor_model_thrustrates.cpp",
"chars": 13095,
"preview": "/* rpg_quadrotor_mpc\n * A model predictive control implementation for quadrotors.\n * Copyright (C) 2017-2018 Ph"
},
{
"path": "model/quadrotor_mpc_codegen/acado_auxiliary_functions.c",
"chars": 4906,
"preview": "/*\n * This file was auto-generated using the ACADO Toolkit.\n * \n * While ACADO Toolkit is free software release"
},
{
"path": "model/quadrotor_mpc_codegen/acado_auxiliary_functions.h",
"chars": 3428,
"preview": "/*\n * This file was auto-generated using the ACADO Toolkit.\n * \n * While ACADO Toolkit is free software release"
},
{
"path": "model/quadrotor_mpc_codegen/acado_common.h",
"chars": 8838,
"preview": "/*\n * This file was auto-generated using the ACADO Toolkit.\n * \n * While ACADO Toolkit is free software release"
},
{
"path": "model/quadrotor_mpc_codegen/acado_integrator.c",
"chars": 38571,
"preview": "/*\n * This file was auto-generated using the ACADO Toolkit.\n * \n * While ACADO Toolkit is free software release"
},
{
"path": "model/quadrotor_mpc_codegen/acado_qpoases_interface.cpp",
"chars": 1887,
"preview": "/*\n * This file was auto-generated using the ACADO Toolkit.\n * \n * While ACADO Toolkit is free software release"
},
{
"path": "model/quadrotor_mpc_codegen/acado_qpoases_interface.hpp",
"chars": 1819,
"preview": "/*\n * This file was auto-generated using the ACADO Toolkit.\n * \n * While ACADO Toolkit is free software release"
},
{
"path": "model/quadrotor_mpc_codegen/acado_solver.c",
"chars": 528806,
"preview": "/*\n * This file was auto-generated using the ACADO Toolkit.\n * \n * While ACADO Toolkit is free software release"
},
{
"path": "package.xml",
"chars": 576,
"preview": "<?xml version=\"1.0\"?>\n<package format=\"2\">\n <name>rpg_mpc</name>\n <version>0.0.0</version>\n <description>Model Predic"
},
{
"path": "parameters/default.yaml",
"chars": 2146,
"preview": "# rpg_quadrotor_mpc\n# A model predictive control implementation for quadrotors.\n# Copyright (C) 2017-2018 Ph"
},
{
"path": "src/autopilot_mpc_instance.cpp",
"chars": 1582,
"preview": "/* rpg_quadrotor_mpc\n * A model predictive control implementation for quadrotors.\n * Copyright (C) 2017-2018 Ph"
},
{
"path": "src/mpc_controller.cpp",
"chars": 11927,
"preview": "/* rpg_quadrotor_mpc\n * A model predictive control implementation for quadrotors.\n * Copyright (C) 2017-2018 Ph"
},
{
"path": "src/mpc_wrapper.cpp",
"chars": 10962,
"preview": "/* rpg_quadrotor_mpc\n * A model predictive control implementation for quadrotors.\n * Copyright (C) 2017-2018 Ph"
},
{
"path": "test/mpc_node.cpp",
"chars": 1290,
"preview": "/* rpg_quadrotor_mpc\n * A model predictive control implementation for quadrotors.\n * Copyright (C) 2017-2018 Ph"
}
]
// ... and 1 more files (download for full content)
About this extraction
This page contains the full source code of the uzh-rpg/rpg_mpc GitHub repository, extracted and formatted as plain text for AI agents and large language models (LLMs). The extraction includes 62 files (1.1 MB), approximately 425.7k tokens, and a symbol index with 226 extracted functions, classes, methods, constants, and types. Use this with OpenClaw, Claude, ChatGPT, Cursor, Windsurf, or any other AI tool that accepts text input. You can copy the full output to your clipboard or download it as a .txt file.
Extracted by GitExtract — free GitHub repo to text converter for AI. Built by Nikandr Surkov.