Showing preview only (481K chars total). Download the full file or copy to clipboard to get everything.
Repository: TUM-AAS/neural-mpc
Branch: main
Commit: 5f15661c2d1a
Files: 51
Total size: 460.3 KB
Directory structure:
gitextract__1r96zfd/
├── .gitignore
├── .gitmodules
├── LICENSE
├── README.md
├── requirements.txt
└── ros_dd_mpc/
├── CMakeLists.txt
├── config/
│ ├── agisim_simulation_run.yaml
│ ├── arena_limits.yaml
│ ├── arena_run.yaml
│ ├── basic.world
│ ├── circle_and_lemniscate_options.yaml
│ ├── configuration_parameters.py
│ ├── flyingroom_limits.yaml
│ ├── ground_effect_limits.yaml
│ ├── kingfisher.yaml
│ └── simulation_run.yaml
├── launch/
│ └── dd_mpc_wrapper.launch
├── msg/
│ └── ReferenceTrajectory.msg
├── nodes/
│ ├── dd_mpc_node.py
│ └── reference_publisher_node.py
├── package.xml
└── src/
├── __init__.py
├── experiments/
│ ├── __init__.py
│ ├── comparative_experiment.py
│ ├── point_tracking_and_record.py
│ └── trajectory_test.py
├── model_fitting/
│ ├── __init__.py
│ ├── gp.py
│ ├── gp_common.py
│ ├── gp_fitting.py
│ ├── gp_visualization.py
│ ├── mlp_common.py
│ ├── mlp_fitting.py
│ ├── mlp_quad_res_fitting.py
│ ├── process_neurobem_dataset.py
│ ├── rdrv_fitting.py
│ └── system_identification.py
├── quad_mpc/
│ ├── __init__.py
│ ├── create_ros_dd_mpc.py
│ ├── quad_3d.py
│ ├── quad_3d_mpc.py
│ └── quad_3d_optimizer.py
└── utils/
├── __init__.py
├── animator.py
├── ground_map.py
├── keyframe_3d_gen.py
├── quad_3d_opt_utils.py
├── trajectories.py
├── trajectory_generator.py
├── utils.py
└── visualization.py
================================================
FILE CONTENTS
================================================
================================================
FILE: .gitignore
================================================
venv/*
data/
results/
.idea/
*.pyc
ros_gp_mpc/acados_models/*
c_generated_code/
*.egg-info/
dist/
build/
acados_models/
================================================
FILE: .gitmodules
================================================
[submodule "ml-casadi"]
path = ml-casadi
url = https://github.com/TUM-AAS/ml-casadi
================================================
FILE: LICENSE
================================================
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
================================================
# Real-Time Neural MPC
This repository contains the code for experiments associated to our paper
```
Real-time Neural-MPC: Deep Learning Model Predictive Control for Quadrotors and Agile Robotic Platforms
```
[Arxiv Link](https://arxiv.org/pdf/2203.07747)
If you are looking for the ML-CasADi framework code you can find it [here](https://github.com/TUM-AAS/ml-casadi).
## Installation
### Checkout Submodules
```
git submodule update --init --recursive
```
### Acados
- Follow the [installation instructions](https://docs.acados.org/installation/index.html).
- Install the [Python Interface](https://docs.acados.org/python_interface/index.html).
- Ensure that `LD_LIBRARY_PATH` is set correctly (`DYLD_LIBRARY_PATH`on MacOS).
- Ensure that `ACADOS_SOURCE_DIR` is set correctly.
### Further Requirements
```
pip install -r requirements.txt
```
Make sure the ML-CasADi framework is part of the python path.
```
export PYTHONPATH="${PYTHONPATH}:<path-to-git>/ml-casadi"
```
Python 3.9 is recommended.
# Experiments
The provided code is based on the work of [Torrente et al.](https://github.com/uzh-rpg/data_driven_mpc) All functionality of the original code base is retained.
Change the working directory to
```
cd ros_dd_mpc
```
## Simulation
### Data Collection
Run the following script to collect a few minutes of flight samples
```
python src/experiments/point_tracking_and_record.py --recording --dataset_name simplified_sim_dataset --simulation_time 300
```
### Fitting a MLP Model
Edit the following variables of configuration file in `config/configuration_parameters.py` (class `ModelFitConfig`) so that the training script is referenced to the desired dataset. For redundancy, in order to identify the correct data file, we require to specify both the name of the dataset as well as the parameters used while acquiring the data.
In other words, you must input the simulator options used while running the previous python script. If you did not modify these variables earlier, you don't need to change anything this time as the default setting will work:
```
# ## Dataset loading ## #
ds_name = "simplified_sim_dataset"
ds_metadata = {
"noisy": True,
"drag": True,
"payload": False,
"motor_noise": True
}
```
The following command will train an MLP model with 4 hidden layers 64 neurons each to model the residual error on the velocities in x, y, and z direction (7, 8, 9 in the state).
We assign a name to the model for future referencing, e.g.: `simple_sim_mlp`
```
python src/model_fitting/mlp_fitting.py --model_name simple_sim_mlp --hidden_size 64 --hidden_layers 4 --x 7 8 9 --y 7 8 9 --epochs 100
```
The model will be saved under the directory `ros_dd_mpc/results/model_fitting/<git_hash>/`
### Fitting GP and RDRv
For instructions on how to fit a GP or RDRv model for comparison see the [here](https://github.com/uzh-rpg/data_driven_mpc)
### Test the Fitted Model
```
python src/experiments/trajectory_test.py --model_version <git_hash> --model_name simple_sim_mlp --model_typ mlp_approx
```
where the `model_type` argument can be one of `mlp_approx`(Real-time Neural MPC), `mlp` (Naive Integration), `gp` (Gaussian Process Model).
For a baseline comparison result run the same script without model parameters:
```
python src/experiments/trajectory_test.py
```
Multiple models can be compared at once via
```
python src/experiments/comparative_experiment.py --model_version <git_hash_1 git_hash_2 ...> --model_name <name_1 name_2 ...> --model_type <type_1 type_2> --fast
```
Results are saved in the `results/` folder.
### Citing
If you use this code in an academic context, please cite the following publication:
```
@article{salzmann2023neural,
title={Real-time Neural-MPC: Deep Learning Model Predictive Control for Quadrotors and Agile Robotic Platforms},
author={Salzmann, Tim and Kaufmann, Elia and Arrizabalaga, Jon and Pavone, Marco and Scaramuzza, Davide and Ryll, Markus},
journal={IEEE Robotics and Automation Letters},
doi={10.1109/LRA.2023.3246839},
year={2023}
}
```
================================================
FILE: requirements.txt
================================================
numpy==2.2
scipy==1.15
tqdm
matplotlib==3.9
scikit-learn==1.6
casadi==3.6
pyquaternion
joblib
pandas
PyYAML
rospkg>=1.3
torch==2.6
-i https://rospypi.github.io/simple/
rospy
================================================
FILE: ros_dd_mpc/CMakeLists.txt
================================================
cmake_minimum_required(VERSION 2.8.3)
project(ros_dd_mpc)
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
rospy
std_msgs
message_generation
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
add_message_files(
FILES
ReferenceTrajectory.msg
)
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
generate_messages(
DEPENDENCIES
std_msgs # Or other packages containing msgs
)
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES ros_python3_issues
CATKIN_DEPENDS message_runtime
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
${catkin_INCLUDE_DIRS}
)
## Declare a C++ library
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/ros_python3_issues.cpp
# )
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/ros_python3_issues_node.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
# ${catkin_LIBRARIES}
# )
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables and/or libraries for installation
# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_ros_python3_issues.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)
# if (CATKIN_ENABLE_TESTING)
# find_package(rostest REQUIRED)
# add_rostest(test/test_issue_rosunit.launch)
# endif()
================================================
FILE: ros_dd_mpc/config/agisim_simulation_run.yaml
================================================
quad_name: 'kingfisher'
world_limits: None
use_ekf_synchronization: False
control_freq_factor: 5
environment: 'agisim'
================================================
FILE: ros_dd_mpc/config/arena_limits.yaml
================================================
x_min: -6.0
x_max: 12.0
y_min: -7.0
y_max: 7.0
z_min: 1.0
z_max: 4.0
================================================
FILE: ros_dd_mpc/config/arena_run.yaml
================================================
quad_name: 'kingfisher'
world_limits: arena_limits
use_ekf_synchronization: False
control_freq_factor: 5
environment: 'arena'
================================================
FILE: ros_dd_mpc/config/basic.world
================================================
<?xml version="1.0" ?>
<sdf version="1.4">
<world name="default">
<include>
<uri>model://ground_plane</uri>
</include>
<include>
<uri>model://sun</uri>
</include>
<!-- Only one ROS interface plugin is required per world, as any other plugin can connect a Gazebo
topic to a ROS topic (or vise versa). -->
<plugin name="ros_interface_plugin" filename="librotors_gazebo_ros_interface_plugin.so"/>
<spherical_coordinates>
<surface_model>EARTH_WGS84</surface_model>
<latitude_deg>47.3667</latitude_deg>
<longitude_deg>8.5500</longitude_deg>
<elevation>500.0</elevation>
<heading_deg>0</heading_deg>
</spherical_coordinates>
<physics type='ode'>
<ode>
<solver>
<type>quick</type>
<iters>1000</iters>
<sor>1.3</sor>
</solver>
<constraints>
<cfm>0</cfm>
<erp>0.2</erp>
<contact_max_correcting_vel>100</contact_max_correcting_vel>
<contact_surface_layer>0.001</contact_surface_layer>
</constraints>
</ode>
<max_step_size>0.01</max_step_size>
<real_time_factor>0.5</real_time_factor>
<real_time_update_rate>50</real_time_update_rate>
<gravity>0 0 -9.8</gravity>
</physics>
</world>
</sdf>
================================================
FILE: ros_dd_mpc/config/circle_and_lemniscate_options.yaml
================================================
# Parameters for the loop and lemniscate trajectories at successively increasing speed
loop_z: 2.5 # Z position of loop plane [m]
loop_r: 5.0 # Radius of loop [m]
loop_v_max: 10.0 # Maximum speed achieved (approx.) [m/s]
loop_lin_a: 0.25 # Linear acceleration and deceleration of trajectory [m/s^2]
loop_clockwise: False # Rotation direction of the quad
loop_yawing: False # Yaw quadrotor along the circle (only for loop)
================================================
FILE: ros_dd_mpc/config/configuration_parameters.py
================================================
""" Set of tunable parameters for the Simplified Simulator and model fitting.
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/>.
"""
import os
class DirectoryConfig:
"""
Class for storing directories within the package
"""
_dir_path = os.path.dirname(os.path.realpath(__file__))
SAVE_DIR = _dir_path + '/../results/model_fitting'
RESULTS_DIR = _dir_path + '/../results'
CONFIG_DIR = _dir_path + ''
DATA_DIR = _dir_path + '/../data'
class SimpleSimConfig:
"""
Class for storing the Simplified Simulator configurations.
"""
# Set to True to show a real-time Matplotlib animation of the experiments for the Simplified Simulator. Execution
# will be slower if the GUI is turned on. Note: setting to True may require some further library installation work.
custom_sim_gui = False
# Set to True to display a plot describing the trajectory tracking results after the execution.
result_plots = True
# Set to True to show the trajectory that will be executed before the execution time
pre_run_debug_plots = True
# Choice of disturbances modeled in our Simplified Simulator. For more details about the parameters used refer to
# the script: src/quad_mpc/quad_3d.py.
simulation_disturbances = {
"noisy": True, # Thrust and torque gaussian noises
"drag": True, # 2nd order polynomial aerodynamic drag effect
"payload": False, # Payload force in the Z axis
"motor_noise": True # Asymmetric voltage noise in the motors
}
class ModelFitConfig:
"""
Class for storing flags for the model fitting scripts.
"""
# Dataset loading ## #
ds_name = "simplified_sim_dataset"
ds_metadata = {
"noisy": True,
"drag": True,
"payload": False,
"motor_noise": True
}
# ds_name = "agisim_dataset"
# ds_metadata = {
# "agisim": "default",
# }
# ds_name = "arena_dataset"
# ds_metadata = {
# "arena": "default",
# }
# ds_name = "neurobem_dataset"
# ds_metadata = {
# "arena": "default",
# }
# ## Visualization ## #
# Training mode
visualize_training_result = True
visualize_data = False
# Visualization mode
grid_sampling_viz = True
x_viz = [7, 8, 9]
u_viz = []
y_viz = [7, 8, 9]
# ## Data post-processing ## #
histogram_bins = 40 # Cluster data using histogram binning
histogram_threshold = 0.001 # Remove bins where the total ratio of data is lower than this threshold
velocity_cap = 16 # Also remove datasets point if abs(velocity) > x_cap
# ############# Experimental ############# #
# ## Use fit model to generate synthetic data ## #
use_dense_model = False
dense_model_version = ""
dense_model_name = ""
dense_training_points = 200
# ## Clustering for multidimensional models ## #
clusters = 1
load_clusters = False
class GroundEffectMapConfig:
"""
Class for storing parameters for the ground effect map.
"""
resolution = 0.1
origin = (-4, 9)
horizon = ((-7, 7), (-7, 7))
box_min = (-4.25, 9.37)
box_max = (-2.76, 10.13)
box_height = 0.7
================================================
FILE: ros_dd_mpc/config/flyingroom_limits.yaml
================================================
x_min: -0.4
x_max: 4.3
y_min: -2.4
y_max: 2.3
z_min: 0.5
z_max: 2.0
================================================
FILE: ros_dd_mpc/config/ground_effect_limits.yaml
================================================
x_min: -4.25
x_max: -2.76
y_min: 9.37
y_max: 10.13
z_min: 0.8 # 0.95
z_max: 0.85
================================================
FILE: ros_dd_mpc/config/kingfisher.yaml
================================================
mass: 0.752 # [kg]
tbm_fr: [ 0.075, -0.10, 0.0] # [m]
tbm_bl: [-0.075, 0.10, 0.0] # [m]
tbm_br: [-0.075, -0.10, 0.0] # [m]
tbm_fl: [ 0.075, 0.10, 0.0] # [m]
inertia: [0.0025, 0.0021, 0.0043] # [kgm^2]
motor_omega_min: 150.0 # [rad/s]
motor_omega_max: 2800.0 # [rad/s]
motor_tau: 0.033 # [s]
omega_max: [10.0, 10.0, 4.0] # [rad/s]
comm_delay: 0.04 # [s]
thrust_map: [1.562522e-6, 0.0, 0.0]
kappa: 0.022 # [Nm/N]
thrust_min: 0.0 # [N]
thrust_max: 8.5 # [N] per motor
rotors_config: "cross"
aero_coeff_1: [0.0, 0.0, 0.0] # [0.26, 0.28, 0.42] # [N/(v/m)]
aero_coeff_3: [0.0, 0.0, 0.0] # [N/(v/m)]
aero_coeff_h: 0.0 # 0.01 [N/(v^2/m^2)]
================================================
FILE: ros_dd_mpc/config/simulation_run.yaml
================================================
quad_name: 'kingfisher'
world_limits: None
use_ekf_synchronization: False
control_freq_factor: 5
environment: 'gazebo'
================================================
FILE: ros_dd_mpc/launch/dd_mpc_wrapper.launch
================================================
<?xml version="1.0"?>
<launch>
<!-- true if running the nodes in the gazebo simulator environment. false if running on the real platform-->
<arg name="environment" default="flying_room"/>
<!-- Recording parameters -->
<arg name="recording" default="false"/>
<arg name="dataset_name" default="gazebo_dataset"/>
<arg name="overwrite" default="true"/>
<arg name="train_split" default="true"/>
<arg name="record_raw_optitrack" default="false"/>
<!-- Reference trajectory parameters -->
<arg name="n_seeds" default="1"/> <!-- How many random seeds to use to generate trajectories -->
<arg name="trajectory_speeds" default="[4.0]"/>
<arg name="flight_mode" default="random"/> <!-- one of: random, hover, loop -->
<arg name="plot" default="false"/>
<arg name="world_limits" default="None"/>
<!-- Stable version: Use time horizon of 1 second, 10 control nodes and control @ 50 Hz (control_freq_factor=5) -->
<arg name="t_horizon" default="1"/>
<arg name="n_nodes" default="10"/>
<!-- Model loading parameters -->
<arg name="model_version" default=""/>
<arg name="model_name" default=""/>
<arg name="model_type" default="gp"/>
<!-- Trajectory tracking experiment reset -->
<arg name="reset_experiment" default="true"/>
<group ns="gp_mpc">
<group if="$(eval arg('environment')=='agisim')">
<!-- Node that runs the MPC -->
<node pkg="ros_dd_mpc" type="dd_mpc_node.py" name="mpc_wrapper" output="screen">
<rosparam file="$(find ros_dd_mpc)/config/agisim_simulation_run.yaml"/>
<param name="recording" value="$(arg recording)"/>
<param name="dataset_name" value="$(arg dataset_name)"/>
<param name="overwrite" value="$(arg overwrite)"/>
<param name="training_split" value="$(arg train_split)"/>
<param name="record_raw_optitrack" value="$(arg record_raw_optitrack)"/>
<param name="model_type" value="$(arg model_type)"/>
<param name="model_version" value="$(arg model_version)"/>
<param name="model_name" value="$(arg model_name)"/>
<param name="plot" value="$(arg plot)"/>
<param name="t_horizon" value="$(arg t_horizon)"/>
<param name="n_nodes" value="$(arg n_nodes)"/>
<param name="reset_experiment" value="$(arg reset_experiment)"/>
</node>
<!-- Random trajectory generator -->
<node pkg="ros_dd_mpc" type="reference_publisher_node.py" name="ref_gen" output="screen">
<rosparam file="$(find ros_dd_mpc)/config/agisim_simulation_run.yaml"/>
<rosparam file="$(find ros_dd_mpc)/config/circle_and_lemniscate_options.yaml"/>
<param name="n_seeds" value="$(arg n_seeds)"/>
<param name="v_list" value="$(arg trajectory_speeds)"/>
<param name="mode" value="$(arg flight_mode)"/>
<param name="world_limits" value="$(arg world_limits)"/>
<param name="t_horizon" value="$(arg t_horizon)"/>
<param name="n_nodes" value="$(arg n_nodes)"/>
<param name="plot" value="$(arg plot)"/>
</node>
</group>
<group if="$(eval arg('environment')=='gazebo')">
<!-- Node that runs the MPC -->
<node pkg="ros_dd_mpc" type="dd_mpc_node.py" name="mpc_wrapper" output="screen">
<rosparam file="$(find ros_dd_mpc)/config/simulation_run.yaml"/>
<param name="recording" value="$(arg recording)"/>
<param name="dataset_name" value="$(arg dataset_name)"/>
<param name="overwrite" value="$(arg overwrite)"/>
<param name="training_split" value="$(arg train_split)"/>
<param name="record_raw_optitrack" value="$(arg record_raw_optitrack)"/>
<param name="model_type" value="$(arg model_type)"/>
<param name="model_version" value="$(arg model_version)"/>
<param name="model_name" value="$(arg model_name)"/>
<param name="plot" value="$(arg plot)"/>
<param name="t_horizon" value="$(arg t_horizon)"/>
<param name="n_nodes" value="$(arg n_nodes)"/>
<param name="reset_experiment" value="$(arg reset_experiment)"/>
</node>
<!-- Random trajectory generator -->
<node pkg="ros_dd_mpc" type="reference_publisher_node.py" name="ref_gen" output="screen">
<rosparam file="$(find ros_dd_mpc)/config/simulation_run.yaml"/>
<rosparam file="$(find ros_dd_mpc)/config/circle_and_lemniscate_options.yaml"/>
<param name="n_seeds" value="$(arg n_seeds)"/>
<param name="v_list" value="$(arg trajectory_speeds)"/>
<param name="mode" value="$(arg flight_mode)"/>
<param name="world_limits" value="$(arg world_limits)"/>
<param name="t_horizon" value="$(arg t_horizon)"/>
<param name="n_nodes" value="$(arg n_nodes)"/>
<param name="plot" value="$(arg plot)"/>
</node>
</group>
<group if="$(eval arg('environment')=='flying_room')">
<!-- Node that runs the MPC -->
<node pkg="ros_dd_mpc" type="dd_mpc_node.py" name="mpc_wrapper" output="screen">
<rosparam file="$(find ros_dd_mpc)/config/flying_room_run.yaml"/>
<param name="recording" value="$(arg recording)"/>
<param name="dataset_name" value="$(arg dataset_name)"/>
<param name="overwrite" value="$(arg overwrite)"/>
<param name="training_split" value="$(arg train_split)"/>
<param name="record_raw_optitrack" value="$(arg record_raw_optitrack)"/>
<param name="model_type" value="$(arg model_type)"/>
<param name="model_version" value="$(arg model_version)"/>
<param name="model_name" value="$(arg model_name)"/>
<param name="plot" value="$(arg plot)"/>
<param name="t_horizon" value="$(arg t_horizon)"/>
<param name="n_nodes" value="$(arg n_nodes)"/>
<param name="reset_experiment" value="$(arg reset_experiment)"/>
</node>
<!-- Random trajectory generator -->
<node pkg="ros_dd_mpc" type="reference_publisher_node.py" name="ref_gen" output="screen">
<rosparam file="$(find ros_dd_mpc)/config/flying_room_run.yaml"/>
<rosparam file="$(find ros_dd_mpc)/config/circle_and_lemniscate_options.yaml"/>
<param name="n_seeds" value="$(arg n_seeds)"/>
<param name="v_list" value="$(arg trajectory_speeds)"/>
<param name="mode" value="$(arg flight_mode)"/>
<param name="world_limits" value="$(arg world_limits)"/>
<param name="t_horizon" value="$(arg t_horizon)"/>
<param name="n_nodes" value="$(arg n_nodes)"/>
<param name="plot" value="$(arg plot)"/>
</node>
</group>
<group if="$(eval arg('environment')=='arena')">
<!-- Node that runs the MPC -->
<node pkg="ros_dd_mpc" type="dd_mpc_node.py" name="mpc_wrapper" output="screen">
<rosparam file="$(find ros_dd_mpc)/config/arena_run.yaml"/>
<param name="recording" value="$(arg recording)"/>
<param name="dataset_name" value="$(arg dataset_name)"/>
<param name="overwrite" value="$(arg overwrite)"/>
<param name="training_split" value="$(arg train_split)"/>
<param name="record_raw_optitrack" value="$(arg record_raw_optitrack)"/>
<param name="model_type" value="$(arg model_type)"/>
<param name="model_version" value="$(arg model_version)"/>
<param name="model_name" value="$(arg model_name)"/>
<param name="plot" value="$(arg plot)"/>
<param name="t_horizon" value="$(arg t_horizon)"/>
<param name="n_nodes" value="$(arg n_nodes)"/>
<param name="reset_experiment" value="$(arg reset_experiment)"/>
</node>
<!-- Random trajectory generator -->
<node pkg="ros_dd_mpc" type="reference_publisher_node.py" name="ref_gen" output="screen">
<rosparam file="$(find ros_dd_mpc)/config/arena_run.yaml"/>
<rosparam file="$(find ros_dd_mpc)/config/circle_and_lemniscate_options.yaml"/>
<param name="n_seeds" value="$(arg n_seeds)"/>
<param name="v_list" value="$(arg trajectory_speeds)"/>
<param name="mode" value="$(arg flight_mode)"/>
<param name="world_limits" value="$(arg world_limits)"/>
<param name="t_horizon" value="$(arg t_horizon)"/>
<param name="n_nodes" value="$(arg n_nodes)"/>
<param name="plot" value="$(arg plot)"/>
</node>
</group>
</group>
</launch>
================================================
FILE: ros_dd_mpc/msg/ReferenceTrajectory.msg
================================================
int32 seq_len
string traj_name
float64 v_input
float64[] trajectory
float64[] dt
float64[] inputs
================================================
FILE: ros_dd_mpc/nodes/dd_mpc_node.py
================================================
#!/usr/bin/env python3.6
""" ROS node for the data-augmented MPC, to use in the Gazebo simulator and real world experiments.
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/>.
"""
import json
import os
import threading
import time
import numpy as np
import pandas as pd
import rosbag
import rospy
import std_msgs.msg
from agiros_msgs.msg import Command
from gazebo_msgs.srv import GetPhysicsProperties
from geometry_msgs.msg import PoseStamped
from nav_msgs.msg import Odometry
from ros_dd_mpc.msg import ReferenceTrajectory
from std_msgs.msg import Bool, Empty
from tqdm import tqdm
from src.experiments.point_tracking_and_record import make_record_dict, get_record_file_and_dir, check_out_data
from src.model_fitting.rdrv_fitting import load_rdrv
from src.quad_mpc.create_ros_dd_mpc import ROSDDMPC
from src.utils.utils import jsonify, interpol_mse, quaternion_state_mse, load_pickled_models, \
separate_variables, get_model_dir_and_file
from src.utils.visualization import trajectory_tracking_results, mse_tracking_experiment_plot, \
load_past_experiments, get_experiment_files
def odometry_parse(odom_msg):
p = [odom_msg.pose.pose.position.x, odom_msg.pose.pose.position.y, odom_msg.pose.pose.position.z]
q = [odom_msg.pose.pose.orientation.w, odom_msg.pose.pose.orientation.x, odom_msg.pose.pose.orientation.y,
odom_msg.pose.pose.orientation.z]
v = [odom_msg.twist.twist.linear.x, odom_msg.twist.twist.linear.y, odom_msg.twist.twist.linear.z]
w = [odom_msg.twist.twist.angular.x, odom_msg.twist.twist.angular.y, odom_msg.twist.twist.angular.z]
return p, q, v, w
def state_parse(state_msg):
p = [state_msg.pose.position.x, state_msg.pose.position.y, state_msg.pose.position.z]
q = [state_msg.pose.orientation.w, state_msg.pose.orientation.x, state_msg.pose.orientation.y,
state_msg.pose.orientation.z]
v = [state_msg.velocity.linear.x, state_msg.velocity.linear.y, state_msg.velocity.linear.z]
w = [state_msg.velocity.angular.x, state_msg.velocity.angular.y, state_msg.velocity.angular.z]
omega = state_msg.motor_speeds
return p, q, v, w, omega
def make_raw_optitrack_dict():
rec_dict_raw = make_record_dict(state_dim=7)
# Remove unnecessary entries
keys = list(rec_dict_raw.keys())
for key in keys:
if key not in ["state_in", "timestamp"]:
rec_dict_raw.pop(key)
return rec_dict_raw
def make_state_record_dict(state_dim):
blank_state_recording_dict = {
"state_in": np.zeros((0, state_dim)),
"state_ref": np.zeros((0, state_dim)),
"input_in": np.zeros((0, 4)),
"timestamp": np.zeros((0, 1)),
}
return blank_state_recording_dict
def odometry_skipped_warning(last_seq, current_seq, stage):
skip_msg = "Odometry skipped at %s step. Last: %d, current: %d" % (stage, last_seq, current_seq)
rospy.logwarn(skip_msg)
class DDMPCWrapper:
def __init__(self, quad_name, environment="agisim", recording_options=None, load_options=None,
rdrv=None, plot=False, reset_experiment=False):
if recording_options is None:
recording_options = {"recording": False}
# If on a simulation environment, figure out if physics are slowed down
if environment == "gazebo":
try:
get_gazebo_physics = rospy.ServiceProxy('/gazebo/get_physics_properties', GetPhysicsProperties)
resp = get_gazebo_physics()
physics_speed = resp.max_update_rate * resp.time_step
rospy.loginfo("Physics running at %.2f normal speed" % physics_speed)
except rospy.ServiceException as e:
print("Service call failed: %s" % e)
physics_speed = 1
else:
physics_speed = 1
self.physics_speed = physics_speed
self.environment = environment
self.plot = plot
self.recording_options = recording_options
# Control at 50 hz. Use time horizon=1 and 10 nodes
self.n_mpc_nodes = rospy.get_param('~n_nodes', default=10)
self.t_horizon = rospy.get_param('~t_horizon', default=1.0)
self.control_freq_factor = rospy.get_param('~control_freq_factor', default=5 if environment == "gazebo" else 6)
self.opt_dt = self.t_horizon / (self.n_mpc_nodes * self.control_freq_factor)
# Load trained model
mlp_conf = None
if load_options is not None:
if load_options['model_type'] == 'gp':
rospy.loginfo("Attempting to load GP model from:\n git: {}\n name: {}\n meta: {}".format(
load_options["git"], load_options["model_name"], load_options["params"]))
pre_trained_models = load_pickled_models(model_options=load_options)
else:
import torch
import ml_casadi.torch as mc
from src.model_fitting.mlp_common import NormalizedMLP, QuadResidualModel
directory, file_name = get_model_dir_and_file(load_options)
saved_dict = torch.load(os.path.join(directory, f"{file_name}.pt"), map_location="cpu")
model_type = load_options['model_type']
if '_qres' in model_type:
mlp_model = QuadResidualModel(saved_dict['hidden_size'], saved_dict['hidden_layers'])
else:
mlp_model = mc.nn.MultiLayerPerceptron(saved_dict['input_size'], saved_dict['hidden_size'],
saved_dict['output_size'], saved_dict['hidden_layers'], 'Tanh')
model = NormalizedMLP(mlp_model, torch.tensor(np.zeros((saved_dict['input_size'],))).float(),
torch.tensor(np.zeros((saved_dict['input_size'],))).float(),
torch.tensor(np.zeros((saved_dict['output_size'],))).float(),
torch.tensor(np.zeros((saved_dict['output_size'],))).float())
model.load_state_dict(saved_dict['state_dict'])
model.eval()
if 'gpu' in model_type:
model = model.to('cuda:0')
pre_trained_models = model
mlp_conf = {'approximated': False, 'v_inp': True, 'u_inp': False, 'ground_map_input': False,
'torque_output': False}
print(model_type)
if model_type.endswith('approx') or model_type.endswith('approx1'):
mlp_conf['approximated'] = True
mlp_conf['approx_order'] = 1
if model_type.endswith('approx2'):
mlp_conf['approximated'] = True
mlp_conf['approx_order'] = 2
if '_u' in model_type:
mlp_conf['u_inp'] = True
if '_ge' in model_type:
mlp_conf['ground_map_input'] = True
if '_T' in model_type:
mlp_conf['torque_output'] = True
if '_qres' in model_type:
mlp_conf['u_inp'] = True
mlp_conf['torque_output'] = True
if pre_trained_models is None:
rospy.logwarn("Model parameters specified did not match with any pre-trained model")
else:
pre_trained_models = None
self.pre_trained_models = pre_trained_models
self.git_v = load_options["git"]
if self.pre_trained_models is not None:
rospy.loginfo("Successfully loaded model")
self.model_name = load_options["model_name"]
elif rdrv is not None:
self.model_name = "rdrv"
else:
self.model_name = "Nominal"
# Initialize MPC for point tracking
self.dd_mpc = ROSDDMPC(self.t_horizon, self.n_mpc_nodes, self.opt_dt, quad_name=quad_name,
model_conf=mlp_conf, point_reference=False, models=pre_trained_models,
rdrv=rdrv)
# Last state obtained from odometry
self.x = None
# Elapsed time between two recordings
self.last_update_time = time.time()
# Last references. Use hovering activation as input reference
self.last_x_ref = None
self.last_u_ref = None
# Reference trajectory variables
self.x_ref = None
self.t_ref = None
self.u_ref = None
self.current_idx = 0
self.skipped_idx = []
self.quad_trajectory = None
self.quad_controls = None
self.w_control = None
# Provisional reference for "waiting_for_reference" hovering mode
self.x_ref_prov = None
# To measure optimization elapsed time
self.optimization_dt = 0
self.optimization_steps = 0
# Thread for MPC optimization
self.mpc_thread = threading.Thread()
# Trajectory tracking experiment. Dims: seed x av_v x n_samples
if reset_experiment:
self.metadata_dict = {}
else:
self.metadata_dict, _, _, _ = load_past_experiments()
self.mse_exp = np.zeros((0, 0, 0, 1))
self.t_opt = np.zeros((0, 0, 0))
self.mse_exp_v_max = np.zeros((0, 0))
self.ref_traj_name = ""
self.ref_v = 0
self.run_traj_counter = 0
# Keep track of status of MPC object
self.odom_available = False
# Binary variable to run MPC only once every other odometry callback
self.optimize_next = True
# Binary variable to completely skip an odometry if in flying arena
self.skip_next = False
# Remember the sequence number of the last odometry message received.
self.last_odom_seq_number = 0
# Measure if trajectory starting point is reached
self.x_initial_reached = False
# Variables for recording mode
self.recording_warmup = True
self.x_pred = None
self.w_opt = None
# Get recording file and directory
blank_recording_dict = make_record_dict(state_dim=13)
if recording_options["recording"]:
record_raw_optitrack = recording_options["record_raw_optitrack"]
overwrite = recording_options["overwrite"]
metadata = {self.environment: "default"}
rec_dict, rec_file = get_record_file_and_dir(
blank_recording_dict, recording_options, simulation_setup=metadata, overwrite=overwrite)
state_rec_dict = make_state_record_dict(state_dim=13)
# If flying with the optitrack system, also record raw optitrack estimates
if self.environment == "flying_room" or self.environment == 'arena' and record_raw_optitrack:
rec_dict_raw = make_raw_optitrack_dict()
metadata = {self.environment: "optitrack_raw"}
rec_dict_raw, rec_file_raw = get_record_file_and_dir(
rec_dict_raw, recording_options, simulation_setup=metadata, overwrite=overwrite)
else:
rec_dict_raw = rec_file_raw = None
if recording_options["record_raw_state_control"]:
record_raw_state_control = True
raw_state_control_bag = rosbag.Bag(os.path.splitext(rec_file)[0] + '.bag', 'w')
else:
state_rec_dict = None
record_raw_optitrack = False
rec_dict = rec_file = None
rec_dict_raw = rec_file_raw = None
record_raw_state_control = False
raw_state_control_bag = None
self.state_rec_dict = state_rec_dict
self.rec_dict = rec_dict
self.rec_file = rec_file
self.rec_dict_raw = rec_dict_raw
self.rec_file_raw = rec_file_raw
self.record_raw_state_control = record_raw_state_control
self.raw_state_control_bag = raw_state_control_bag
self.landing = False
self.override_land = False
self.ground_level = False
self.controller_off = False
# Setup node publishers and subscribers. The odometry (sub) and control (pub) topics will vary depending on
# which environment is being used
odom_topic = "/" + quad_name + "/agiros_pilot/odometry"
raw_topic = None
if self.environment == "arena":
raw_topic = "/vicon/" + quad_name
land_topic = "/" + quad_name + "/agiros_pilot/land"
control_topic = "/" + quad_name + "/agiros_pilot/feedthrough_command"
off_topic = "/" + quad_name + "/agiros_pilot/off"
reference_topic = "reference"
status_topic = "busy"
# Publishers
self.control_pub = rospy.Publisher(control_topic, Command, queue_size=1, tcp_nodelay=True)
self.status_pub = rospy.Publisher(status_topic, Bool, queue_size=1)
self.off_pub = rospy.Publisher(off_topic, Empty, queue_size=1)
# Subscribers
self.land_sub = rospy.Subscriber(land_topic, Empty, self.land_callback)
self.ref_sub = rospy.Subscriber(reference_topic, ReferenceTrajectory, self.reference_callback)
self.odom_sub = rospy.Subscriber(odom_topic, Odometry, self.odometry_callback, queue_size=1, tcp_nodelay=True)
if raw_topic is not None and record_raw_optitrack:
self.raw_sub = rospy.Subscriber(raw_topic, PoseStamped, self.raw_odometry_callback)
rate = rospy.Rate(1)
while not rospy.is_shutdown():
# Publish if MPC is busy with a current trajectory
msg = Bool()
msg.data = not (self.x_ref is None and self.odom_available)
self.status_pub.publish(msg)
rate.sleep()
def land_callback(self, _):
"""
Trigger landing sequence.
:param _: empty message
"""
rospy.loginfo("Controller disabled. Landing with RPG MPC")
self.override_land = True
def rest_state(self):
"""
Set quad reference to hover state at position (0, 0, 0.1)
"""
self.last_x_ref = [[self.x[0], self.x[1], self.x[2]], [1, 0, 0, 0], [0, 0, 0], [0, 0, 0]]
self.last_u_ref = self.dd_mpc.quad.g[-1] * self.dd_mpc.quad.mass / (self.dd_mpc.quad.max_thrust * 4)
self.last_u_ref = self.last_u_ref[0] * np.array([1, 1, 1, 1])
def create_command_msg(self, w_opt, x_opt):
"""
Creates Command Msg from MPC output.
"""
next_control = Command()
next_control.header = std_msgs.msg.Header()
next_control.header.stamp = rospy.Time.now()
next_control.is_single_rotor_thrust = True if self.environment == 'agisim' else False
next_control.collective_thrust = np.sum(w_opt[:4]) * self.dd_mpc.quad.max_thrust / self.dd_mpc.quad.mass
next_control.bodyrates.x = x_opt[1, -3]
next_control.bodyrates.y = x_opt[1, -2]
next_control.bodyrates.z = x_opt[1, -1]
next_control.thrusts = w_opt[:4] * self.dd_mpc.quad.max_thrust
return next_control
def run_mpc(self, odom, recording=True):
"""
:param odom: message from subscriber.
:type odom: Odometry
:param recording: If False, some messages were skipped between this and last optimization. Don't record any data
during this optimization if in recording mode.
"""
if not self.odom_available:
return
# Measure time between initial state was checked in and now
dt = odom.header.stamp.to_time() - self.last_update_time
model_data, x_guess, u_guess = self.set_reference()
# Run MPC and publish control
try:
tic = time.time()
w_opt, x_opt = self.dd_mpc.optimize(model_data)
next_control = self.create_command_msg(w_opt, x_opt)
self.optimization_dt += time.time() - tic
self.optimization_steps += 1
if time.time() - tic > 0.01:
print("MPC thread. Seq: %d. Topt: %.4f" % (odom.header.seq, (time.time() - tic) * 1000))
# print("MPC thread. Seq: %d. Topt: %.4f" % (odom.header.seq, (time.time() - tic) * 1000))
self.control_pub.publish(next_control)
if self.x_initial_reached and self.current_idx < self.w_control.shape[0]:
self.w_control[self.current_idx, 0] = next_control.bodyrates.x
self.w_control[self.current_idx, 1] = next_control.bodyrates.y
self.w_control[self.current_idx, 2] = next_control.bodyrates.z
except KeyError:
self.recording_warmup = True
# Should not happen anymore.
rospy.logwarn("Tried to run an MPC optimization but MPC is not ready yet.")
return
if w_opt is not None:
# Check out final states. self.recording_warmup can only be true in recording mode.
if not self.recording_warmup and recording and self.x_initial_reached:
x_out = np.array(self.x)[np.newaxis, :]
self.rec_dict = check_out_data(self.rec_dict, x_out, None, self.w_opt, dt)
if self.record_raw_state_control:
self.raw_state_control_bag.write('control', next_control)
self.w_opt = w_opt
if self.x_initial_reached and self.current_idx < self.quad_controls.shape[0]:
self.quad_controls[self.current_idx, :] = np.expand_dims(self.w_opt[:4], axis=0)
def check_out_initial_state(self, odom):
"""
Add the initial state to the recording dictionary and start counting until next optimization
:param odom: message from subscriber.
:type odom: Odometry
"""
if self.w_opt is not None:
self.last_update_time = odom.header.stamp.to_time()
self.rec_dict["state_in"] = np.append(self.rec_dict["state_in"], np.array(self.x)[np.newaxis, :], 0)
self.rec_dict["timestamp"] = np.append(self.rec_dict["timestamp"], odom.header.stamp.to_time())
if self.current_idx < self.x_ref.shape[0]:
self.rec_dict["state_ref"] = np.append(self.rec_dict["state_ref"], self.x_ref[np.newaxis, self.current_idx, :], 0)
self.recording_warmup = False
def reference_callback(self, msg):
"""
Callback for receiving a reference trajectory
:param msg: message from subscriber
:type msg: ReferenceTrajectory
"""
seq_len = msg.seq_len
if seq_len == 0:
# Hover-in-place mode
self.x_ref = self.x[:7]
self.u_ref = None
self.t_ref = None
off_msg = Empty()
self.off_pub.publish(off_msg)
self.controller_off = True
# If this is the end of a reference tracking experiment, generate the results plot
if self.mse_exp.shape[0] != 0 and self.run_traj_counter > 0:
self.plot_tracking_mse_experiment()
self.landing = False
rospy.loginfo("No more references will be received")
if self.raw_state_control_bag is not None:
self.record_raw_state_control = False
self.raw_state_control_bag.close()
return
# Save reference name
self.ref_traj_name = msg.traj_name
self.ref_v = msg.v_input
# Save reference trajectory, relative times and inputs
self.x_ref = np.array(msg.trajectory).reshape(seq_len, -1)
self.t_ref = np.array(msg.dt)
self.u_ref = np.array(msg.inputs).reshape(seq_len, -1)
self.quad_trajectory = np.zeros((len(self.t_ref), len(self.x)))
self.quad_controls = np.zeros((len(self.t_ref), 4))
self.w_control = np.zeros((len(self.t_ref), 3))
rospy.loginfo("New trajectory received. Time duration: %.2f s" % self.t_ref[-1])
def odometry_callback(self, msg):
"""
Callback function for Odometry subscriber
:param msg: message from subscriber.
:type msg: Odometry
"""
if self.controller_off:
return
p, q, v, w = odometry_parse(msg)
self.x = p + q + v + w
try:
# Update the state estimate of the quad
self.dd_mpc.set_state(self.x)
except AttributeError:
# The DD MPC object instantiation is still not finished
return
if self.override_land:
return
# If the above try passed then MPC is ready
self.odom_available = True
if self.record_raw_state_control:
self.raw_state_control_bag.write('state', msg)
# We only optimize once every two odometry messages
if not self.optimize_next:
self.mpc_thread.join()
# If currently on trajectory tracking, pay close attention to any skipped messages.
if self.x_initial_reached:
# Count how many messages were skipped (ideally 0)
skipped_messages = int(msg.header.seq - self.last_odom_seq_number - 1)
if skipped_messages > 0:
warn_msg = "Recording time skipped messages: %d" % skipped_messages
rospy.logwarn(warn_msg)
# Adjust current index in trajectory
for i in range(divmod(skipped_messages, 2)[0]):
if self.current_idx + i < self.x_ref.shape[0]:
self.skipped_idx.append(self.current_idx + i)
self.current_idx += divmod(skipped_messages, 2)[0]
# If odd number of skipped messages, do optimization
if skipped_messages > 0 and skipped_messages % 2 == 1:
if self.recording_options["recording"]:
self.check_out_initial_state(msg)
# Run MPC now
self.run_mpc(msg)
self.last_odom_seq_number = msg.header.seq
self.optimize_next = False
return
self.optimize_next = True
if self.recording_options["recording"] and self.x_initial_reached:
self.check_out_initial_state(msg)
return
# Run MPC
if msg.header.seq > self.last_odom_seq_number + 2 and self.last_odom_seq_number > 0 and self.x_initial_reached:
# If one message was skipped at this point, then the reference is already late. Compensate by
# optimizing twice in a row and hope to do it fast...
if self.recording_options["recording"] and self.x_initial_reached:
self.check_out_initial_state(msg)
self.run_mpc(msg)
self.optimize_next = True
self.last_odom_seq_number = msg.header.seq
odometry_skipped_warning(self.last_odom_seq_number, msg.header.seq, "optimization")
return
def _thread_func():
self.run_mpc(msg)
self.mpc_thread = threading.Thread(target=_thread_func(), args=(), daemon=True)
self.mpc_thread.start()
self.last_odom_seq_number = msg.header.seq
self.optimize_next = False
def raw_odometry_callback(self, msg):
"""
Callback function for the raw Optitrack subscriber. Adds the data to the raw data dictionary.
:param msg: Raw data from Optitrack estimator
:type msg: PoseStamped
"""
if not self.recording_options["recording"] or not self.x_initial_reached:
return
x = np.array([msg.pose.position.x, msg.pose.position.y, msg.pose.position.z,
msg.pose.orientation.w, msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z])
self.rec_dict_raw["state_in"] = np.append(self.rec_dict_raw["state_in"], x[np.newaxis, :], 0)
self.rec_dict_raw["timestamp"] = np.append(self.rec_dict_raw["timestamp"], msg.header.stamp.to_time())
def hover_here(self, x):
self.rest_state()
x_ref = [x[:3], x[3:7], [0, 0, 0], [0, 0, 0]]
u_ref = self.last_u_ref
x_guess = np.tile(np.concatenate(x_ref)[np.newaxis, :], (self.n_mpc_nodes, 1))
u_guess = np.tile(self.last_u_ref[np.newaxis, :], (self.n_mpc_nodes, 1))
return self.dd_mpc.set_reference(x_ref, u_ref), x_guess, u_guess
def set_reference(self):
if self.environment == "gazebo" or self.environment== "agisim":
th = 0.1
else:
th = 0.2
mask = [1] * 9 + [0] * 3
x_ref = self.last_x_ref
u_ref = self.last_u_ref
x_guess = None
u_guess = None
if not self.odom_available:
return
# Check if landing mode
if self.landing:
dz = np.sign(0.3 - self.x[2])
dz = dz * 0.1 if self.environment != "agisim" else dz * 0.5
x_ref[0][2] = min(0.1, self.x[2] + dz) if dz > 0 else max(0.1, self.x[2] + dz)
# Check if z position is close to target.
if abs(self.x[2] - 0.2) < 0.05:
executed_x_ref = self.x_ref
executed_u_ref = self.u_ref
executed_t_ref = self.t_ref
self.x_ref = None
self.u_ref = None
self.t_ref = None
self.x_initial_reached = False
if self.recording_options["recording"]:
self.save_recording_data()
# Calculate MSE of position tracking and maximum axial velocity achieved
rmse = interpol_mse(np.delete(executed_t_ref, self.skipped_idx, axis=0),
np.delete(executed_x_ref[:, :3], self.skipped_idx, axis=0),
np.delete(executed_t_ref, self.skipped_idx, axis=0),
np.delete(self.quad_trajectory[:, :3], self.skipped_idx, axis=0))
self.optimization_dt /= self.optimization_steps
if self.ref_traj_name in self.metadata_dict.keys():
if self.model_name in self.metadata_dict[self.ref_traj_name].keys():
self.metadata_dict[self.ref_traj_name][self.model_name][self.ref_v] = [rmse,
self.optimization_dt]
else:
self.metadata_dict[self.ref_traj_name][self.model_name] = {
self.ref_v: [rmse, self.optimization_dt]}
else:
self.metadata_dict[self.ref_traj_name] = {
self.model_name: {self.ref_v: [rmse, self.optimization_dt]}}
n_trajectories = len(self.metadata_dict.keys())
n_models = len(self.metadata_dict[self.ref_traj_name].keys())
n_vel = len(self.metadata_dict[self.ref_traj_name][self.model_name].keys())
# Figure out dimensions of data so far
self.mse_exp = np.zeros((n_trajectories, n_vel, n_models, 1))
self.t_opt = np.zeros((n_trajectories, n_vel, n_models))
self.mse_exp_v_max = np.zeros((n_trajectories, n_vel))
# Add data to array
# Dimensions of mse_exp: n_trajectories x n_average_speeds x n_models x n_sim_options
for traj_id, traj_type in enumerate(self.metadata_dict.keys()):
for model_id, model_type in enumerate(self.metadata_dict[traj_type].keys()):
for vel_id, vel in enumerate(self.metadata_dict[traj_type][model_type].keys()):
self.mse_exp[traj_id, vel_id, model_id, 0] = self.metadata_dict[traj_type][model_type][vel][0]
self.t_opt[traj_id, vel_id, model_id] = self.optimization_dt
self.mse_exp_v_max[traj_id, vel_id] = vel
v_max = np.max(self.quad_trajectory[:, 7:10])
rospy.loginfo("Tracking complete. Total RMSE: %.5f m. Max axial vel: %.3f. "
"Mean optimization time: %.3f ms" % (rmse, v_max, self.optimization_dt * 1000))
self.current_idx = 0
if self.plot:
with_gp = ' + GP ' if self.pre_trained_models is not None else ' - GP '
tit = r'$v_{max}$=%.2f m/s | RMSE: %.4f | %s ' % (v_max, float(rmse), with_gp)
trajectory_tracking_results(np.delete(executed_t_ref, self.skipped_idx, axis=0),
np.delete(executed_x_ref, self.skipped_idx, axis=0),
np.delete(self.quad_trajectory, self.skipped_idx, axis=0),
np.delete(executed_u_ref, self.skipped_idx, axis=0),
np.delete(self.quad_controls, self.skipped_idx, axis=0),
w_control=np.delete(self.w_control, self.skipped_idx, axis=0), title=tit)
rospy.loginfo('Saved Plot!')
# Stop landing. Quad is close to ground level
self.landing = False
self.ground_level = True
return self.dd_mpc.set_reference(x_ref, u_ref), x_guess, u_guess
# Check if reference trajectory is set up. If not, pick current position and keep hover
if self.x_ref is None:
self.ground_level = False
# We are waiting for a new reference. Set in provisional hover mode at current position
if self.x_ref_prov is None:
rospy.loginfo("Entering provisional hovering mode while to reference is available at: ")
self.x_ref_prov = self.x
rospy.loginfo(self.x_ref_prov)
# Provisional hovering mode
return self.hover_here(self.x_ref_prov)
if self.x_ref_prov is not None:
self.x_ref_prov = None
rospy.loginfo("Abandoning provisional hovering mode.")
# Check if reference is hovering mode
if isinstance(self.x_ref, list):
return self.hover_here(self.x_ref)
# Trajectory tracking mode. Check if target reached
if quaternion_state_mse(np.array(self.x), self.x_ref[0, :], mask) < th and not self.x_initial_reached:
if self.record_raw_state_control:
self.raw_state_control_bag.write('recording_ctrl', std_msgs.msg.Bool(True))
# Initial position of trajectory has been reached
self.x_initial_reached = True
self.odom_available = False
self.optimization_dt = 0
rospy.loginfo("Reached initial position of trajectory.")
model_data = self.dd_mpc.set_reference(separate_variables(self.x_ref[:1, :]), self.u_ref[:1, :])
return model_data, x_guess, u_guess
# Raise the drone towards the initial position of the trajectory
if not self.x_initial_reached:
dx = 0.3 * np.sign(self.x_ref[0, 0] - self.x[0])
dy = 0.3 * np.sign(self.x_ref[0, 1] - self.x[1])
dz = 0.3 * np.sign(self.x_ref[0, 2] - self.x[2])
x_ref[0][0] = min(self.x_ref[0, 0], self.x[0] + dx) if dx > 0 else max(self.x_ref[0, 0], self.x[0] + dx)
x_ref[0][1] = min(self.x_ref[0, 1], self.x[1] + dy) if dy > 0 else max(self.x_ref[0, 1], self.x[1] + dy)
x_ref[0][2] = min(self.x_ref[0, 2], self.x[2] + dz) if dz > 0 else max(self.x_ref[0, 2], self.x[2] + dz)
elif self.current_idx < self.x_ref.shape[0]:
self.quad_trajectory[self.current_idx, :] = np.expand_dims(self.x, axis=0)
# Trajectory tracking
ref_traj = self.x_ref[self.current_idx:self.current_idx + self.n_mpc_nodes * self.control_freq_factor, :]
ref_u = self.u_ref[self.current_idx:self.current_idx + self.n_mpc_nodes * self.control_freq_factor, :]
# Indices for down-sampling the reference to number of MPC nodes
downsample_ref_ind = np.arange(0, min(self.control_freq_factor * self.n_mpc_nodes, ref_traj.shape[0]),
self.control_freq_factor, dtype=int)
# Sparser references (same dt as node separation)
x_ref = ref_traj[downsample_ref_ind, :]
u_ref = ref_u[downsample_ref_ind, :]
# Initial guesses
u_guess = u_ref
x_guess = x_ref
while u_guess.shape[0] < self.n_mpc_nodes:
x_guess = np.concatenate((x_guess, x_guess[-1:, :]), axis=0)
u_guess = np.concatenate((u_guess, u_guess[-1:, :]), axis=0)
x_ref = separate_variables(x_ref)
self.current_idx += 1
# End of reference reached
elif self.current_idx == self.x_ref.shape[0]:
rospy.loginfo("Finished trajectory - Landing.")
# Add one to the completed trajectory counter
self.run_traj_counter += 1
# Lower drone to a safe height
self.landing = True
self.rest_state()
x_ref = self.last_x_ref
u_ref = self.last_u_ref
# Stop recording
self.x_initial_reached = False
self.recording_warmup = True
if self.record_raw_state_control:
self.raw_state_control_bag.write('recording_ctrl', std_msgs.msg.Bool(False))
self.last_x_ref = x_ref
self.last_u_ref = u_ref
return self.dd_mpc.set_reference(x_ref, u_ref), x_guess, u_guess
def plot_tracking_mse_experiment(self):
metadata_file, _, _, _ = get_experiment_files()
# Save data for reload
with open(metadata_file, 'w') as json_file:
json.dump(self.metadata_dict, json_file, indent=4)
# Sort seeds dictionary by value
traj_type_labels = [k for k in self.metadata_dict.keys()]
model_type_labels = [k for k in self.metadata_dict[traj_type_labels[0]].keys()]
mse_tracking_experiment_plot(v_max=self.mse_exp_v_max, mse=self.mse_exp, traj_type_vec=traj_type_labels,
train_samples_vec=model_type_labels, legends=model_type_labels,
y_labels=["RotorS"], t_opt=self.t_opt)
def save_recording_data(self):
# Remove exceeding data entry if needed
if len(self.rec_dict['state_in']) > len(self.rec_dict['input_in']):
self.rec_dict['state_in'] = self.rec_dict['state_in'][:-1]
self.rec_dict['timestamp'] = self.rec_dict['timestamp'][:-1]
# Compute predictions offline to avoid extra overhead while in trajectory tracking control
rospy.loginfo("Filling in dataset and saving...")
for i in tqdm(range(len(self.rec_dict['input_in']))):
x_0 = self.rec_dict['state_in'][i]
x_f = self.rec_dict['state_out'][i]
u = self.rec_dict['input_in'][i]
dt = self.rec_dict['dt'][i]
x_pred, _ = self.dd_mpc.quad_mpc.forward_prop(x_0, u, t_horizon=dt)
x_pred = x_pred[-1, np.newaxis, :]
self.rec_dict['state_pred'] = np.append(self.rec_dict['state_pred'], x_pred, axis=0)
self.rec_dict['error'] = np.append(self.rec_dict['error'], x_f - x_pred, axis=0)
# Save datasets
x_dim = self.rec_dict["state_in"].shape[1]
for key in self.rec_dict.keys():
print(key, " ", self.rec_dict[key].shape)
self.rec_dict[key] = jsonify(self.rec_dict[key])
df = pd.DataFrame(self.rec_dict)
df.to_csv(self.rec_file, index=True, mode='a', header=False)
if self.rec_dict_raw is not None:
data_len = min(self.rec_dict_raw["state_in"].shape[0], len(self.rec_dict_raw["timestamp"]))
# To ensure same length of all entries
for key in self.rec_dict_raw.keys():
self.rec_dict_raw[key] = self.rec_dict_raw[key][:data_len]
print(key, " ", self.rec_dict_raw[key].shape)
self.rec_dict_raw[key] = jsonify(self.rec_dict_raw[key])
df = pd.DataFrame(self.rec_dict_raw)
df.to_csv(self.rec_file_raw, index=True, mode='a', header=False)
self.rec_dict_raw = make_raw_optitrack_dict()
# Reset recording dictionaries
self.rec_dict = make_record_dict(x_dim)
self.state_rec_dict = make_state_record_dict(x_dim)
def main():
rospy.init_node("dd_mpc")
# Recording parameters
recording_options = {
"recording": rospy.get_param('~recording', default=True),
"dataset_name": "deleteme",
"training_split": True,
"overwrite": True,
"record_raw_optitrack": True,
"record_raw_state_control": True,
}
dataset_name = rospy.get_param('~dataset_name', default=None)
overwrite = rospy.get_param('~overwrite', default=None)
training = rospy.get_param('~training_split', default=None)
raw_optitrack = rospy.get_param('~record_raw_optitrack', default=None)
raw_state_control = rospy.get_param('~record_raw_state_control', default=None)
if dataset_name is not None:
recording_options["dataset_name"] = dataset_name
if overwrite is not None:
recording_options["overwrite"] = overwrite
if training is not None:
recording_options["training_split"] = training
if raw_optitrack is not None:
recording_options["record_raw_optitrack"] = raw_optitrack
if raw_state_control is not None:
recording_options["record_raw_state_control"] = raw_state_control
# Model loading parameters
load_options = {
"git": "b6e73a5",
"model_name": "",
"params": None
}
git_id = rospy.get_param('~model_version', default=None)
model_name = rospy.get_param('~model_name', default=None)
model_type = rospy.get_param('~model_type', default="gp")
if git_id is not None:
load_options["git"] = git_id
if model_name is not None:
load_options["model_name"] = str(model_name)
if model_type is not None:
load_options["model_type"] = str(model_type)
plot = False
plot = rospy.get_param('~plot', default=None) if rospy.get_param('~plot', default=None) is not None else plot
env = rospy.get_param('~environment', default='gazebo')
default_quad = "hummingbird" if env == "gazebo" else "kingfisher"
load_options["params"] = {env: "default"}
if model_type == "rdrv":
rdrv = load_rdrv(model_options=load_options)
else:
rdrv = None
quad_name = rospy.get_param('~quad_name', default=None)
quad_name = quad_name if quad_name is not None else default_quad
# Change if needed. This is currently the supported combination.
if env == "gazebo":
assert quad_name == "hummingbird"
elif env == "agisim":
assert quad_name == "kingfisher"
# Reset experiments switch
reset = rospy.get_param('~reset_experiment', default=True)
DDMPCWrapper(quad_name, env, recording_options, load_options, rdrv=rdrv, plot=plot,
reset_experiment=reset)
if __name__ == "__main__":
main()
================================================
FILE: ros_dd_mpc/nodes/reference_publisher_node.py
================================================
#!/usr/bin/env python3.6
""" Node wrapper for publishing trajectories for the MPC pipeline to track.
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/>.
"""
from std_msgs.msg import Bool
from ros_dd_mpc.msg import ReferenceTrajectory
from src.quad_mpc.create_ros_dd_mpc import custom_quad_param_loader
from src.utils.trajectories import loop_trajectory, random_trajectory, lemniscate_trajectory, flyover_trajectory, \
flyover_trajectory_collect
import numpy as np
import rospy
class ReferenceGenerator:
def __init__(self):
self.gp_mpc_busy = True
rospy.init_node("reference_generator")
plot = rospy.get_param('~plot', default=True)
quad_name = rospy.get_param('~quad_name', default='hummingbird')
quad = custom_quad_param_loader(quad_name)
# Configuration for random flight mode
n_seeds = rospy.get_param('~n_seeds', default=1)
v_list = rospy.get_param('~v_list', default=[3.5])
if isinstance(v_list, str):
v_list = v_list.split('[')[1].split(']')[0]
v_list = [float(v.strip()) for v in v_list.split(',')]
# Select if generate "random" trajectories, "hover" mode or increasing speed "loop" mode
mode = rospy.get_param('~mode', default="random")
if mode != "random" and mode != "flyover":
n_seeds = 1
# Load parameters of loop trajectory
loop_r = rospy.get_param('~loop_r', default=1.5)
loop_z = rospy.get_param('~loop_z', default=1)
loop_v_max = rospy.get_param('~loop_v_max', default=3)
loop_a = rospy.get_param('~loop_lin_a', default=0.075)
loop_cc = rospy.get_param('~loop_clockwise', default=True)
loop_yawing = rospy.get_param('~loop_yawing', default=True)
# Load world limits if any
map_limits = rospy.get_param('~world_limits', default=None)
# Control at 50 hz. Use time horizon=1 and 10 nodes
n_mpc_nodes = rospy.get_param('~n_nodes', default=10)
t_horizon = rospy.get_param('~t_horizon', default=1.0)
control_freq_factor = rospy.get_param('~control_freq_factor', default=5 if quad_name == "hummingbird" else 6)
opt_dt = t_horizon / (n_mpc_nodes * control_freq_factor)
reference_topic = "reference"
status_topic = "busy"
reference_pub = rospy.Publisher(reference_topic, ReferenceTrajectory, queue_size=1)
rospy.Subscriber(status_topic, Bool, self.status_callback)
v_ind = 0
seed = 0
# Calculate total number of trajectories
n_trajectories = n_seeds * len(v_list)
curr_trajectory_ind = 0
rate = rospy.Rate(0.2)
while not rospy.is_shutdown():
if not self.gp_mpc_busy and mode == "hover":
rospy.loginfo("Sending hover-in-place command")
msg = ReferenceTrajectory()
reference_pub.publish(msg)
rospy.signal_shutdown("All trajectories were sent to the MPC")
break
if not self.gp_mpc_busy and curr_trajectory_ind == n_trajectories:
msg = ReferenceTrajectory()
reference_pub.publish(msg)
rospy.signal_shutdown("All trajectories were sent to the MPC")
break
if not self.gp_mpc_busy and mode == "loop":
rospy.loginfo("Sending increasing speed loop trajectory")
x_ref, t_ref, u_ref = loop_trajectory(quad, opt_dt, v_max=loop_v_max, radius=loop_r, z=loop_z,
lin_acc=loop_a, clockwise=loop_cc, map_name=map_limits,
yawing=loop_yawing, plot=plot)
msg = ReferenceTrajectory()
msg.traj_name = "circle"
msg.v_input = loop_v_max
msg.seq_len = x_ref.shape[0]
msg.trajectory = np.reshape(x_ref, (-1,)).tolist()
msg.dt = t_ref.tolist()
msg.inputs = np.reshape(u_ref, (-1,)).tolist()
reference_pub.publish(msg)
curr_trajectory_ind += 1
self.gp_mpc_busy = True
elif not self.gp_mpc_busy and mode == "lemniscate":
rospy.loginfo("Sending increasing speed lemniscate trajectory")
x_ref, t_ref, u_ref = lemniscate_trajectory(quad, opt_dt, v_max=loop_v_max, radius=loop_r, z=loop_z,
lin_acc=loop_a, clockwise=loop_cc, map_name=map_limits,
yawing=loop_yawing, plot=plot)
msg = ReferenceTrajectory()
msg.traj_name = "lemniscate"
msg.v_input = loop_v_max
msg.seq_len = x_ref.shape[0]
msg.trajectory = np.reshape(x_ref, (-1,)).tolist()
msg.dt = t_ref.tolist()
msg.inputs = np.reshape(u_ref, (-1,)).tolist()
reference_pub.publish(msg)
curr_trajectory_ind += 1
self.gp_mpc_busy = True
elif not self.gp_mpc_busy and mode == "random":
speed = v_list[v_ind]
log_msg = "Random trajectory generator %d/%d. Seed: %d. Mean vel: %.3f m/s" % \
(curr_trajectory_ind + 1, n_trajectories, seed, speed)
rospy.loginfo(log_msg)
x_ref, t_ref, u_ref = random_trajectory(quad, opt_dt, seed=seed, speed=speed, map_name=map_limits,
plot=plot)
msg = ReferenceTrajectory()
msg.traj_name = "random_" + str(seed)
msg.v_input = speed
msg.seq_len = x_ref.shape[0]
msg.trajectory = np.reshape(x_ref, (-1, )).tolist()
msg.dt = t_ref.tolist()
msg.inputs = np.reshape(u_ref, (-1, )).tolist()
reference_pub.publish(msg)
curr_trajectory_ind += 1
self.gp_mpc_busy = True
if v_ind + 1 < len(v_list):
v_ind += 1
else:
seed += 1
v_ind = 0
elif not self.gp_mpc_busy and mode == "flyover_collect":
speed = v_list[v_ind]
log_msg = "Flyover trajectory generator %d/%d. Seed: %d. Mean vel: %.3f m/s" % \
(curr_trajectory_ind + 1, n_trajectories, seed, speed)
rospy.loginfo(log_msg)
x_ref, t_ref, u_ref = flyover_trajectory_collect(quad, opt_dt, seed=seed, speed=speed,
flyover_box_name=map_limits)
msg = ReferenceTrajectory()
msg.traj_name = "flyover_" + str(seed)
msg.v_input = speed
msg.seq_len = x_ref.shape[0]
msg.trajectory = np.reshape(x_ref, (-1, )).tolist()
msg.dt = t_ref.tolist()
msg.inputs = np.reshape(u_ref, (-1, )).tolist()
reference_pub.publish(msg)
curr_trajectory_ind += 1
self.gp_mpc_busy = True
if v_ind + 1 < len(v_list):
v_ind += 1
else:
seed += 1
v_ind = 0
elif not self.gp_mpc_busy and mode == "flyover":
speed = v_list[v_ind]
log_msg = "Flyover trajectory generator %d/%d. Seed: %d. Mean vel: %.3f m/s" % \
(curr_trajectory_ind + 1, n_trajectories, seed, speed)
rospy.loginfo(log_msg)
x_ref, t_ref, u_ref = flyover_trajectory(quad, opt_dt, seed=seed, speed=speed,
flyover_box_name=map_limits)
msg = ReferenceTrajectory()
msg.traj_name = "flyover_" + str(seed)
msg.v_input = speed
msg.seq_len = x_ref.shape[0]
msg.trajectory = np.reshape(x_ref, (-1, )).tolist()
msg.dt = t_ref.tolist()
msg.inputs = np.reshape(u_ref, (-1, )).tolist()
reference_pub.publish(msg)
curr_trajectory_ind += 1
self.gp_mpc_busy = True
if v_ind + 1 < len(v_list):
v_ind += 1
else:
seed += 1
v_ind = 0
elif not self.gp_mpc_busy:
raise ValueError("Unknown trajectory type: %s" % mode)
rate.sleep()
def status_callback(self, msg):
"""
Callback function for tracking if the dd_mpc node is busy
:param msg: Message from the subscriber
:type msg: Bool
"""
self.gp_mpc_busy = msg.data
if __name__ == "__main__":
ReferenceGenerator()
================================================
FILE: ros_dd_mpc/package.xml
================================================
<?xml version="1.0"?>
<package format="2">
<name>ros_dd_mpc</name>
<version>0.0.0</version>
<description>The ros_dd_mpc package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="guillemtorrente@hotmail.com">Guillem Torrente</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/ros_python3_issues</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<build_depend>message_generation</build_depend>
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<exec_depend>message_runtime</exec_depend>
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>rospy</build_depend>
<build_export_depend>rospy</build_export_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>agiros_msgs</exec_depend>
<exec_depend>deep_casadi</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
================================================
FILE: ros_dd_mpc/src/__init__.py
================================================
================================================
FILE: ros_dd_mpc/src/experiments/__init__.py
================================================
================================================
FILE: ros_dd_mpc/src/experiments/comparative_experiment.py
================================================
""" Runs the experimental setup to compare different data-learned models for the MPC on the Simplified Simulator.
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/>.
"""
import os.path
import time
import argparse
import numpy as np
import torch
import ml_casadi.torch as mc
from config.configuration_parameters import SimpleSimConfig
from src.model_fitting.mlp_common import NormalizedMLP
from src.quad_mpc.quad_3d_mpc import Quad3DMPC
from src.quad_mpc.quad_3d import Quadrotor3D
from src.utils.quad_3d_opt_utils import get_reference_chunk
from src.utils.utils import load_pickled_models, interpol_mse, separate_variables, get_model_dir_and_file
from src.utils.visualization import initialize_drone_plotter, draw_drone_simulation, trajectory_tracking_results, \
get_experiment_files
from src.utils.visualization import mse_tracking_experiment_plot
from src.utils.trajectories import random_trajectory, lemniscate_trajectory, loop_trajectory
from src.model_fitting.rdrv_fitting import load_rdrv
global model_num
def prepare_quadrotor_mpc(simulation_options, version=None, name=None, reg_type="gp", quad_name=None,
t_horizon=1.0, q_diagonal=None, r_diagonal=None, q_mask=None):
"""
Creates a Quad3DMPC for the custom simulator.
@param simulation_options: Parameters for the Quadrotor3D object.
@param version: loading version for the GP/RDRv model.
@param name: name to load for the GP/RDRv model.
@param reg_type: either `gp` or `rdrv`.
@param quad_name: Name for the quadrotor. Default name will be used if not specified.
@param t_horizon: Time horizon of MPC in seconds.
@param q_diagonal: 12-dimensional diagonal of the Q matrix (p_xyz, a_xyz, v_xyz, w_xyz)
@param r_diagonal: 4-dimensional diagonal of the R matrix (motor inputs 1-4)
@param q_mask: State variable weighting mask (boolean). Which state variables compute towards state loss function?
@return: A Quad3DMPC wrapper for the custom simulator.
@rtype: Quad3DMPC
"""
# Default Q and R matrix for LQR cost
if q_diagonal is None:
q_diagonal = np.array([10, 10, 10, 0.1, 0.1, 0.1, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05])
if r_diagonal is None:
r_diagonal = np.array([0.1, 0.1, 0.1, 0.1])
if q_mask is None:
q_mask = np.array([1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]).T
# Simulation integration step (the smaller the more "continuous"-like simulation.
simulation_dt = 5e-4
# Number of MPC optimization nodes
n_mpc_nodes = 10
# Calculate time between two MPC optimization nodes [s]
node_dt = t_horizon / n_mpc_nodes
# Quadrotor simulator
my_quad = Quadrotor3D(**simulation_options)
model_name = quad_name
# Configuration for MLP Model
mlp_conf = None
if version is not None and name is not None:
load_ops = {"params": simulation_options}
load_ops.update({"git": version, "model_name": name})
# Load trained GP model
if reg_type == "gp":
pre_trained_models = load_pickled_models(model_options=load_ops)
rdrv_d = None
elif 'mlp' in reg_type:
mlp_conf = {'approximated': False, 'v_inp': True, 'u_inp': False, 'T_out': False, 'ground_map_input': False,
'torque_output': False, 'two_step_rti': False}
directory, file_name = get_model_dir_and_file(load_ops)
saved_dict = torch.load(os.path.join(directory, f"{file_name}.pt"))
mlp_model = mc.nn.MultiLayerPerceptron(saved_dict['input_size'], saved_dict['hidden_size'],
saved_dict['output_size'], saved_dict['hidden_layers'], 'Tanh')
model = NormalizedMLP(mlp_model, torch.tensor(np.zeros((saved_dict['input_size'],))).float(),
torch.tensor(np.zeros((saved_dict['input_size'],))).float(),
torch.tensor(np.zeros((saved_dict['output_size'],))).float(),
torch.tensor(np.zeros((saved_dict['output_size'],))).float())
model.load_state_dict(saved_dict['state_dict'])
model.eval()
pre_trained_models = model
rdrv_d = None
if reg_type.endswith('approx2'):
mlp_conf['approximated'] = True
mlp_conf['approx_order'] = 2
elif reg_type.endswith('approx') or reg_type.endswith('approx_1'):
mlp_conf['approximated'] = True
mlp_conf['approx_order'] = 1
if '_u' in reg_type:
mlp_conf['u_inp'] = True
if '_T' in reg_type:
mlp_conf['T_out'] = True
else:
rdrv_d = load_rdrv(model_options=load_ops)
pre_trained_models = None
else:
pre_trained_models = rdrv_d = None
if model_name is None:
model_name = "my_quad_" + str(globals()['model_num'])
globals()['model_num'] += 1
# Initialize quad MPC
quad_mpc = Quad3DMPC(my_quad, t_horizon=t_horizon, optimization_dt=node_dt, simulation_dt=simulation_dt,
q_cost=q_diagonal, r_cost=r_diagonal, n_nodes=n_mpc_nodes,
pre_trained_models=pre_trained_models, model_name=model_name, q_mask=q_mask, rdrv_d_mat=rdrv_d,
model_conf=mlp_conf)
return quad_mpc
def main(quad_mpc, av_speed, reference_type=None, plot=False):
"""
:param quad_mpc:
:type quad_mpc: Quad3DMPC
:param av_speed:
:param reference_type:
:param plot:
:return:
"""
# Recover some necessary variables from the MPC object
my_quad = quad_mpc.quad
n_mpc_nodes = quad_mpc.n_nodes
simulation_dt = quad_mpc.simulation_dt
t_horizon = quad_mpc.t_horizon
reference_over_sampling = 5
mpc_period = t_horizon / (n_mpc_nodes * reference_over_sampling)
# Choose the reference trajectory:
if reference_type == "loop":
# Circular trajectory
reference_traj, reference_timestamps, reference_u = loop_trajectory(
quad=my_quad, discretization_dt=mpc_period, radius=5, z=1, lin_acc=av_speed * 0.25, clockwise=True,
yawing=False, v_max=av_speed, map_name=None, plot=plot)
elif reference_type == "lemniscate":
# Lemniscate trajectory
reference_traj, reference_timestamps, reference_u = lemniscate_trajectory(
quad=my_quad, discretization_dt=mpc_period, radius=5, z=1, lin_acc=av_speed * 0.25, clockwise=True,
yawing=False, v_max=av_speed, map_name=None, plot=plot)
else:
# Get a random smooth position trajectory
reference_traj, reference_timestamps, reference_u = random_trajectory(
quad=my_quad, discretization_dt=mpc_period, seed=reference_type["random"], speed=av_speed, plot=plot)
# Set quad initial state equal to the initial reference trajectory state
quad_current_state = reference_traj[0, :].tolist()
my_quad.set_state(quad_current_state)
real_time_artists = None
if plot:
# Initialize real time plot stuff
world_radius = np.max(np.abs(reference_traj[:, :2])) * 1.2
real_time_artists = initialize_drone_plotter(n_props=n_mpc_nodes, quad_rad=my_quad.length,
world_rad=world_radius, full_traj=reference_traj)
start_time = time.time()
max_simulation_time = 10000
ref_u = reference_u[0, :]
quad_trajectory = np.zeros((len(reference_timestamps), len(quad_current_state)))
u_optimized_seq = np.zeros((len(reference_timestamps), 4))
# Sliding reference trajectory initial index
current_idx = 0
# Measure the MPC optimization time
mean_opt_time = 0.0
# Measure total simulation time
total_sim_time = 0.0
while (time.time() - start_time) < max_simulation_time and current_idx < reference_traj.shape[0]:
quad_current_state = my_quad.get_state(quaternion=True, stacked=True)
quad_trajectory[current_idx, :] = np.expand_dims(quad_current_state, axis=0)
u_optimized_seq[current_idx, :] = np.reshape(ref_u, (1, -1))
# ##### Optimization runtime (outer loop) ##### #
# Get the chunk of trajectory required for the current optimization
ref_traj_chunk, ref_u_chunk = get_reference_chunk(
reference_traj, reference_u, current_idx, n_mpc_nodes, reference_over_sampling)
model_ind = quad_mpc.set_reference(x_reference=separate_variables(ref_traj_chunk), u_reference=ref_u_chunk)
# Optimize control input to reach pre-set target
t_opt_init = time.time()
w_opt, x_pred = quad_mpc.optimize(use_model=model_ind, return_x=True)
mean_opt_time += time.time() - t_opt_init
# Select first input (one for each motor) - MPC applies only first optimized input to the plant
ref_u = np.squeeze(np.array(w_opt[:4]))
if len(quad_trajectory) > 0 and plot and current_idx > 0:
draw_drone_simulation(real_time_artists, quad_trajectory[:current_idx, :], my_quad, targets=None,
targets_reached=None, pred_traj=x_pred, x_pred_cov=None)
simulation_time = 1e-8
# ##### Simulation runtime (inner loop) ##### #
while simulation_time < mpc_period:
simulation_time += simulation_dt
total_sim_time += simulation_dt
quad_mpc.simulate(ref_u)
u_optimized_seq[current_idx, :] = np.reshape(ref_u, (1, -1))
current_idx += 1
quad_current_state = my_quad.get_state(quaternion=True, stacked=True)
quad_trajectory[-1, :] = np.expand_dims(quad_current_state, axis=0)
u_optimized_seq[-1, :] = np.reshape(ref_u, (1, -1))
# Average elapsed time per optimization
mean_opt_time /= current_idx
rmse = interpol_mse(reference_timestamps, reference_traj[:, :3], reference_timestamps, quad_trajectory[:, :3])
max_vel = np.max(np.sqrt(np.sum(reference_traj[:, 7:10] ** 2, 1)))
with_gp = ' + GP ' if quad_mpc.gp_ensemble is not None else ' - GP '
title = r'$v_{max}$=%.2f m/s | RMSE: %.4f m | %s ' % (max_vel, float(rmse), with_gp)
if plot:
trajectory_tracking_results(reference_timestamps, reference_traj, quad_trajectory,
reference_u, u_optimized_seq, title)
return rmse, max_vel, mean_opt_time
if __name__ == '__main__':
parser = argparse.ArgumentParser()
parser.add_argument("--model_version", type=str, default="", nargs="+",
help="Versions to load for the regression models. By default it is an 8 digit git hash."
"Must specify the version for each model separated by spaces.")
parser.add_argument("--model_name", type=str, default="", nargs="+",
help="Name of the regression models within the specified <model_version> folders. "
"Must specify the names for all models separated by spaces.")
parser.add_argument("--model_type", type=str, default="", nargs="+",
help="Type of regression models (GP or RDRv linear). "
"Must be specified for all models separated by spaces.")
parser.add_argument("--fast", dest="fast", action="store_true",
help="Set to True to run a fast experiment with less velocity samples.")
parser.add_argument("--print_results", dest="print_results", action="store_true",
help="Print the results data frame.")
parser.set_defaults(print_results=False)
input_arguments = parser.parse_args()
globals()['model_num'] = 0
# Trajectory options
traj_type_vec = [{"random": 1}, "loop", "lemniscate"]
traj_type_labels = ["Random", "Circle", "Lemniscate"]
if input_arguments.fast:
av_speed_vec = [[2.0, 3.5],
[2.0, 12.0],
[2.0, 12.0]]
else:
av_speed_vec = [[2.0, 2.7, 3.0, 3.2, 3.5],
[2.0, 4.5, 7.0, 9.5, 12.0],
[2.0, 4.5, 7.0, 9.5, 12.0]]
# Model options
git_list = input_arguments.model_version
name_list = input_arguments.model_name
type_list = input_arguments.model_type
assert len(git_list) == len(name_list) == len(type_list)
# Simulation options
plot_sim = SimpleSimConfig.custom_sim_gui
noisy_sim_options = SimpleSimConfig.simulation_disturbances
perfect_sim_options = {"payload": False, "drag": False, "noisy": False, "motor_noise": False}
model_vec = [
{"simulation_options": perfect_sim_options, "model": None},
{"simulation_options": noisy_sim_options, "model": None}]
legends = ['perfect', 'nominal']
for git, m_name, gp_or_rdrv in zip(git_list, name_list, type_list):
model_vec += [{"simulation_options": noisy_sim_options,
"model": {"version": git, "name": m_name, "reg_type": gp_or_rdrv}}]
legends += [gp_or_rdrv + ": " + m_name]
y_label = "RMSE [m]"
# Define result vectors
mse = np.zeros((len(traj_type_vec), len(av_speed_vec[0]), len(model_vec)))
v_max = np.zeros((len(traj_type_vec), len(av_speed_vec[0])))
t_opt = np.zeros((len(traj_type_vec), len(av_speed_vec[0]), len(model_vec)))
for n_train_id, model_type in enumerate(model_vec):
for traj_id, traj_type in enumerate(traj_type_vec):
for v_id, speed in enumerate(av_speed_vec[traj_id]):
if model_type["model"] is not None:
custom_mpc = prepare_quadrotor_mpc(model_type["simulation_options"], **model_type["model"])
else:
custom_mpc = prepare_quadrotor_mpc(model_type["simulation_options"])
traj_params = {"av_speed": speed, "reference_type": traj_type, "plot": plot_sim}
mse[traj_id, v_id, n_train_id], traj_v, opt_dt = main(custom_mpc, **traj_params)
t_opt[traj_id, v_id, n_train_id] += opt_dt
if v_max[traj_id, v_id] == 0:
v_max[traj_id, v_id] = traj_v
_, err_file, v_file, t_file = get_experiment_files()
np.save(err_file, mse)
np.save(v_file, v_max)
np.save(t_file, t_opt)
# from src.utils.visualization import load_past_experiments
# _, mse, v_max, t_opt = load_past_experiments()
mse_tracking_experiment_plot(v_max, mse, traj_type_labels, model_vec, legends, [y_label], t_opt=t_opt, font_size=12)
if input_arguments.print_results:
import pandas as pd
pd.options.display.float_format = "{:,.2f}".format
track_dfs = []
for i, track in enumerate(traj_type_labels):
index = pd.MultiIndex.from_arrays([[track]*len(av_speed_vec[i]), av_speed_vec[i], v_max[i]],
names=['Track', 'v_avg', 'v_max'])
track_df = pd.DataFrame(mse[i].T*1000, columns=index, index=legends)
track_dfs.append(track_df)
track_dfs = pd.concat(track_dfs, axis=1)
print(track_dfs.to_string())
pd.options.display.float_format = "{:,.3f}".format
t_df = pd.DataFrame(np.mean(np.mean(t_opt, axis=0), axis=0)*1000, columns=['avg'], index=legends)
print(t_df.to_string())
================================================
FILE: ros_dd_mpc/src/experiments/point_tracking_and_record.py
================================================
""" Executes aggressive maneuvers for collecting flight data on the Simplified Simulator to later train models on.
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/>.
"""
import os
import sys
import time
import copy
import argparse
import itertools
import pandas as pd
import numpy as np
import casadi as cs
from src.utils.visualization import draw_drone_simulation, initialize_drone_plotter
from src.experiments.comparative_experiment import prepare_quadrotor_mpc
from src.utils.utils import safe_mknode_recursive, jsonify, euclidean_dist, get_data_dir_and_file
from config.configuration_parameters import SimpleSimConfig
def get_record_file_and_dir(record_dict_template, recording_options, simulation_setup, overwrite=True):
dataset_name = recording_options["dataset_name"]
training_split = recording_options["training_split"]
# Directory and file name for data recording
rec_file_dir, rec_file_name = get_data_dir_and_file(dataset_name, training_split, simulation_setup)
overwritten = safe_mknode_recursive(rec_file_dir, rec_file_name, overwrite=overwrite)
rec_dict = copy.deepcopy(record_dict_template)
rec_file = os.path.join(rec_file_dir, rec_file_name)
if overwrite or (not overwrite and not overwritten):
for key in rec_dict.keys():
rec_dict[key] = jsonify(rec_dict[key])
df = pd.DataFrame(rec_dict)
df.to_csv(rec_file, index=False, header=True)
rec_dict = copy.deepcopy(record_dict_template)
return rec_dict, rec_file
def make_record_dict(state_dim):
blank_recording_dict = {
"state_in": np.zeros((0, state_dim)),
"state_ref": np.zeros((0, state_dim)),
"error": np.zeros((0, state_dim)),
"input_in": np.zeros((0, 4)),
"state_out": np.zeros((0, state_dim)),
"state_pred": np.zeros((0, state_dim)),
"timestamp": np.zeros((0, 1)),
"dt": np.zeros((0, 1)),
}
return blank_recording_dict
def check_out_data(rec_dict, state_final, x_pred, w_opt, dt):
rec_dict["dt"] = np.append(rec_dict["dt"], dt)
rec_dict["input_in"] = np.append(rec_dict["input_in"], w_opt[np.newaxis, :4], axis=0)
rec_dict["state_out"] = np.append(rec_dict["state_out"], state_final, 0)
if x_pred is not None:
err = state_final - x_pred
rec_dict["error"] = np.append(rec_dict["error"], err, axis=0)
rec_dict["state_pred"] = np.append(rec_dict["state_pred"], x_pred[np.newaxis, :], axis=0)
return rec_dict
def sample_random_target(x_current, world_radius, aggressive=True):
"""
Creates a new target point to reach.
:param x_current: current position of the quad. Only used if aggressive=True
:param world_radius: radius of the area where points are sampled from
:param aggressive: if aggressive=True, points will be sampled away from the current position. If False, then points
will be sampled uniformly from the world area.
:return: new sampled target point. A 3-dimensional numpy array.
"""
if aggressive:
# Polar 3D coordinates
theta = np.random.uniform(0, 2 * np.pi, 1)
psi = np.random.uniform(0, 2 * np.pi, 1)
r = 1 * world_radius + np.random.uniform(-0.5, 0.5, 1) * world_radius
# Transform to cartesian
x = r * np.sin(theta) * np.cos(psi)
y = r * np.sin(theta) * np.sin(psi)
z = r * np.cos(theta)
return x_current + np.array([x, y, z]).reshape((1, 3))
else:
return np.random.uniform(-world_radius, world_radius, (1, 3))
def main(model_options, recording_options, simulation_options, parameters):
world_radius = 3
if parameters["initial_state"] is None:
initial_state = [0.0, 0.0, 0.0] + [1, 0, 0, 0] + [0, 0, 0] + [0, 0, 0]
else:
initial_state = parameters["initial_state"]
sim_starting_pos = initial_state
quad_current_state = sim_starting_pos
if parameters["preset_targets"] is not None:
targets = parameters["preset_targets"]
else:
targets = sample_random_target(np.array(initial_state[:3]), world_radius,
aggressive=recording_options["recording"])
quad_mpc = prepare_quadrotor_mpc(simulation_options, **model_options, t_horizon=0.5,
q_mask=np.array([1, 1, 1, 0.01, 0.01, 0.01, 1, 1, 1, 0, 0, 0]))
# Recover some necessary variables from the MPC object
my_quad = quad_mpc.quad
n_mpc_nodes = quad_mpc.n_nodes
t_horizon = quad_mpc.t_horizon
simulation_dt = quad_mpc.simulation_dt
reference_over_sampling = 3
control_period = t_horizon / (n_mpc_nodes * reference_over_sampling)
my_quad.set_state(quad_current_state)
# Real time plot params
n_forward_props = n_mpc_nodes
plot_sim_traj = False
x_pred = None
w_opt = None
initial_guess = None
# The optimization should be faster or equal than the duration of the optimization time step
assert control_period <= t_horizon / n_mpc_nodes
state = quad_mpc.get_state()
# ####### Recording mode code ####### #
recording = recording_options["recording"]
state_dim = state.shape[0]
blank_recording_dict = make_record_dict(state_dim)
# Get recording file and directory
if recording:
if parameters["real_time_plot"]:
parameters["real_time_plot"] = False
print("Turned off real time plot during recording mode.")
rec_dict, rec_file = get_record_file_and_dir(blank_recording_dict, recording_options, simulation_options)
else:
rec_dict = rec_file = None
# Generate necessary art pack for real time plot
if parameters["real_time_plot"]:
real_time_art_pack = initialize_drone_plotter(n_props=n_forward_props, quad_rad=my_quad.x_f,
world_rad=world_radius)
else:
real_time_art_pack = None
start_time = time.time()
simulation_time = 0.0
# Simulation tracking stuff
targets_reached = np.array([False for _ in targets])
quad_trajectory = np.array(quad_current_state).reshape(1, -1)
n_iteration_count = 0
print("Targets reached: ", end='')
# All targets loop
while False in targets_reached and (time.time() - start_time) < parameters["max_sim_time"]:
current_target_i = np.where(targets_reached == False)[0][0]
current_target = targets[current_target_i]
current_target_reached = False
quad_target_state = [list(current_target), [1, 0, 0, 0], [0, 0, 0], [0, 0, 0]]
model_ind = quad_mpc.set_reference(quad_target_state)
# Provide an initial guess without the uncertainty prop.
if initial_guess is None:
initial_guess = quad_mpc.optimize(use_model=model_ind)
initial_guess = quad_mpc.reshape_input_sequence(initial_guess)
# MPC loop
while not current_target_reached and (time.time() - start_time) < parameters["max_sim_time"]:
# Emergency recovery (quad controller gone out of control lol)
if np.any(state[7:10] > 14) or n_iteration_count > 100:
n_iteration_count = 0
my_quad.set_state(list(itertools.chain.from_iterable(quad_target_state)))
state = quad_mpc.get_state()
if recording:
rec_dict["state_in"] = np.append(rec_dict["state_in"], state.T, 0)
rec_dict["timestamp"] = np.append(rec_dict["timestamp"], time.time() - start_time)
stacked_ref = np.array(list(itertools.chain.from_iterable(quad_target_state)))[np.newaxis, :]
rec_dict["state_ref"] = np.append(rec_dict["state_ref"], stacked_ref, 0)
if simulation_time != 0.0:
rec_dict = check_out_data(rec_dict, state.T, x_pred, w_opt, simulation_time)
simulation_time = 0.0
# Optimize control input to reach pre-set target
w_opt, x_pred_horizon = quad_mpc.optimize(use_model=model_ind, return_x=True)
if np.any(w_opt > (my_quad.max_input_value + 0.01)) or np.any(w_opt < (my_quad.min_input_value - 0.01)):
print("MPC constraints were violated")
initial_guess = quad_mpc.reshape_input_sequence(w_opt)
# Save initial guess for future optimization. It is a time-shift of the current optimized variables
initial_guess = np.array(cs.vertcat(initial_guess[1:, :], cs.DM.zeros(4).T))
# Select first input (one for each motor) - MPC applies only first optimized input to the plant
ref_u = np.squeeze(np.array(w_opt[:4]))
if recording:
# Integrate first input. Will be used as nominal model prediction during next save
x_pred, _ = quad_mpc.forward_prop(np.squeeze(state), w_opt=w_opt[:4],
t_horizon=control_period, use_gp=False)
x_pred = x_pred[-1, :]
if parameters["real_time_plot"]:
prop_params = {"x_0": np.squeeze(state), "w_opt": w_opt, "use_model": model_ind, "t_horizon": t_horizon}
x_int, _ = quad_mpc.forward_prop(**prop_params, use_gp=False)
if plot_sim_traj:
x_sim = quad_mpc.simulate_plant(quad_mpc.reshape_input_sequence(w_opt))
else:
x_sim = None
draw_drone_simulation(real_time_art_pack, quad_trajectory, my_quad, targets,
targets_reached, x_sim, x_int, x_pred_horizon, follow_quad=False)
while simulation_time < control_period:
# Simulation runtime (inner loop)
simulation_time += simulation_dt
quad_mpc.simulate(ref_u)
quad_current_state = quad_mpc.get_state()
# Target is reached
if euclidean_dist(current_target[0:3], quad_current_state[0:3, 0], thresh=0.05):
print("*", end='')
sys.stdout.flush()
n_iteration_count = 0
# Check out data immediately as new target will be optimized in next step
if recording and len(rec_dict['state_in']) > len(rec_dict['input_in']):
x_pred, _ = quad_mpc.forward_prop(np.squeeze(state), w_opt=w_opt[:4], t_horizon=simulation_time,
use_gp=False)
x_pred = x_pred[-1, :]
rec_dict = check_out_data(rec_dict, quad_mpc.get_state().T, x_pred, w_opt, simulation_time)
# Reset optimization time -> Ask for new optimization for next target in next dt
simulation_time = 0.0
# Mark current target as reached
current_target_reached = True
targets_reached[current_target_i] = True
# Remove initial guess
initial_guess = None
if parameters["preset_targets"] is None:
new_target = sample_random_target(quad_current_state[:3], world_radius, aggressive=recording)
targets = np.append(targets, new_target, axis=0)
targets_reached = np.append(targets_reached, False)
# Reset PID integral and past errors
quad_mpc.reset()
break
n_iteration_count += 1
if parameters["real_time_plot"]:
quad_trajectory = np.append(quad_trajectory, quad_current_state.T, axis=0)
if len(quad_trajectory) > 300:
quad_trajectory = np.delete(quad_trajectory, 0, 0)
# Current target was reached. Remove incomplete recordings
if recording:
if len(rec_dict['state_in']) > len(rec_dict['input_in']):
rec_dict["state_in"] = rec_dict["state_in"][:-1]
rec_dict["timestamp"] = rec_dict["timestamp"][:-1]
rec_dict["state_ref"] = rec_dict["state_ref"][:-1]
for key in rec_dict.keys():
rec_dict[key] = jsonify(rec_dict[key])
df = pd.DataFrame(rec_dict)
df.to_csv(rec_file, index=True, mode='a', header=False)
rec_dict = copy.deepcopy(blank_recording_dict)
if __name__ == '__main__':
parser = argparse.ArgumentParser()
parser.add_argument("--model_version", type=str, default="",
help="Version to load for the regression models. By default it is an 8 digit git hash.")
parser.add_argument("--model_name", type=str, default="",
help="Name of the regression model within the specified <model_version> folder.")
parser.add_argument("--model_type", type=str, default="gp", choices=["gp", "rdrv"],
help="Type of regression model (GP or RDRv linear)")
parser.add_argument("--recording", dest="recording", action="store_true",
help="Set to True to enable recording mode.")
parser.set_defaults(recording=False)
parser.add_argument("--dataset_name", type=str, default="simplified_sim_dataset",
help="Name for the generated dataset.")
parser.add_argument("--test_split", dest="test_split", action="store_true",
help="If the data set is the training or test split.")
parser.set_defaults(test_split=False)
parser.add_argument("--simulation_time", type=float, default=300,
help="Total duration of the simulation in seconds.")
args = parser.parse_args()
np.random.seed(123 if args.test_split else 456)
acados_config = {
"solver_type": "SQP",
"terminal_cost": True
}
run_options = {
"model_options": {
"version": args.model_version,
"name": args.model_name,
"reg_type": args.model_type,
"quad_name": "my_quad"
},
"recording_options": {
"recording": args.recording,
"dataset_name": args.dataset_name,
"training_split": not args.test_split,
},
"simulation_options": SimpleSimConfig.simulation_disturbances,
"parameters": {
"real_time_plot": SimpleSimConfig.custom_sim_gui,
"max_sim_time": args.simulation_time,
"preset_targets": None,
"initial_state": None,
"acados_options": acados_config
}
}
main(**run_options)
================================================
FILE: ros_dd_mpc/src/experiments/trajectory_test.py
================================================
""" Tracks a specified trajectory on the simplified simulator using the data-augmented MPC.
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/>.
"""
import time
import json
import argparse
import numpy as np
from tqdm import tqdm
from src.utils.utils import separate_variables
from src.utils.quad_3d_opt_utils import get_reference_chunk
from src.utils.trajectories import loop_trajectory, lemniscate_trajectory, check_trajectory
from src.utils.visualization import initialize_drone_plotter, draw_drone_simulation, trajectory_tracking_results
from src.experiments.comparative_experiment import prepare_quadrotor_mpc
from config.configuration_parameters import SimpleSimConfig
def main(args):
params = {
"version": args.model_version,
"name": args.model_name,
"reg_type": args.model_type,
"quad_name": "my_quad"
}
# Load the disturbances for the custom offline simulator.
simulation_options = SimpleSimConfig.simulation_disturbances
debug_plots = SimpleSimConfig.pre_run_debug_plots
tracking_results_plot = SimpleSimConfig.result_plots
sim_gui = SimpleSimConfig.custom_sim_gui
quad_mpc = prepare_quadrotor_mpc(simulation_options, **params)
# Recover some necessary variables from the MPC object
my_quad = quad_mpc.quad
n_mpc_nodes = quad_mpc.n_nodes
t_horizon = quad_mpc.t_horizon
simulation_dt = quad_mpc.simulation_dt
reference_over_sampling = 5
control_period = t_horizon / (n_mpc_nodes * reference_over_sampling)
if args.trajectory == "loop":
reference_traj, reference_timestamps, reference_u = loop_trajectory(
my_quad, control_period, radius=args.trajectory_radius, z=1, lin_acc=args.acceleration, clockwise=True,
yawing=False, v_max=args.max_speed, map_name=None, plot=debug_plots)
elif args.trajectory == "lemniscate":
reference_traj, reference_timestamps, reference_u = lemniscate_trajectory(
my_quad, control_period, radius=args.trajectory_radius, z=1, lin_acc=args.acceleration, clockwise=True,
yawing=False, v_max=args.max_speed, map_name=None, plot=debug_plots)
else:
raise ValueError("Unknown trajectory {}. Options are `lemniscate` and `loop`".format(args.trajectory))
if not check_trajectory(reference_traj, reference_u, reference_timestamps, debug_plots):
return
# Set quad initial state equal to the initial reference trajectory state
quad_current_state = reference_traj[0, :].tolist()
my_quad.set_state(quad_current_state)
real_time_artists = None
if sim_gui:
# Initialize real time plot stuff
world_radius = np.max(np.abs(reference_traj[:, :2])) * 1.2
real_time_artists = initialize_drone_plotter(n_props=n_mpc_nodes, quad_rad=my_quad.length,
world_rad=world_radius, full_traj=reference_traj)
ref_u = reference_u[0, :]
quad_trajectory = np.zeros((len(reference_timestamps), len(quad_current_state)))
u_optimized_seq = np.zeros((len(reference_timestamps), 4))
# Sliding reference trajectory initial index
current_idx = 0
# Measure the MPC optimization time
mean_opt_time = 0.0
# Measure total simulation time
total_sim_time = 0.0
print("\nRunning simulation...")
for current_idx in tqdm(range(reference_traj.shape[0])):
quad_current_state = my_quad.get_state(quaternion=True, stacked=True)
quad_trajectory[current_idx, :] = np.expand_dims(quad_current_state, axis=0)
# ##### Optimization runtime (outer loop) ##### #
# Get the chunk of trajectory required for the current optimization
ref_traj_chunk, ref_u_chunk = get_reference_chunk(
reference_traj, reference_u, current_idx, n_mpc_nodes, reference_over_sampling)
# Set the reference for the OCP
model_ind = quad_mpc.set_reference(x_reference=separate_variables(ref_traj_chunk), u_reference=ref_u_chunk)
# Optimize control input to reach pre-set target
t_opt_init = time.time()
w_opt, x_pred = quad_mpc.optimize(use_model=model_ind, return_x=True)
mean_opt_time += time.time() - t_opt_init
# Select first input (one for each motor) - MPC applies only first optimized input to the plant
ref_u = np.squeeze(np.array(w_opt[:4]))
u_optimized_seq[current_idx, :] = np.reshape(ref_u, (1, -1))
if len(quad_trajectory) > 0 and sim_gui and current_idx > 0:
draw_drone_simulation(real_time_artists, quad_trajectory[:current_idx, :], my_quad, targets=None,
targets_reached=None, pred_traj=x_pred, x_pred_cov=None)
simulation_time = 0.0
# ##### Simulation runtime (inner loop) ##### #
while simulation_time < control_period:
simulation_time += simulation_dt
total_sim_time += simulation_dt
quad_mpc.simulate(ref_u)
u_optimized_seq[current_idx, :] = np.reshape(ref_u, (1, -1))
quad_current_state = my_quad.get_state(quaternion=True, stacked=True)
quad_trajectory[-1, :] = np.expand_dims(quad_current_state, axis=0)
u_optimized_seq[-1, :] = np.reshape(ref_u, (1, -1))
# Average elapsed time per optimization
mean_opt_time = mean_opt_time / current_idx * 1000
tracking_rmse = np.mean(np.sqrt(np.sum((reference_traj[:, :3] - quad_trajectory[:, :3]) ** 2, axis=1)))
if tracking_results_plot:
v_max = np.max(reference_traj[:, 7:10])
with_gp = ' + GP ' if quad_mpc.gp_ensemble is not None else ' - GP '
title = r'$v_{max}$=%.2f m/s | RMSE: %.4f m | %s ' % (v_max, float(tracking_rmse), with_gp)
trajectory_tracking_results(reference_timestamps, reference_traj, quad_trajectory, reference_u, u_optimized_seq,
title)
v_max_abs = np.max(np.sqrt(np.sum(reference_traj[:, 7:10] ** 2, 1)))
print("\n:::::::::::::: SIMULATION SETUP ::::::::::::::\n")
print("Simulation: Applied disturbances: ")
print(json.dumps(simulation_options))
if quad_mpc.gp_ensemble is not None:
print("\nModel: Using GP regression model: ", params["version"] + '/' + params["name"])
elif quad_mpc.mlp is not None:
print("\nModel: Using MLP regression model: ", params["version"] + '/' + params["name"])
elif quad_mpc.rdrv is not None:
print("\nModel: Using RDRv regression model: ", params["version"] + '/' + params["name"])
else:
print("\nModel: No regression model loaded")
print("\nReference: Executed trajectory", '`' + args.trajectory + '`', "with a peak axial velocity of",
args.max_speed, "m/s, and a maximum speed of %2.3f m/s" % v_max_abs)
print("\n::::::::::::: SIMULATION RESULTS :::::::::::::\n")
print("Mean optimization time: %.3f ms" % mean_opt_time)
print("Tracking RMSE: %.4f m\n" % tracking_rmse)
if __name__ == '__main__':
parser = argparse.ArgumentParser()
parser.add_argument("--model_version", type=str, default="",
help="Version to load for the regression models. By default it is an 8 digit git hash.")
parser.add_argument("--model_name", type=str, default="",
help="Name of the regression model within the specified <model_version> folder.")
parser.add_argument("--model_type", type=str, default="gp",
help="Type of regression model (GP or RDRv linear)")
parser.add_argument("--trajectory", type=str, default="loop", choices=["loop", "lemniscate"],
help='path to other necessary data files (eg. vocabularies)')
parser.add_argument("--max_speed", type=float, default=8,
help="Maximum axial speed executed during the flight in m/s. For the `loop` trajectory, "
"velocities are feasible up to 14 m/s, and for the `lemniscate` up to 8 m/s")
parser.add_argument("--acceleration", type=float, default=1,
help="Acceleration of the reference trajectory. Higher accelerations will shorten the execution"
"time of the tracking")
parser.add_argument("--trajectory_radius", type=float, default=5, help="Radius of the reference trajectories")
input_arguments = parser.parse_args()
main(input_arguments)
================================================
FILE: ros_dd_mpc/src/model_fitting/__init__.py
================================================
================================================
FILE: ros_dd_mpc/src/model_fitting/gp.py
================================================
""" Gaussian Process custom implementation for the data-augmented MPC.
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/>.
"""
import numpy as np
import casadi as cs
import joblib
from tqdm import tqdm
from operator import itemgetter
from numpy.linalg import inv, cholesky, lstsq
from numpy.random import mtrand
from scipy.optimize import minimize
from scipy.spatial.distance import pdist, cdist, squareform
from src.utils.utils import safe_mknode_recursive, make_bz_matrix
class CustomKernelFunctions:
def __init__(self, kernel_func, params=None):
self.params = params
self.kernel_type = kernel_func
if self.kernel_type == 'squared_exponential':
if params is None:
self.params = {'l': [1.0], 'sigma_f': 1.0}
self.kernel = self.squared_exponential_kernel
else:
raise NotImplementedError("only squared_exponential is supported")
self.theta = self.get_trainable_parameters()
def __call__(self, x_1, x_2):
return self.kernel(x_1, x_2)
def __str__(self):
if self.kernel_type == 'squared_exponential':
len_scales = np.reshape(self.params['l'], -1)
len_scale_str = '['
for i in range(len(len_scales)):
len_scale_str += '%.3f, ' % len_scales[i] if i < len(len_scales) - 1 else '%.3f' % len_scales[i]
len_scale_str += ']'
summary = '%.3f' % self.params['sigma_f']
summary += '**2*RBF(length_scale=' + len_scale_str + ')'
return summary
else:
raise NotImplementedError("only squared_exponential is supported")
def get_trainable_parameters(self):
trainable_params = []
if self.kernel_type == 'squared_exponential':
trainable_params += \
np.reshape(np.squeeze(self.params['l']), -1).tolist() if hasattr(self.params['l'], "__len__") \
else [self.params['l']]
trainable_params += [self.params['sigma_f']]
return trainable_params
@staticmethod
def _check_length_scale(x, length_scale):
length_scale = np.squeeze(length_scale).astype(float)
if np.ndim(length_scale) > 1:
raise ValueError("length_scale cannot be of dimension greater than 1")
if np.ndim(length_scale) == 1 and x.shape[1] != length_scale.shape[0]:
raise ValueError("Anisotropic kernel must have the same number of dimensions as data (%d!=%d)"
% (length_scale.shape[0], x.shape[1]))
return length_scale
def squared_exponential_kernel(self, x_1, x_2=None):
"""
Anisotropic (diagonal length-scale) matrix squared exponential kernel. Computes a covariance matrix from points
in x_1 and x_2.
Args:
x_1: Array of m points (m x d).
x_2: Array of n points (n x d).
Returns:
Covariance matrix (m x n).
"""
if isinstance(x_2, cs.MX):
return self._squared_exponential_kernel_cs(x_1, x_2)
# Length scale parameter
len_scale = self.params['l'] if 'l' in self.params.keys() else 1.0
# Vertical variation parameter
sigma_f = self.params['sigma_f'] if 'sigma_f' in self.params.keys() else 1.0
x_1 = np.atleast_2d(x_1)
length_scale = self._check_length_scale(x_1, len_scale)
if x_2 is None:
dists = pdist(x_1 / length_scale, metric='sqeuclidean')
k = sigma_f * np.exp(-.5 * dists)
# convert from upper-triangular matrix to square matrix
k = squareform(k)
np.fill_diagonal(k, 1)
else:
dists = cdist(x_1 / length_scale, x_2 / length_scale, metric='sqeuclidean')
k = sigma_f * np.exp(-.5 * dists)
return k
def _squared_exponential_kernel_cs(self, x_1, x_2):
"""
Symbolic implementation of the anisotropic squared exponential kernel
:param x_1: Array of m points (m x d).
:param x_2: Array of n points (m x d).
:return: Covariance matrix (m x n).
"""
# Length scale parameter
len_scale = self.params['l'] if 'l' in self.params.keys() else 1.0
# Vertical variation parameter
sigma_f = self.params['sigma_f'] if 'sigma_f' in self.params.keys() else 1.0
if x_1.shape != x_2.shape and x_2.shape[0] == 1:
tiling_ones = cs.MX.ones(x_1.shape[0], 1)
d = x_1 - cs.mtimes(tiling_ones, x_2)
dist = cs.sum2(d ** 2 / cs.mtimes(tiling_ones, cs.MX(len_scale ** 2).T))
else:
d = x_1 - x_2
dist = cs.sum1(d ** 2 / cs.MX(len_scale ** 2))
return sigma_f * cs.SX.exp(-.5 * dist)
def diff(self, z, z_train):
"""
Computes the symbolic differentiation of the kernel function, evaluated at point z and using the training
dataset z_train. This function implements equation (80) from overleaf document, without the c^{v_x} vector,
and for all the partial derivatives possible (m), instead of just one.
:param z: evaluation point. Symbolic vector of length m
:param z_train: training dataset. Symbolic matrix of shape n x m
:return: an m x n matrix, which is the derivative of the exponential kernel function evaluated at point z
against the training dataset.
"""
if self.kernel_type != 'squared_exponential':
raise NotImplementedError
len_scale = self.params['l'] if len(self.params['l']) > 0 else self.params['l'] * cs.MX.ones(z_train.shape[1])
len_scale = np.atleast_2d(len_scale ** 2)
# Broadcast z vector to have the shape of z_train (tile z to to the number of training points n)
z_tile = cs.mtimes(cs.MX.ones(z_train.shape[0], 1), z.T)
# Compute k_zZ. Broadcast it to shape of z_tile and z_train, i.e. by the number of variables in z.
k_zZ = cs.mtimes(cs.MX.ones(z_train.shape[1], 1), self.__call__(z_train, z.T).T)
return - k_zZ * (z_tile - z_train).T / cs.mtimes(cs.MX.ones(z_train.shape[0], 1), len_scale).T
class CustomGPRegression:
def __init__(self, x_features, u_features, reg_dim, mean=None, y_mean=None, kernel=None, sigma_n=1e-8,
n_restarts=1):
"""
:param x_features: list of indices for the quadrotor state-derived features
:param u_features: list of indices for the input state-derived features
:param reg_dim: state dimension that this regressor is meant to be used for.
:param mean: prior mean value
:param y_mean: average y value for data normalization
:param kernel: Kernel Function object
:param sigma_n: noise sigma value
:param n_restarts: number of optimization re-initializations
"""
if kernel is None:
kernel = CustomKernelFunctions('squared_exponential')
# Avoid non-invertible error
assert sigma_n != 0.0
# For inference time
self.x_features = x_features
self.u_features = u_features
self.reg_dim = reg_dim
self.kernel_ = kernel
self.kernel_type = kernel.kernel_type
# Noise variance prior
self.sigma_n = sigma_n
# GP center of local feature space
self.mean = mean
self.y_mean = y_mean
# Pre-computed training data kernel
self._K = np.zeros((0, 0))
self._K_inv = np.zeros((0, 0))
self._K_inv_y = np.zeros((0, ))
# Training dataset memory
self._x_train = np.zeros((0, 0))
self._y_train = np.zeros((0, ))
# CasADi symbolic equivalents
self._K_cs = None
self._K_inv_cs = None
self._K_inv_y_cs = None
self._x_train_cs = None
self._y_train_cs = None
self.sym_pred = None
self.sym_jacobian_dz = None
self.n_restarts = n_restarts
@property
def kernel(self):
return self.kernel_
@kernel.setter
def kernel(self, ker):
self.kernel_ = ker
@property
def K(self):
return self._K
@K.setter
def K(self, k):
self._K = k
self._K_cs = cs.DM(k)
@property
def K_inv(self):
return self._K_inv
@K_inv.setter
def K_inv(self, k):
self._K_inv = k
self._K_inv_cs = cs.DM(k)
@property
def K_inv_y(self):
return self._K_inv_y
@K_inv_y.setter
def K_inv_y(self, k):
self._K_inv_y = k
self._K_inv_y_cs = cs.DM(k)
@property
def x_train(self):
return self._x_train
@x_train.setter
def x_train(self, k):
self._x_train = k
self._x_train_cs = cs.DM(k)
@property
def y_train(self):
return self._y_train
@y_train.setter
def y_train(self, k):
self._y_train = k
self._y_train_cs = cs.DM(k)
def log_marginal_likelihood(self, theta):
l_params = np.exp(theta[:-1])
sigma_f = np.exp(theta[-1])
sigma_n = self.sigma_n
kernel = CustomKernelFunctions(self.kernel_type, params={'l': l_params, 'sigma_f': sigma_f})
k_train = kernel(self.x_train, self.x_train) + sigma_n ** 2 * np.eye(len(self.x_train))
l_mat = cholesky(k_train)
nll = np.sum(np.log(np.diagonal(l_mat))) + \
0.5 * self.y_train.T.dot(lstsq(l_mat.T, lstsq(l_mat, self.y_train, rcond=None)[0], rcond=None)[0]) + \
0.5 * len(self.x_train) * np.log(2 * np.pi)
return nll
def _nll(self, x_train, y_train):
"""
Returns a numerically stable function implementation of the negative log likelihood using the cholesky
decomposition of the kernel matrix. http://www.gaussianprocess.org/gpml/chapters/RW2.pdf, Section 2.2,
Algorithm 2.1.
:param x_train: Array of m points (m x d).
:param y_train: Array of m points (m x 1)
:return: negative log likelihood (scalar) computing function
"""
def nll_func(theta):
l_params = np.exp(theta[:-2])
sigma_f = np.exp(theta[-2])
sigma_n = np.exp(theta[-1])
kernel = CustomKernelFunctions(self.kernel_type, params={'l': l_params, 'sigma_f': sigma_f})
k_train = kernel(x_train, x_train) + sigma_n ** 2 * np.eye(len(x_train))
l_mat = cholesky(k_train)
nll = np.sum(np.log(np.diagonal(l_mat))) + \
0.5 * y_train.T.dot(lstsq(l_mat.T, lstsq(l_mat, y_train, rcond=None)[0], rcond=None)[0]) + \
0.5 * len(x_train) * np.log(2 * np.pi)
return nll
return nll_func
def _constrained_minimization(self, x_train, y_train, x_0, bounds):
try:
res = minimize(self._nll(x_train, y_train), x0=x_0, bounds=bounds, method='L-BFGS-B')
return np.exp(res.x), res.fun
except np.linalg.LinAlgError:
return x_0, np.inf
def fit(self, x_train, y_train):
"""
Fit a GP regressor to the training dataset by minimizing the negative log likelihood of the training data
:param x_train: Array of m points (m x d).
:param y_train: Array of m points (m x 1)
"""
initial_guess = self.kernel.get_trainable_parameters()
initial_guess += [self.sigma_n]
initial_guess = np.array(initial_guess)
bounds = [(1e-5, 1e1) for _ in range(len(initial_guess) - 1)]
bounds = bounds + [(1e-8, 1e0)]
log_bounds = np.log(tuple(bounds))
y_train -= self.y_mean
optima = [self._constrained_minimization(x_train, y_train, initial_guess, log_bounds)]
if self.n_restarts > 1:
random_state = mtrand._rand
for iteration in range(self.n_restarts - 1):
theta_initial = random_state.uniform(log_bounds[:, 0], log_bounds[:, 1])
optima.append(self._constrained_minimization(x_train, y_train, theta_initial, log_bounds))
lml_values = list(map(itemgetter(1), optima))
theta_opt = optima[int(np.argmin(lml_values))][0]
# Update kernel with new parameters
l_new = theta_opt[:-2]
sigma_f_new = theta_opt[-2]
self.sigma_n = theta_opt[-1]
self.kernel = CustomKernelFunctions(self.kernel_type, params={'l': l_new, 'sigma_f': sigma_f_new})
# Pre-compute kernel matrices
self.K = self.kernel(x_train, x_train) + self.sigma_n ** 2 * np.eye(len(x_train))
self.K_inv = inv(self.K)
self.K_inv_y = self.K_inv.dot(y_train)
# Update training dataset points
self.x_train = x_train
self.y_train = y_train
self.compute_gp_jac()
def compute_gp_jac(self):
self.sym_jacobian_dz = self._linearized_state_estimate()
def eval_gp_jac(self, z):
if self.sym_jacobian_dz is None:
self.compute_gp_jac()
return self.sym_jacobian_dz(z)
def _linearized_state_estimate(self):
"""
Computes the symbolic linearization of the GP prediction expected state with respect to the inputs of the GP
itself.
:return: a CasADi function that computes the linearized GP prediction estimate wrt the input features of the
GP regressor itself. The output of the function is a vector of shape m, where m is the number of regression
features.
"""
if self.kernel_type != 'squared_exponential':
raise NotImplementedError
# Symbolic variable for input state
z = cs.MX.sym('z', self.x_train.shape[1])
# Compute the kernel derivative:
dgpdz = cs.mtimes(self.kernel.diff(z, self._x_train_cs), self._K_inv_y_cs)
return cs.Function('f', [z], [dgpdz], ['z'], ['dgpdz'])
def predict(self, x_test, return_std=False, return_cov=False):
"""
Computes the sufficient statistics of the GP posterior predictive distribution
from m training data X_train and Y_train and n new inputs X_s.
Args:
x_test: test input locations (n x d).
return_std: boolean - return a standard deviation vector of size n
return_cov: boolean - return a covariance vector of size n (sqrt of standard deviation)
Returns:
Posterior mean vector (n) and covariance diagonal or standard deviation vectors (n) if also requested.
"""
# Ensure at least n=1
x_test = np.atleast_2d(x_test) if isinstance(x_test, np.ndarray) else x_test
if isinstance(x_test, cs.MX):
return self._predict_sym(x_test=x_test, return_std=return_std, return_cov=return_cov)
if isinstance(x_test, cs.DM):
x_test = np.array(x_test).T
k_s = self.kernel(x_test, self.x_train)
k_ss = self.kernel(x_test, x_test) + 1e-8 * np.eye(len(x_test))
# Posterior mean value
mu_s = k_s.dot(self.K_inv_y) + self.y_mean
# Posterior covariance
cov_s = k_ss - k_s.dot(self.K_inv).dot(k_s.T)
std_s = np.sqrt(np.diag(cov_s))
if not return_std and not return_cov:
return mu_s
# Return covariance
if return_cov:
return mu_s, std_s ** 2
# Return standard deviation
return mu_s, std_s
def _predict_sym(self, x_test, return_std=False, return_cov=False):
"""
Computes the GP posterior mean and covariance at a given a test sample using CasADi symbolics.
:param x_test: vector of size mx1, where m is the number of features used for the GP prediction
:return: the posterior mean (scalar) and covariance (scalar).
"""
k_s = self.kernel(self._x_train_cs, x_test.T)
# Posterior mean value
mu_s = cs.mtimes(k_s.T, self._K_inv_y_cs) + self.y_mean
if not return_std and not return_cov:
return {'mu': mu_s}
k_ss = self.kernel(x_test, x_test) + 1e-8 * cs.MX.eye(x_test.shape[1])
# Posterior covariance
cov_s = k_ss - cs.mtimes(cs.mtimes(k_s.T, self._K_inv_cs), k_s)
cov_s = cs.diag(cov_s)
if return_std:
return {'mu': mu_s, 'std': np.sqrt(cov_s)}
return {'mu': mu_s, 'cov': cov_s}
def sample_y(self, x_test, n_samples=3):
"""
Sample a number of functions from the process at the given test points
:param x_test: test input locations (n x d).
:param n_samples: integer, number of samples to draw
:return: the drawn samples from the gaussian process. An array of shape n x n_samples
"""
mu, cov = self.predict(x_test, return_cov=True)
# Draw three samples from the prior
samples = np.random.multivariate_normal(mu.ravel(), np.diag(cov), n_samples)
return samples.T
def save(self, path):
"""
Saves the current GP regressor to the specified path as a pickle file. Must be re-loaded with the function load
:param path: absolute path to save the regressor to
"""
saved_vars = {
"kernel_params": self.kernel.params,
"kernel_type": self.kernel.kernel_type,
"x_train": self.x_train,
"y_train": self.y_train,
"k_inv_y": self.K_inv_y,
"k_inv": self.K_inv,
"sigma_n": self.sigma_n,
"reg_dim": self.reg_dim,
"x_features": self.x_features,
"u_features": self.u_features,
"mean": self.mean,
"y_mean": self.y_mean
}
split_path = path.split('/')
directory = '/'.join(split_path[:-1])
file = split_path[-1]
safe_mknode_recursive(directory, file, overwrite=True)
with open(path, 'wb') as f:
joblib.dump(saved_vars, f)
def load(self, data_dict):
"""
Load a pre-trained GP regressor
:param data_dict: a dictionary with all the pre-trained matrices of the GP regressor
"""
self.K_inv = data_dict['k_inv']
self.K_inv_y = data_dict['k_inv_y']
self.x_train = data_dict['x_train']
self.y_train = data_dict['y_train']
self.kernel_type = data_dict['kernel_type']
self.kernel = CustomKernelFunctions(self.kernel_type, data_dict['kernel_params'])
self.sigma_n = data_dict['sigma_n']
self.mean = data_dict['mean'] if 'mean' in data_dict.keys() else np.array([0, 0, 0])
self.y_mean = data_dict['y_mean'] if 'y_mean' in data_dict.keys() else np.array(0)
self.compute_gp_jac()
class GPEnsemble:
def __init__(self):
"""
Sets up a GP regression ensemble. This essentially divides the prediction domain into different GP's, so that
less training samples need to be used per GP.
"""
self.out_dim = 0
self.n_models_dict = {}
# Make index to dim to make dimensions iterable
self.dim_idx = np.zeros(0, dtype=int)
# Dictionary of lists. Each element of the dictionary is indexed by the index of the GP output in the state
# space, and contains a with all the GP's (one per cluster) used in that dimension.
self.gp = {}
# Store the centroids of all GP's
self.gp_centroids = {}
# Store the B_z matrices
self.B_z_dict = {}
# Whether the same clustering is used for all dimensions or not
self.homogeneous = True
# Whether the GP model has no ensembles in it (i.e. no GP has more than 1 cluster)
self.no_ensemble = True
@property
def n_models(self):
if self.homogeneous or self.no_ensemble:
return self.n_models_dict[next(iter(self.n_models_dict))]
return self.n_models_dict
@property
def B_z(self):
return self.B_z_dict[next(iter(self.B_z_dict))] if self.homogeneous else self.B_z_dict
def add_model(self, gp):
""""
:param gp: A list of n CustomGPRegression objects, where n is the number of GP's used to divide the feature
space domain of the dimension in particular.
:type gp: list
"""
# Check if dimension is already "occupied" by another GP
gp_dim = gp[0].reg_dim
if gp_dim in self.gp.keys():
raise ValueError("This dimension is already taken by another GP")
self.out_dim += 1
self.dim_idx = np.append(self.dim_idx, gp_dim)
self.gp[gp_dim] = np.array(gp)
# Store centroids and sort along first dimension for easier comparison
self.gp_centroids[gp_dim] = np.array([gp_cluster.mean for gp_cluster in gp])
sorted_cluster_idx = np.argsort(self.gp_centroids[gp_dim][:, 0])
self.gp_centroids[gp_dim] = self.gp_centroids[gp_dim][sorted_cluster_idx]
self.gp[gp_dim] = self.gp[gp_dim][sorted_cluster_idx]
# Calculate if Ensemble is still homogeneous
self.homogeneous = self.homogeneous_feature_space()
# Check if current gp is an actual ensemble
self.n_models_dict[gp_dim] = len(gp)
if len(gp) > 1:
self.no_ensemble = False
# Pre-compute B_z matrix
self.B_z_dict[gp_dim] = make_bz_matrix(x_dims=13, u_dims=4, x_feats=gp[0].x_features, u_feats=gp[0].u_features)
def get_z(self, x, u, dim):
"""
Computes the z features from the x and u vectors, and the target output dimension.
:param x: state vector. Shape 13x1. Can be np.array or cs.MX.
:param u: control input vector. Shape 4x1. Can be np.array or cs.MX.
:param dim: output dimension target.
:return: A vector of shape mx1 of the same format as inputs. m is determined by the B_z matrix for dim.
"""
# Get input feature indices
x_feats = self.gp[dim][0].x_features
u_feats = self.gp[dim][0].u_features
# Stack into a single matrix
if isinstance(x, np.ndarray):
z = np.concatenate((x[x_feats], u[u_feats]), axis=0)
elif isinstance(x, cs.MX):
z = cs.mtimes(self.B_z_dict[dim], cs.vertcat(x, u))
else:
raise TypeError
return z
def predict(self, x_test, u_test, return_std=False, return_cov=False, return_gp_id=False, return_z=False,
progress_bar=False, gp_idx=None):
"""
Runs GP inference. First, select the GP optimally for the test samples. Then, run inference on that GP.
:param x_test: array of shape d x n. n is the number of test samples and d their dimension.
:param u_test: array of shape d' x n. n is the number of test samples and d' their dimension.
:param return_std: True if also return the standard deviation of the GP inference.
:param return_cov: True if also return the covariance of the GP inference.
:param return_gp_id: True if also return the id of the GP used for inference.
:param return_z: True if also return the z features computed for inference.
:param progress_bar: If True, a progress bar will be shown when evaluating the test data.
:param gp_idx: Dictionary of indices with the same length as the GP output dimension indicating which GP to use
for each one. If None, select best based on x_test.
:type gp_idx: dict
:return: m x n arrays, where m is the output dimension and n is the number of samples.
"""
if return_std:
assert not return_cov, "Can only return the std or the cov"
if return_cov:
assert not return_std, "Can only return the std or the cov"
# Dictionary for function return
outputs = {}
# Build regression features and evaluation cluster indices for each GP output dimension
z = {}
gp_idx = {} if gp_idx is None else gp_idx
if not self.homogeneous:
for dim in self.gp.keys():
z[dim] = self.get_z(x_test, u_test, dim)
if dim not in gp_idx.keys():
# Calculate optimal GP clusters to use for each test point
gp_idx[dim] = self.select_gp(z=z[dim], dim=dim)
gp_idx[dim] = np.atleast_1d(gp_idx[dim])
else:
z_ = self.get_z(x_test, u_test, self.dim_idx[0])
z = {k: v for k, v in zip(self.dim_idx, [z_] * self.out_dim)}
if not bool(gp_idx):
gp_idx_ = self.select_gp(z=z_, dim=self.dim_idx[0])
gp_idx = {k: v for k, v in zip(self.dim_idx, [gp_idx_] * self.out_dim)}
# Add stuff to output dictionary
if return_z:
outputs["z"] = z
if return_gp_id:
outputs["gp_id"] = gp_idx
pred = []
cov_or_std = []
noise_prior = []
# Test data loop
range_data = tqdm(range(x_test.shape[1])) if progress_bar else range(x_test.shape[1])
for j in range_data:
pred_j = []
cov_or_std_j = []
noise_prior_j = []
# Output dim loop
for dim in self.gp.keys():
out = self.gp[dim][gp_idx[dim][j]].predict(z[dim][:, j], return_std, return_cov)
if not return_std and not return_cov:
if isinstance(out, dict):
pred_j += [out['mu']]
else:
pred_j += [out]
else:
if isinstance(out, dict):
pred_j += [out['mu']]
cov_or_std_j += [out['cov'] if 'cov' in out.keys() else out['std']]
else:
pred_j += [out[0]]
cov_or_std_j += [out[1]]
noise_prior_j += [np.array([self.gp[dim][gp_idx[dim][j]].sigma_n])]
pred += [pred_j]
cov_or_std += [cov_or_std_j]
noise_prior += [noise_prior_j]
# Convert to CasADi symbolic or numpy matrix depending on the input type
pred = cs.horzcat(*[cs.vertcat(*pred[i]) for i in range(x_test.shape[1])]) \
if isinstance(x_test, cs.MX) else np.squeeze(np.array(pred)).T
outputs["pred"] = pred
if not return_std and not return_cov:
return outputs
# Convert to CasADi symbolic or numpy matrix depending on the input type
cov_or_std = cs.horzcat(*[cs.vertcat(*cov_or_std[i]) for i in range(x_test.shape[1])]) \
if isinstance(x_test, cs.MX) else np.squeeze(np.array(cov_or_std)).T
noise_prior = cs.horzcat(*[cs.vertcat(*noise_prior[i]) for i in range(x_test.shape[1])]) \
if isinstance(x_test, cs.MX) else np.squeeze(np.array(noise_prior)).T
outputs["cov_or_std"] = cov_or_std
outputs["noise_cov"] = noise_prior
return outputs
def select_gp(self, dim, x=None, u=None, z=None):
"""
Selects the best GP's for computing inference at the given test points x for a given regression output
dimension. This calculation is done by computing the distance of all n test points to all available GP's
centroids and selecting the one minimizing the Euclidean distance.
:param z: np array of shape d x n corresponding to the processed feature vector. If unknown one may call this
method with x and u instead.
:param x: np array of shape 13 x n corresponding to the query quadrotor states. Only necessary if z=None.
:param u: np.array of shape 4 x n corresponding to the query quadrotor control vectors. Only necessary if
z=None.
:param dim: index of GP output dimension. If None, evaluate on all dimensions.
:return: a numpy vector of length n, indicating which GP to use for every test sample x.
"""
if dim is None:
dim = self.dim_idx
if isinstance(dim, np.ndarray):
# If the ensemble is homogeneous only one evaluation is necessary
if self.homogeneous or self.no_ensemble:
return self.select_gp(dim[0], x, u, z)[0]
return np.array([self.select_gp(i, x, u, z) for i in dim])
if z is None:
z = self.get_z(x, u, dim)
z = np.atleast_2d(z)
centroids = self.gp_centroids[dim]
# Select subset of features for current dimension
return np.argmin(np.sqrt(np.sum((z[np.newaxis, :, :] - centroids[:, :, np.newaxis]) ** 2, 1)), 0)
def homogeneous_feature_space(self):
if self.out_dim == 1:
return True
equal_clusters = True
last_centroids = None
for i, key in enumerate(self.gp_centroids.keys()):
centroids = self.gp_centroids[key]
if i == 0:
last_centroids = centroids
continue
if np.any(last_centroids != centroids):
equal_clusters = False
break
last_centroids = centroids
return equal_clusters
================================================
FILE: ros_dd_mpc/src/model_fitting/gp_common.py
================================================
""" Contains a set of utility functions and classes for the GP training and inference.
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/>.
"""
import copy
import os
import joblib
import numpy as np
import pandas as pd
from sklearn.mixture import GaussianMixture
from src.model_fitting.gp import GPEnsemble, CustomGPRegression as npGPRegression
from src.utils.utils import undo_jsonify, prune_dataset, safe_mknode_recursive, get_data_dir_and_file, \
separate_variables, v_dot_q, quaternion_inverse
from src.utils.visualization import visualize_data_distribution
class GPDataset:
def __init__(self, raw_ds=None,
x_features=None, u_features=None, y_dim=None,
cap=None, n_bins=None, thresh=None,
visualize_data=False):
self.data_labels = [r'$p_x$', r'$p_y$', r'$p_z$', r'$q_w$', r'$q_x$', r'$q_y$', r'$q_z$',
r'$v_x$', r'$v_y$', r'$v_z$', r'$w_x$', r'$w_y$', r'$w_z$']
# Raw dataset data
self.x_raw = None
self.x_out_raw = None
self.u_raw = None
self.y_raw = None
self.x_pred_raw = None
self.dt_raw = None
self.x_features = x_features
self.u_features = u_features
self.y_dim = y_dim
# Data pruning parameters
self.cap = cap
self.n_bins = n_bins
self.thresh = thresh
self.plot = visualize_data
# GMM clustering
self.pruned_idx = []
self.cluster_agency = None
self.centroids = None
# Number of data clusters
self.n_clusters = 1
if raw_ds is not None:
self.load_data(raw_ds)
self.prune()
def load_data(self, ds):
if isinstance(ds, np.lib.npyio.NpzFile):
x_raw = ds['state_in']
x_out = ds['state_out']
x_pred = ds['state_pred']
u_raw = ds['input_in']
dt = ds["dt"]
else:
x_raw = undo_jsonify(ds['state_in'].to_numpy())
x_out = undo_jsonify(ds['state_out'].to_numpy())
x_pred = undo_jsonify(ds['state_pred'].to_numpy())
u_raw = undo_jsonify(ds['input_in'].to_numpy())
dt = ds["dt"].to_numpy()
invalid = np.where(dt == 0)
# Remove invalid entries (dt = 0)
x_raw = np.delete(x_raw, invalid, axis=0)
x_out = np.delete(x_out, invalid, axis=0)
x_pred = np.delete(x_pred, invalid, axis=0)
u_raw = np.delete(u_raw, invalid, axis=0)
dt = np.delete(dt, invalid, axis=0)
# Rotate velocities to body frame and recompute errors
x_raw = world_to_body_velocity_mapping(x_raw)
x_pred = world_to_body_velocity_mapping(x_pred)
x_out = world_to_body_velocity_mapping(x_out)
y_err = x_out - x_pred
# Normalize error by window time (i.e. predict error dynamics instead of error itself)
y_err /= np.expand_dims(dt, 1)
# Select features
self.x_raw = x_raw
self.x_out_raw = x_out
self.u_raw = u_raw
self.y_raw = y_err
self.x_pred_raw = x_pred
self.dt_raw = dt
def prune(self):
# Prune noisy data
if self.cap is not None and self.n_bins is not None and self.thresh is not None:
x_interest = np.array([7, 8, 9])
y_interest = np.array([7, 8, 9])
labels = [self.data_labels[dim] for dim in np.atleast_1d(y_interest)]
prune_x_data = self.get_x(pruned=False, raw=True)[:, x_interest]
prune_y_data = self.get_y(pruned=False, raw=True)[:, y_interest]
self.pruned_idx.append(prune_dataset(prune_x_data, prune_y_data, self.cap, self.n_bins, self.thresh,
plot=self.plot, labels=labels))
def get_x(self, cluster=None, pruned=True, raw=False):
if cluster is not None:
assert pruned
if raw:
return self.x_raw[tuple(self.pruned_idx)] if pruned else self.x_raw
if self.x_features is not None and self.u_features is not None:
x_f = self.x_features
u_f = self.u_features
data = np.concatenate((self.x_raw[:, x_f], self.u_raw[:, u_f]), axis=1) if u_f else self.x_raw[:, x_f]
else:
data = np.concatenate((self.x_raw, self.u_raw), axis=1)
data = data[:, np.newaxis] if len(data.shape) == 1 else data
if pruned or cluster is not None:
data = data[tuple(self.pruned_idx)]
data = data[self.cluster_agency[cluster]] if cluster is not None else data
return data
@property
def x(self):
return self.get_x()
def get_x_out(self, cluster=None, pruned=True):
if cluster is not None:
assert pruned
if pruned or cluster is not None:
data = self.x_out_raw[tuple(self.pruned_idx)]
data = data[self.cluster_agency[cluster]] if cluster is not None else data
return data
return self.x_out_raw[tuple(self.pruned_idx)] if pruned else self.x_out_raw
@property
def x_out(self):
return self.get_x_out()
def get_u(self, cluster=None, pruned=True, raw=False):
if cluster is not None:
assert pruned
if raw:
return self.u_raw[tuple(self.pruned_idx)] if pruned else self.u_raw
data = self.u_raw[:, self.u_features] if self.u_features is not None else self.u_raw
data = data[:, np.newaxis] if len(data.shape) == 1 else data
if pruned or cluster is not None:
data = data[tuple(self.pruned_idx)]
data = data[self.cluster_agency[cluster]] if cluster is not None else data
return data
@property
def u(self):
return self.get_u()
def get_y(self, cluster=None, pruned=True, raw=False):
if cluster is not None:
assert pruned
if raw:
return self.y_raw[self.pruned_idx] if pruned else self.y_raw
data = self.y_raw[:, self.y_dim] if self.y_dim is not None else self.y_raw
data = data[:, np.newaxis] if len(data.shape) == 1 else data
if pruned or cluster is not None:
data = data[tuple(self.pruned_idx)]
data = data[self.cluster_agency[cluster]] if cluster is not None else data
return data
@property
def y(self):
return self.get_y()
def get_x_pred(self, pruned=True, raw=False):
if raw:
return self.x_pred_raw[tuple(self.pruned_idx)] if pruned else self.x_pred_raw
data = self.x_pred_raw[:, self.y_dim] if self.y_dim is not None else self.x_pred_raw
data = data[:, np.newaxis] if len(data.shape) == 1 else data
if pruned:
data = data[tuple(self.pruned_idx)]
return data
@property
def x_pred(self):
return self.get_x_pred()
def get_dt(self, pruned=True):
return self.dt_raw[tuple(self.pruned_idx)] if pruned else self.dt_raw
@property
def dt(self):
return self.get_dt()
def cluster(self, n_clusters, load_clusters=False, save_dir="", visualize_data=False):
self.n_clusters = n_clusters
x_pruned = self.x
y_pruned = self.y
file_name = os.path.join(save_dir, 'gmm.pkl')
loaded = False
gmm = None
if os.path.exists(file_name) and load_clusters:
print("Loaded GP clusters from last GP training session. File: {}".format(file_name))
gmm = joblib.load(file_name)
if gmm.n_components != n_clusters:
print("The loaded GP does not coincide with the number of set clusters: Found {} but requested"
"is {}".format(gmm.n_components, n_clusters))
else:
loaded = True
if not loaded:
gmm = GaussianMixture(n_clusters).fit(x_pruned)
clusters = gmm.predict_proba(x_pruned)
centroids = gmm.means_
if not load_clusters and n_clusters > 1:
safe_mknode_recursive(save_dir, 'gmm.pkl', overwrite=True)
joblib.dump(gmm, file_name)
index_aux = np.arange(0, clusters.shape[0])
cluster_agency = {}
# Add to the points corresponding to each cluster the points that correspond to it with top 2 probability,
# if this probability is high
for cluster in range(n_clusters):
top_1 = np.argmax(clusters, 1)
clusters_ = copy.deepcopy(clusters)
clusters_[index_aux, top_1] *= 0
top_2 = np.argmax(clusters_, 1)
idx = np.where(top_1 == cluster)[0]
idx = np.append(idx, np.where((top_2 == cluster) * (clusters_[index_aux, top_2] > 0.2))[0])
cluster_agency[cluster] = idx
# Only works in len(x_features) >= 3
if visualize_data:
x_aux = self.get_x(pruned=False)
y_aux = self.get_y(pruned=False)
visualize_data_distribution(x_aux, y_aux, cluster_agency, x_pruned, y_pruned)
self.cluster_agency = cluster_agency
self.centroids = centroids
def restore_gp_regressors(pre_trained_models):
"""
:param pre_trained_models: A dictionary with all the GP models to be restored in 'models'.
:return: The GP ensemble restored from the models.
:rtype: GPEnsemble
"""
gp_reg_ensemble = GPEnsemble()
# TODO: Deprecate compatibility mode with old models.
if all(item in list(pre_trained_models.keys()) for item in ["x_features", "u_features"]):
x_features = pre_trained_models["x_features"]
u_features = pre_trained_models["u_features"]
else:
x_features = u_features = None
if isinstance(pre_trained_models['models'][0], dict):
pre_trained_gp_reg = {}
for _, model_dict in enumerate(pre_trained_models['models']):
if x_features is not None:
gp_reg = npGPRegression(x_features, u_features, model_dict["reg_dim"])
else:
gp_reg = npGPRegression(model_dict["x_features"], model_dict["u_features"], model_dict["reg_dim"])
gp_reg.load(model_dict)
if model_dict["reg_dim"] not in pre_trained_gp_reg.keys():
pre_trained_gp_reg[model_dict["reg_dim"]] = [gp_reg]
else:
pre_trained_gp_reg[model_dict["reg_dim"]] += [gp_reg]
# Add the GP's in a per-output-dim basis into the Ensemble
for key in np.sort(list(pre_trained_gp_reg.keys())):
gp_reg_ensemble.add_model(pre_trained_gp_reg[key])
else:
gitextract__1r96zfd/
├── .gitignore
├── .gitmodules
├── LICENSE
├── README.md
├── requirements.txt
└── ros_dd_mpc/
├── CMakeLists.txt
├── config/
│ ├── agisim_simulation_run.yaml
│ ├── arena_limits.yaml
│ ├── arena_run.yaml
│ ├── basic.world
│ ├── circle_and_lemniscate_options.yaml
│ ├── configuration_parameters.py
│ ├── flyingroom_limits.yaml
│ ├── ground_effect_limits.yaml
│ ├── kingfisher.yaml
│ └── simulation_run.yaml
├── launch/
│ └── dd_mpc_wrapper.launch
├── msg/
│ └── ReferenceTrajectory.msg
├── nodes/
│ ├── dd_mpc_node.py
│ └── reference_publisher_node.py
├── package.xml
└── src/
├── __init__.py
├── experiments/
│ ├── __init__.py
│ ├── comparative_experiment.py
│ ├── point_tracking_and_record.py
│ └── trajectory_test.py
├── model_fitting/
│ ├── __init__.py
│ ├── gp.py
│ ├── gp_common.py
│ ├── gp_fitting.py
│ ├── gp_visualization.py
│ ├── mlp_common.py
│ ├── mlp_fitting.py
│ ├── mlp_quad_res_fitting.py
│ ├── process_neurobem_dataset.py
│ ├── rdrv_fitting.py
│ └── system_identification.py
├── quad_mpc/
│ ├── __init__.py
│ ├── create_ros_dd_mpc.py
│ ├── quad_3d.py
│ ├── quad_3d_mpc.py
│ └── quad_3d_optimizer.py
└── utils/
├── __init__.py
├── animator.py
├── ground_map.py
├── keyframe_3d_gen.py
├── quad_3d_opt_utils.py
├── trajectories.py
├── trajectory_generator.py
├── utils.py
└── visualization.py
SYMBOL INDEX (260 symbols across 28 files)
FILE: ros_dd_mpc/config/configuration_parameters.py
class DirectoryConfig (line 17) | class DirectoryConfig:
class SimpleSimConfig (line 29) | class SimpleSimConfig:
class ModelFitConfig (line 54) | class ModelFitConfig:
class GroundEffectMapConfig (line 112) | class GroundEffectMapConfig:
FILE: ros_dd_mpc/nodes/dd_mpc_node.py
function odometry_parse (line 42) | def odometry_parse(odom_msg):
function state_parse (line 51) | def state_parse(state_msg):
function make_raw_optitrack_dict (line 61) | def make_raw_optitrack_dict():
function make_state_record_dict (line 71) | def make_state_record_dict(state_dim):
function odometry_skipped_warning (line 81) | def odometry_skipped_warning(last_seq, current_seq, stage):
class DDMPCWrapper (line 86) | class DDMPCWrapper:
method __init__ (line 87) | def __init__(self, quad_name, environment="agisim", recording_options=...
method land_callback (line 327) | def land_callback(self, _):
method rest_state (line 336) | def rest_state(self):
method create_command_msg (line 344) | def create_command_msg(self, w_opt, x_opt):
method run_mpc (line 359) | def run_mpc(self, odom, recording=True):
method check_out_initial_state (line 410) | def check_out_initial_state(self, odom):
method reference_callback (line 425) | def reference_callback(self, msg):
method odometry_callback (line 470) | def odometry_callback(self, msg):
method raw_odometry_callback (line 554) | def raw_odometry_callback(self, msg):
method hover_here (line 570) | def hover_here(self, x):
method set_reference (line 578) | def set_reference(self):
method plot_tracking_mse_experiment (line 765) | def plot_tracking_mse_experiment(self):
method save_recording_data (line 781) | def save_recording_data(self):
function main (line 830) | def main():
FILE: ros_dd_mpc/nodes/reference_publisher_node.py
class ReferenceGenerator (line 24) | class ReferenceGenerator:
method __init__ (line 26) | def __init__(self):
method status_callback (line 215) | def status_callback(self, msg):
FILE: ros_dd_mpc/src/experiments/comparative_experiment.py
function prepare_quadrotor_mpc (line 35) | def prepare_quadrotor_mpc(simulation_options, version=None, name=None, r...
function main (line 134) | def main(quad_mpc, av_speed, reference_type=None, plot=False):
FILE: ros_dd_mpc/src/experiments/point_tracking_and_record.py
function get_record_file_and_dir (line 32) | def get_record_file_and_dir(record_dict_template, recording_options, sim...
function make_record_dict (line 55) | def make_record_dict(state_dim):
function check_out_data (line 69) | def check_out_data(rec_dict, state_final, x_pred, w_opt, dt):
function sample_random_target (line 82) | def sample_random_target(x_current, world_radius, aggressive=True):
function main (line 110) | def main(model_options, recording_options, simulation_options, parameters):
FILE: ros_dd_mpc/src/experiments/trajectory_test.py
function main (line 28) | def main(args):
FILE: ros_dd_mpc/src/model_fitting/gp.py
class CustomKernelFunctions (line 29) | class CustomKernelFunctions:
method __init__ (line 31) | def __init__(self, kernel_func, params=None):
method __call__ (line 45) | def __call__(self, x_1, x_2):
method __str__ (line 48) | def __str__(self):
method get_trainable_parameters (line 62) | def get_trainable_parameters(self):
method _check_length_scale (line 72) | def _check_length_scale(x, length_scale):
method squared_exponential_kernel (line 81) | def squared_exponential_kernel(self, x_1, x_2=None):
method _squared_exponential_kernel_cs (line 117) | def _squared_exponential_kernel_cs(self, x_1, x_2):
method diff (line 140) | def diff(self, z, z_train):
class CustomGPRegression (line 168) | class CustomGPRegression:
method __init__ (line 170) | def __init__(self, x_features, u_features, reg_dim, mean=None, y_mean=...
method kernel (line 226) | def kernel(self):
method kernel (line 230) | def kernel(self, ker):
method K (line 234) | def K(self):
method K (line 238) | def K(self, k):
method K_inv (line 243) | def K_inv(self):
method K_inv (line 247) | def K_inv(self, k):
method K_inv_y (line 252) | def K_inv_y(self):
method K_inv_y (line 256) | def K_inv_y(self, k):
method x_train (line 261) | def x_train(self):
method x_train (line 265) | def x_train(self, k):
method y_train (line 270) | def y_train(self):
method y_train (line 274) | def y_train(self, k):
method log_marginal_likelihood (line 278) | def log_marginal_likelihood(self, theta):
method _nll (line 292) | def _nll(self, x_train, y_train):
method _constrained_minimization (line 318) | def _constrained_minimization(self, x_train, y_train, x_0, bounds):
method fit (line 325) | def fit(self, x_train, y_train):
method compute_gp_jac (line 371) | def compute_gp_jac(self):
method eval_gp_jac (line 375) | def eval_gp_jac(self, z):
method _linearized_state_estimate (line 382) | def _linearized_state_estimate(self):
method predict (line 403) | def predict(self, x_test, return_std=False, return_cov=False):
method _predict_sym (line 446) | def _predict_sym(self, x_test, return_std=False, return_cov=False):
method sample_y (line 473) | def sample_y(self, x_test, n_samples=3):
method save (line 489) | def save(self, path):
method load (line 518) | def load(self, data_dict):
class GPEnsemble (line 536) | class GPEnsemble:
method __init__ (line 537) | def __init__(self):
method n_models (line 566) | def n_models(self):
method B_z (line 572) | def B_z(self):
method add_model (line 575) | def add_model(self, gp):
method get_z (line 609) | def get_z(self, x, u, dim):
method predict (line 632) | def predict(self, x_test, u_test, return_std=False, return_cov=False, ...
method select_gp (line 738) | def select_gp(self, dim, x=None, u=None, z=None):
method homogeneous_feature_space (line 772) | def homogeneous_feature_space(self):
FILE: ros_dd_mpc/src/model_fitting/gp_common.py
class GPDataset (line 29) | class GPDataset:
method __init__ (line 30) | def __init__(self, raw_ds=None,
method load_data (line 68) | def load_data(self, ds):
method prune (line 108) | def prune(self):
method get_x (line 121) | def get_x(self, cluster=None, pruned=True, raw=False):
method x (line 144) | def x(self):
method get_x_out (line 147) | def get_x_out(self, cluster=None, pruned=True):
method x_out (line 161) | def x_out(self):
method get_u (line 164) | def get_u(self, cluster=None, pruned=True, raw=False):
method u (line 182) | def u(self):
method get_y (line 185) | def get_y(self, cluster=None, pruned=True, raw=False):
method y (line 203) | def y(self):
method get_x_pred (line 206) | def get_x_pred(self, pruned=True, raw=False):
method x_pred (line 220) | def x_pred(self):
method get_dt (line 223) | def get_dt(self, pruned=True):
method dt (line 228) | def dt(self):
method cluster (line 231) | def cluster(self, n_clusters, load_clusters=False, save_dir="", visual...
function restore_gp_regressors (line 281) | def restore_gp_regressors(pre_trained_models):
function read_dataset (line 318) | def read_dataset(name, train_split, sim_options):
function world_to_body_velocity_mapping (line 341) | def world_to_body_velocity_mapping(state_sequence):
FILE: ros_dd_mpc/src/model_fitting/gp_fitting.py
function plot_gp_regression (line 34) | def plot_gp_regression(x_test, y_test, x_train, y_train, gp_mean, gp_std...
function gp_train_and_save (line 88) | def gp_train_and_save(x, y, gp_regressors, save_model, save_file, save_p...
function main (line 127) | def main(x_features, u_features, reg_y_dim, quad_sim_options, dataset_name,
function gp_evaluate_test_set (line 314) | def gp_evaluate_test_set(gp_regressors, gp_dataset, pruned=False, timed=...
FILE: ros_dd_mpc/src/model_fitting/gp_visualization.py
function gp_visualization_experiment (line 23) | def gp_visualization_experiment(quad_sim_options, dataset_name,
FILE: ros_dd_mpc/src/model_fitting/mlp_common.py
class GPToMLPDataset (line 9) | class GPToMLPDataset(Dataset):
method __init__ (line 10) | def __init__(self, gp_dataset, ground_effect=False):
method stats (line 28) | def stats(self):
method __len__ (line 36) | def __len__(self):
method __getitem__ (line 39) | def __getitem__(self, item):
class NormalizedMLP (line 58) | class NormalizedMLP(mc.TorchMLCasadiModule):
method __init__ (line 59) | def __init__(self, model, x_mean, x_std, y_mean, y_std):
method forward (line 69) | def forward(self, x):
method cs_forward (line 72) | def cs_forward(self, x):
class QuadResidualModel (line 76) | class QuadResidualModel(mc.TorchMLCasadiModule):
method __init__ (line 77) | def __init__(self, hidden_size, hidden_layers):
method forward (line 84) | def forward(self, x):
method cs_forward (line 95) | def cs_forward(self, x):
FILE: ros_dd_mpc/src/model_fitting/mlp_fitting.py
function main (line 35) | def main(x_features, u_features, reg_y_dims, model_ground_effect, quad_s...
FILE: ros_dd_mpc/src/model_fitting/mlp_quad_res_fitting.py
function main (line 35) | def main(x_features, u_features, reg_y_dims, quad_sim_options, dataset_n...
FILE: ros_dd_mpc/src/model_fitting/process_neurobem_dataset.py
function main (line 19) | def main(quad):
function quaternion_to_euler (line 83) | def quaternion_to_euler(q):
function val (line 92) | def val(quad):
function process_file (line 123) | def process_file(file_path, quad, rec_dict):
function consecutive_data_points (line 129) | def consecutive_data_points(data):
function f_rate (line 220) | def f_rate(x, u, quad):
function resimulate (line 236) | def resimulate(x_0, x_f, u, dt, quad_mpc, rec_dict):
FILE: ros_dd_mpc/src/model_fitting/rdrv_fitting.py
function linear_rdrv_fitting (line 27) | def linear_rdrv_fitting(x, y, feat_idx):
function load_rdrv (line 36) | def load_rdrv(model_options):
function main (line 42) | def main(model_name, features, quad_sim_options, dataset_name,
FILE: ros_dd_mpc/src/model_fitting/system_identification.py
function odometry_parse (line 16) | def odometry_parse(odom_msg):
function thrust_motor_model (line 26) | def thrust_motor_model(motor_tau, thrust, thrust_des, dt):
function system_identification (line 38) | def system_identification(quad_mpc, ds_name, sim_options):
FILE: ros_dd_mpc/src/quad_mpc/create_ros_dd_mpc.py
function custom_quad_param_loader (line 21) | def custom_quad_param_loader(quad_name):
class ROSDDMPC (line 70) | class ROSDDMPC:
method __init__ (line 71) | def __init__(self, t_horizon, n_mpc_nodes, opt_dt, quad_name, point_re...
method set_state (line 105) | def set_state(self, x):
method set_gp_state (line 113) | def set_gp_state(self, x):
method set_reference (line 124) | def set_reference(self, x_ref, u_ref):
method optimize (line 134) | def optimize(self, model_data):
FILE: ros_dd_mpc/src/quad_mpc/quad_3d.py
class Quadrotor3D (line 20) | class Quadrotor3D:
method __init__ (line 22) | def __init__(self, noisy=False, drag=False, payload=False, motor_noise...
method set_state (line 104) | def set_state(self, *args, **kwargs):
method set_gp_state (line 119) | def set_gp_state(self, *args, **kwargs):
method get_state (line 141) | def get_state(self, quaternion=False, stacked=False):
method get_gp_state (line 155) | def get_gp_state(self, quaternion=False, stacked=False):
method get_control (line 176) | def get_control(self, noisy=False):
method update (line 182) | def update(self, u, dt):
method f_pos (line 229) | def f_pos(self, x):
method f_att (line 239) | def f_att(self, x):
method f_vel (line 251) | def f_vel(self, x, u, f_d):
method f_rate (line 280) | def f_rate(self, x, u, t_d):
FILE: ros_dd_mpc/src/quad_mpc/quad_3d_mpc.py
class Quad3DMPC (line 24) | class Quad3DMPC:
method __init__ (line 25) | def __init__(self, my_quad, t_horizon=1.0, n_nodes=5, q_cost=None, r_c...
method clear (line 95) | def clear(self):
method get_state (line 98) | def get_state(self):
method set_reference (line 107) | def set_reference(self, x_reference, u_reference=None):
method optimize (line 123) | def optimize(self, use_model=0, return_x=False):
method simulate (line 142) | def simulate(self, ref_u):
method simulate_plant (line 152) | def simulate_plant(self, w_opt, t_horizon=None, dt_vec=None, progress_...
method forward_prop (line 172) | def forward_prop(self, x_0, w_opt, cov_0=None, t_horizon=None, dt=None...
method reshape_input_sequence (line 213) | def reshape_input_sequence(u_seq):
method reset (line 226) | def reset(self):
FILE: ros_dd_mpc/src/quad_mpc/quad_3d_optimizer.py
class Quad3DOptimizer (line 33) | class Quad3DOptimizer:
method __init__ (line 34) | def __init__(self, quad, t_horizon=1, n_nodes=20,
method clear_acados_model (line 228) | def clear_acados_model(self):
method add_missing_states (line 240) | def add_missing_states(self, gp_outs):
method remove_extra_states (line 263) | def remove_extra_states(self, vec):
method acados_setup_model (line 276) | def acados_setup_model(self, nominal, model_name):
method quad_dynamics (line 435) | def quad_dynamics(self, rdrv_d):
method p_dynamics (line 452) | def p_dynamics(self):
method q_dynamics (line 455) | def q_dynamics(self):
method v_dynamics (line 458) | def v_dynamics(self, rdrv_d):
method w_dynamics (line 478) | def w_dynamics(self):
method linearized_quad_dynamics (line 489) | def linearized_quad_dynamics(self):
method set_reference_state (line 523) | def set_reference_state(self, x_target=None, u_target=None):
method set_reference_trajectory (line 559) | def set_reference_trajectory(self, x_target, u_target):
method discretize_f_and_q (line 601) | def discretize_f_and_q(self, t_horizon, n, m=1, i=0, use_gp=True, use_...
method run_optimization (line 620) | def run_optimization(self, initial_state=None, use_model=0, return_x=F...
FILE: ros_dd_mpc/src/utils/animator.py
class Dynamic3DTrajectory (line 21) | class Dynamic3DTrajectory:
method __init__ (line 22) | def __init__(self, pos_data, vel_data, pos_ref, vel_ref, t_vec_ref, le...
method on_launch (line 93) | def on_launch(self):
method on_init (line 200) | def on_init(self):
method animate (line 219) | def animate(self, i):
method __call__ (line 261) | def __call__(self, save=False):
FILE: ros_dd_mpc/src/utils/ground_map.py
class GroundMap (line 4) | class GroundMap:
method __init__ (line 5) | def __init__(self, horizon=((-1, 1), (-1, 1)), resolution=0.1):
method at (line 11) | def at(self, center: np.array):
method draw (line 16) | def draw(self, pos, map):
method empty_map (line 20) | def empty_map(self):
class GroundMapWithBox (line 27) | class GroundMapWithBox(GroundMap):
method __init__ (line 28) | def __init__(self, box_min, box_max, height, *args, **kwargs):
method draw (line 34) | def draw(self, pos, map):
FILE: ros_dd_mpc/src/utils/keyframe_3d_gen.py
function apply_map_limits (line 22) | def apply_map_limits(x, y, z, limits):
function center_and_scale (line 47) | def center_and_scale(x, y, z, x_max, x_min, y_max, y_min, z_max, z_min):
function random_periodical_trajectory (line 61) | def random_periodical_trajectory(plot=False, random_state=None, map_limi...
FILE: ros_dd_mpc/src/utils/quad_3d_opt_utils.py
function discretize_dynamics_and_cost (line 20) | def discretize_dynamics_and_cost(t_horizon, n_points, m_steps_per_point,...
function _forward_prop_core (line 61) | def _forward_prop_core(x_0, u_seq, t_horizon, discrete_dynamics_f, dynam...
function uncertainty_forward_propagation (line 145) | def uncertainty_forward_propagation(x_0, u_seq, t_horizon, discrete_dyna...
function gp_prediction_jac (line 165) | def gp_prediction_jac(z, Bx, Bz, gp_ensemble, gp_idx):
function simulate_plant (line 204) | def simulate_plant(quad, w_opt, simulation_dt, simulate_func, t_horizon=...
function get_reference_chunk (line 267) | def get_reference_chunk(reference_traj, reference_u, current_idx, n_mpc_...
FILE: ros_dd_mpc/src/utils/trajectories.py
function check_trajectory (line 28) | def check_trajectory(trajectory, inputs, tvec, plot=False):
function minimum_snap_trajectory_generator (line 126) | def minimum_snap_trajectory_generator(traj_derivatives, yaw_derivatives,...
function load_map_limits_from_file (line 284) | def load_map_limits_from_file(map_limits):
function straight_trajectory (line 307) | def straight_trajectory(quad, discretization_dt, speed):
function flyover_trajectory_collect (line 324) | def flyover_trajectory_collect(quad, discretization_dt, seed, speed, fly...
function flyover_trajectory (line 381) | def flyover_trajectory(quad, discretization_dt, seed, speed, flyover_box...
function random_trajectory (line 436) | def random_trajectory(quad, discretization_dt, seed, speed, map_name=Non...
function loop_trajectory (line 469) | def loop_trajectory(quad, discretization_dt, radius, z, lin_acc, clockwi...
function lemniscate_trajectory (line 579) | def lemniscate_trajectory(quad, discretization_dt, radius, z, lin_acc, c...
FILE: ros_dd_mpc/src/utils/trajectory_generator.py
function draw_poly (line 5) | def draw_poly(traj, u_traj, t, target_points=None, target_t=None):
function get_full_traj (line 91) | def get_full_traj(poly_coeffs, target_dt, int_dt):
function fit_multi_segment_polynomial_trajectory (line 147) | def fit_multi_segment_polynomial_trajectory(p_targets, yaw_targets):
function matrix_generation (line 163) | def matrix_generation(ts):
function multiple_waypoints (line 176) | def multiple_waypoints(n_segments):
function fit_single_segment (line 216) | def fit_single_segment(p_start, p_end, v_start=None, v_end=None, a_start...
function rhs_generation (line 255) | def rhs_generation(x):
FILE: ros_dd_mpc/src/utils/utils.py
function safe_mkdir_recursive (line 34) | def safe_mkdir_recursive(directory, overwrite=False):
function safe_mknode_recursive (line 51) | def safe_mknode_recursive(destiny_dir, node_name, overwrite):
function jsonify (line 62) | def jsonify(array):
function undo_jsonify (line 70) | def undo_jsonify(array):
function get_data_dir_and_file (line 79) | def get_data_dir_and_file(ds_name, training_split, params, read_only=Fal...
function get_model_dir_and_file (line 176) | def get_model_dir_and_file(model_options):
function load_pickled_models (line 192) | def load_pickled_models(directory='', file_name='', model_options=None):
function interpol_mse (line 253) | def interpol_mse(t_1, x_1, t_2, x_2, n_interp_samples=1000):
function euclidean_dist (line 277) | def euclidean_dist(x, y, thresh=None):
function euler_to_quaternion (line 299) | def euler_to_quaternion(roll, pitch, yaw):
function quaternion_to_euler (line 308) | def quaternion_to_euler(q):
function unit_quat (line 314) | def unit_quat(q):
function v_dot_q (line 330) | def v_dot_q(v, q):
function q_to_rot_mat (line 338) | def q_to_rot_mat(q):
function q_dot_q (line 356) | def q_dot_q(q, r):
function rotation_matrix_to_quat (line 380) | def rotation_matrix_to_quat(rot):
function undo_quaternion_flip (line 392) | def undo_quaternion_flip(q_past, q_current):
function skew_symmetric (line 407) | def skew_symmetric(v):
function decompose_quaternion (line 428) | def decompose_quaternion(q):
function quaternion_inverse (line 447) | def quaternion_inverse(q):
function rotation_matrix_to_euler (line 456) | def rotation_matrix_to_euler(r_mat):
function prune_dataset (line 473) | def prune_dataset(x, y, x_cap, bins, thresh, plot, labels=None):
function distance_maximizing_points_1d (line 551) | def distance_maximizing_points_1d(points, n_train_points, dense_gp=None):
function distance_maximizing_points_2d (line 599) | def distance_maximizing_points_2d(points, n_train_points, dense_gp, plot...
function distance_maximizing_points (line 636) | def distance_maximizing_points(x_values, center, n_train_points=7, dense...
function sample_random_points (line 734) | def sample_random_points(points, used_idx, points_to_sample, dense_gp=No...
function parse_xacro_file (line 761) | def parse_xacro_file(xacro):
function make_bx_matrix (line 788) | def make_bx_matrix(x_dims, y_feats):
function make_bz_matrix (line 804) | def make_bz_matrix(x_dims, u_dims, x_feats, u_feats):
function quaternion_state_mse (line 826) | def quaternion_state_mse(x, x_ref, mask):
function separate_variables (line 843) | def separate_variables(traj):
FILE: ros_dd_mpc/src/utils/visualization.py
function angle_to_rot_mat (line 31) | def angle_to_rot_mat(angle):
function draw_arrow (line 43) | def draw_arrow(x_base, y_base, x_body, y_body):
function draw_drone (line 65) | def draw_drone(pos, q_rot, x_f, y_f):
function draw_covariance_ellipsoid (line 85) | def draw_covariance_ellipsoid(center, covar):
function visualize_data_distribution (line 112) | def visualize_data_distribution(x_data, y_data, clusters, x_pruned, y_pr...
function visualize_gp_inference (line 158) | def visualize_gp_inference(x_data, u_data, y_data, gp_ensemble, vis_feat...
function initialize_drone_plotter (line 303) | def initialize_drone_plotter(world_rad, quad_rad, n_props, full_traj=None):
function draw_drone_simulation (line 354) | def draw_drone_simulation(art_pack, x_trajectory, quad, targets, targets...
function trajectory_tracking_results (line 460) | def trajectory_tracking_results(t_ref, x_ref, x_executed, u_ref, u_execu...
function mse_tracking_experiment_plot (line 549) | def mse_tracking_experiment_plot(v_max, mse, traj_type_vec, train_sample...
function load_past_experiments (line 628) | def load_past_experiments():
function get_experiment_files (line 645) | def get_experiment_files():
Condensed preview — 51 files, each showing path, character count, and a content snippet. Download the .json file or copy for the full structured content (489K chars).
[
{
"path": ".gitignore",
"chars": 119,
"preview": "venv/*\ndata/\nresults/\n.idea/\n*.pyc\nros_gp_mpc/acados_models/*\nc_generated_code/\n*.egg-info/\ndist/\nbuild/\nacados_models/"
},
{
"path": ".gitmodules",
"chars": 86,
"preview": "[submodule \"ml-casadi\"]\n\tpath = ml-casadi\n\turl = https://github.com/TUM-AAS/ml-casadi\n"
},
{
"path": "LICENSE",
"chars": 35149,
"preview": " GNU GENERAL PUBLIC LICENSE\n Version 3, 29 June 2007\n\n Copyright (C) 2007 Free "
},
{
"path": "README.md",
"chars": 4072,
"preview": "# Real-Time Neural MPC\n\nThis repository contains the code for experiments associated to our paper \n\n```\nReal-time Neural"
},
{
"path": "requirements.txt",
"chars": 174,
"preview": "numpy==2.2\nscipy==1.15\ntqdm\nmatplotlib==3.9\nscikit-learn==1.6\ncasadi==3.6\npyquaternion\njoblib\npandas\nPyYAML\nrospkg>=1.3\n"
},
{
"path": "ros_dd_mpc/CMakeLists.txt",
"chars": 6951,
"preview": "cmake_minimum_required(VERSION 2.8.3)\nproject(ros_dd_mpc)\n\n## Compile as C++11, supported in ROS Kinetic and newer\n# add"
},
{
"path": "ros_dd_mpc/config/agisim_simulation_run.yaml",
"chars": 118,
"preview": "quad_name: 'kingfisher'\nworld_limits: None\nuse_ekf_synchronization: False\ncontrol_freq_factor: 5\nenvironment: 'agisim'"
},
{
"path": "ros_dd_mpc/config/arena_limits.yaml",
"chars": 68,
"preview": "x_min: -6.0\nx_max: 12.0\ny_min: -7.0\ny_max: 7.0\nz_min: 1.0\nz_max: 4.0"
},
{
"path": "ros_dd_mpc/config/arena_run.yaml",
"chars": 125,
"preview": "quad_name: 'kingfisher'\nworld_limits: arena_limits\nuse_ekf_synchronization: False\ncontrol_freq_factor: 5\nenvironment: 'a"
},
{
"path": "ros_dd_mpc/config/basic.world",
"chars": 1314,
"preview": "<?xml version=\"1.0\" ?>\n<sdf version=\"1.4\">\n <world name=\"default\">\n <include>\n <uri>model://ground_plane</uri>\n"
},
{
"path": "ros_dd_mpc/config/circle_and_lemniscate_options.yaml",
"chars": 462,
"preview": "# Parameters for the loop and lemniscate trajectories at successively increasing speed\nloop_z: 2.5 # Z posi"
},
{
"path": "ros_dd_mpc/config/configuration_parameters.py",
"chars": 3869,
"preview": "\"\"\" Set of tunable parameters for the Simplified Simulator and model fitting.\n\nThis program is free software: you can re"
},
{
"path": "ros_dd_mpc/config/flyingroom_limits.yaml",
"chars": 67,
"preview": "x_min: -0.4\nx_max: 4.3\ny_min: -2.4\ny_max: 2.3\nz_min: 0.5\nz_max: 2.0"
},
{
"path": "ros_dd_mpc/config/ground_effect_limits.yaml",
"chars": 80,
"preview": "x_min: -4.25\nx_max: -2.76\ny_min: 9.37\ny_max: 10.13\nz_min: 0.8 # 0.95\nz_max: 0.85"
},
{
"path": "ros_dd_mpc/config/kingfisher.yaml",
"chars": 1021,
"preview": "mass: 0.752 # [kg]\ntbm_fr: [ 0.075, -0.10, 0.0] # [m]\ntbm_bl: "
},
{
"path": "ros_dd_mpc/config/simulation_run.yaml",
"chars": 118,
"preview": "quad_name: 'kingfisher'\nworld_limits: None\nuse_ekf_synchronization: False\ncontrol_freq_factor: 5\nenvironment: 'gazebo'"
},
{
"path": "ros_dd_mpc/launch/dd_mpc_wrapper.launch",
"chars": 9285,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n <!-- true if running the nodes in the gazebo simulator environment. false if running "
},
{
"path": "ros_dd_mpc/msg/ReferenceTrajectory.msg",
"chars": 98,
"preview": "int32 seq_len\nstring traj_name\nfloat64 v_input\nfloat64[] trajectory\nfloat64[] dt\nfloat64[] inputs\n"
},
{
"path": "ros_dd_mpc/nodes/dd_mpc_node.py",
"chars": 39770,
"preview": "#!/usr/bin/env python3.6\n\"\"\" ROS node for the data-augmented MPC, to use in the Gazebo simulator and real world experime"
},
{
"path": "ros_dd_mpc/nodes/reference_publisher_node.py",
"chars": 9538,
"preview": "#!/usr/bin/env python3.6\n\"\"\" Node wrapper for publishing trajectories for the MPC pipeline to track.\n\nThis program is fr"
},
{
"path": "ros_dd_mpc/package.xml",
"chars": 2704,
"preview": "<?xml version=\"1.0\"?>\n<package format=\"2\">\n <name>ros_dd_mpc</name>\n <version>0.0.0</version>\n <description>The ros_d"
},
{
"path": "ros_dd_mpc/src/__init__.py",
"chars": 0,
"preview": ""
},
{
"path": "ros_dd_mpc/src/experiments/__init__.py",
"chars": 0,
"preview": ""
},
{
"path": "ros_dd_mpc/src/experiments/comparative_experiment.py",
"chars": 15893,
"preview": "\"\"\" Runs the experimental setup to compare different data-learned models for the MPC on the Simplified Simulator.\n\nThis "
},
{
"path": "ros_dd_mpc/src/experiments/point_tracking_and_record.py",
"chars": 15214,
"preview": "\"\"\" Executes aggressive maneuvers for collecting flight data on the Simplified Simulator to later train models on.\n\nThis"
},
{
"path": "ros_dd_mpc/src/experiments/trajectory_test.py",
"chars": 8936,
"preview": "\"\"\" Tracks a specified trajectory on the simplified simulator using the data-augmented MPC.\n\nThis program is free softwa"
},
{
"path": "ros_dd_mpc/src/model_fitting/__init__.py",
"chars": 0,
"preview": ""
},
{
"path": "ros_dd_mpc/src/model_fitting/gp.py",
"chars": 29527,
"preview": "\"\"\" Gaussian Process custom implementation for the data-augmented MPC.\n\nThis program is free software: you can redistrib"
},
{
"path": "ros_dd_mpc/src/model_fitting/gp_common.py",
"chars": 12746,
"preview": "\"\"\" Contains a set of utility functions and classes for the GP training and inference.\n\nThis program is free software: y"
},
{
"path": "ros_dd_mpc/src/model_fitting/gp_fitting.py",
"chars": 18981,
"preview": "\"\"\" Executable script to train a custom Gaussian Process on recorded flight data.\n\nThis program is free software: you ca"
},
{
"path": "ros_dd_mpc/src/model_fitting/gp_visualization.py",
"chars": 6863,
"preview": "\"\"\" Executable script for visual evaluation of the trained GP quality.\n\nThis program is free software: you can redistrib"
},
{
"path": "ros_dd_mpc/src/model_fitting/mlp_common.py",
"chars": 4174,
"preview": "import numpy as np\nfrom torch.utils.data import Dataset\n\nimport ml_casadi.torch as mc\nfrom config.configuration_paramete"
},
{
"path": "ros_dd_mpc/src/model_fitting/mlp_fitting.py",
"chars": 9628,
"preview": "\"\"\" Executable script to train a custom Gaussian Process on recorded flight data.\n\nThis program is free software: you ca"
},
{
"path": "ros_dd_mpc/src/model_fitting/mlp_quad_res_fitting.py",
"chars": 8716,
"preview": "\"\"\" Executable script to train a custom Gaussian Process on recorded flight data.\n\nThis program is free software: you ca"
},
{
"path": "ros_dd_mpc/src/model_fitting/process_neurobem_dataset.py",
"chars": 11453,
"preview": "import argparse\nimport os\nimport random\n\nimport numpy as np\nimport pandas as pd\nfrom scipy.signal import savgol_filter\nf"
},
{
"path": "ros_dd_mpc/src/model_fitting/rdrv_fitting.py",
"chars": 5447,
"preview": "\"\"\" Implementation and fitting of the RDRv linear regression model on flight data.\n\nThis program is free software: you c"
},
{
"path": "ros_dd_mpc/src/model_fitting/system_identification.py",
"chars": 10806,
"preview": "import argparse\nimport os\n\nimport numpy as np\nimport pandas as pd\nimport rosbag\nimport rospy\n\nfrom config.configuration_"
},
{
"path": "ros_dd_mpc/src/quad_mpc/__init__.py",
"chars": 0,
"preview": ""
},
{
"path": "ros_dd_mpc/src/quad_mpc/create_ros_dd_mpc.py",
"chars": 5530,
"preview": "\"\"\" Class for interfacing the data-augmented MPC with ROS.\n\nThis program is free software: you can redistribute it and/o"
},
{
"path": "ros_dd_mpc/src/quad_mpc/quad_3d.py",
"chars": 12128,
"preview": "\"\"\" Implementation of the Simplified Simulator and its quadrotor dynamics.\n\nThis program is free software: you can redis"
},
{
"path": "ros_dd_mpc/src/quad_mpc/quad_3d_mpc.py",
"chars": 11455,
"preview": "\"\"\" Implementation of the data-augmented MPC for quadrotors.\n\nThis program is free software: you can redistribute it and"
},
{
"path": "ros_dd_mpc/src/quad_mpc/quad_3d_optimizer.py",
"chars": 33914,
"preview": "\"\"\" Implementation of the nonlinear optimizer for the data-augmented MPC.\n\nThis program is free software: you can redist"
},
{
"path": "ros_dd_mpc/src/utils/__init__.py",
"chars": 0,
"preview": ""
},
{
"path": "ros_dd_mpc/src/utils/animator.py",
"chars": 11970,
"preview": "\"\"\" Class for generating a comprehensive post-processed visualization of experimental flight results.\n\nThis program is f"
},
{
"path": "ros_dd_mpc/src/utils/ground_map.py",
"chars": 1960,
"preview": "import numpy as np\n\n\nclass GroundMap:\n def __init__(self, horizon=((-1, 1), (-1, 1)), resolution=0.1):\n assert"
},
{
"path": "ros_dd_mpc/src/utils/keyframe_3d_gen.py",
"chars": 6980,
"preview": "\"\"\" Generates a set of keypoints to generate a piece-wise polynomial trajectory between each pair.\n\nThis program is free"
},
{
"path": "ros_dd_mpc/src/utils/quad_3d_opt_utils.py",
"chars": 13824,
"preview": "\"\"\" Set of utility functions for the quadrotor optimizer and simplified simulator.\n\nThis program is free software: you c"
},
{
"path": "ros_dd_mpc/src/utils/trajectories.py",
"chars": 29915,
"preview": "\"\"\" Trajectory generation functions. For the circle, lemniscate and random trajectories.\n\nThis program is free software:"
},
{
"path": "ros_dd_mpc/src/utils/trajectory_generator.py",
"chars": 9739,
"preview": "import numpy as np\nimport matplotlib.pyplot as plt\n\n\ndef draw_poly(traj, u_traj, t, target_points=None, target_t=None):\n"
},
{
"path": "ros_dd_mpc/src/utils/utils.py",
"chars": 31827,
"preview": "\"\"\" Miscellaneous utility functions.\n\nThis program is free software: you can redistribute it and/or modify it under\nthe "
},
{
"path": "ros_dd_mpc/src/utils/visualization.py",
"chars": 28571,
"preview": "\"\"\" Miscellaneous visualization functions.\n\nThis program is free software: you can redistribute it and/or modify it unde"
}
]
About this extraction
This page contains the full source code of the TUM-AAS/neural-mpc GitHub repository, extracted and formatted as plain text for AI agents and large language models (LLMs). The extraction includes 51 files (460.3 KB), approximately 122.5k tokens, and a symbol index with 260 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.