├── .gitignore ├── LICENSE ├── README.md ├── example ├── optimization.py └── plotting.py ├── quads └── quad.yaml ├── requirements.txt ├── src ├── __ini__.py ├── integrator.py ├── planner.py ├── plot.py ├── progress.py ├── quad.py ├── quaternion.py ├── track.py ├── trajectory.py ├── trajectory_conversion.py └── utils.py └── tracks └── track.yaml /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | pip-wheel-metadata/ 24 | share/python-wheels/ 25 | *.egg-info/ 26 | .installed.cfg 27 | *.egg 28 | MANIFEST 29 | 30 | # PyInstaller 31 | # Usually these files are written by a python script from a template 32 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 33 | *.manifest 34 | *.spec 35 | 36 | # Installer logs 37 | pip-log.txt 38 | pip-delete-this-directory.txt 39 | 40 | # Unit test / coverage reports 41 | htmlcov/ 42 | .tox/ 43 | .nox/ 44 | .coverage 45 | .coverage.* 46 | .cache 47 | nosetests.xml 48 | coverage.xml 49 | *.cover 50 | *.py,cover 51 | .hypothesis/ 52 | .pytest_cache/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | target/ 76 | 77 | # Jupyter Notebook 78 | .ipynb_checkpoints 79 | 80 | # IPython 81 | profile_default/ 82 | ipython_config.py 83 | 84 | # pyenv 85 | .python-version 86 | 87 | # pipenv 88 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 89 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 90 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 91 | # install all needed dependencies. 92 | #Pipfile.lock 93 | 94 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 95 | __pypackages__/ 96 | 97 | # Celery stuff 98 | celerybeat-schedule 99 | celerybeat.pid 100 | 101 | # SageMath parsed files 102 | *.sage.py 103 | 104 | # Environments 105 | .env 106 | .venv 107 | env/ 108 | venv/ 109 | ENV/ 110 | env.bak/ 111 | venv.bak/ 112 | 113 | # Spyder project settings 114 | .spyderproject 115 | .spyproject 116 | 117 | # Rope project settings 118 | .ropeproject 119 | 120 | # mkdocs documentation 121 | /site 122 | 123 | # mypy 124 | .mypy_cache/ 125 | .dmypy.json 126 | dmypy.json 127 | 128 | # Pyre type checker 129 | .pyre/ 130 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | Preamble 9 | 10 | The GNU General Public License is a free, copyleft license for 11 | software and other kinds of works. 12 | 13 | The licenses for most software and other practical works are designed 14 | to take away your freedom to share and change the works. By contrast, 15 | the GNU General Public License is intended to guarantee your freedom to 16 | share and change all versions of a program--to make sure it remains free 17 | software for all its users. We, the Free Software Foundation, use the 18 | GNU General Public License for most of our software; it applies also to 19 | any other work released this way by its authors. You can apply it to 20 | your programs, too. 21 | 22 | When we speak of free software, we are referring to freedom, not 23 | price. Our General Public Licenses are designed to make sure that you 24 | have the freedom to distribute copies of free software (and charge for 25 | them if you wish), that you receive source code or can get it if you 26 | want it, that you can change the software or use pieces of it in new 27 | free programs, and that you know you can do these things. 28 | 29 | To protect your rights, we need to prevent others from denying you 30 | these rights or asking you to surrender the rights. Therefore, you have 31 | certain responsibilities if you distribute copies of the software, or if 32 | you modify it: responsibilities to respect the freedom of others. 33 | 34 | For example, if you distribute copies of such a program, whether 35 | gratis or for a fee, you must pass on to the recipients the same 36 | freedoms that you received. You must make sure that they, too, receive 37 | or can get the source code. And you must show them these terms so they 38 | know their rights. 39 | 40 | Developers that use the GNU GPL protect your rights with two steps: 41 | (1) assert copyright on the software, and (2) offer you this License 42 | giving you legal permission to copy, distribute and/or modify it. 43 | 44 | For the developers' and authors' protection, the GPL clearly explains 45 | that there is no warranty for this free software. For both users' and 46 | authors' sake, the GPL requires that modified versions be marked as 47 | changed, so that their problems will not be attributed erroneously to 48 | authors of previous versions. 49 | 50 | Some devices are designed to deny users access to install or run 51 | modified versions of the software inside them, although the manufacturer 52 | can do so. This is fundamentally incompatible with the aim of 53 | protecting users' freedom to change the software. The systematic 54 | pattern of such abuse occurs in the area of products for individuals to 55 | use, which is precisely where it is most unacceptable. Therefore, we 56 | have designed this version of the GPL to prohibit the practice for those 57 | products. If such problems arise substantially in other domains, we 58 | stand ready to extend this provision to those domains in future versions 59 | of the GPL, as needed to protect the freedom of users. 60 | 61 | Finally, every program is threatened constantly by software patents. 62 | States should not allow patents to restrict development and use of 63 | software on general-purpose computers, but in those that do, we wish to 64 | avoid the special danger that patents applied to a free program could 65 | make it effectively proprietary. To prevent this, the GPL assures that 66 | patents cannot be used to render the program non-free. 67 | 68 | The precise terms and conditions for copying, distribution and 69 | modification follow. 70 | 71 | TERMS AND CONDITIONS 72 | 73 | 0. Definitions. 74 | 75 | "This License" refers to version 3 of the GNU General Public License. 76 | 77 | "Copyright" also means copyright-like laws that apply to other kinds of 78 | works, such as semiconductor masks. 79 | 80 | "The Program" refers to any copyrightable work licensed under this 81 | License. Each licensee is addressed as "you". "Licensees" and 82 | "recipients" may be individuals or organizations. 83 | 84 | To "modify" a work means to copy from or adapt all or part of the work 85 | in a fashion requiring copyright permission, other than the making of an 86 | exact copy. The resulting work is called a "modified version" of the 87 | earlier work or a work "based on" the earlier work. 88 | 89 | A "covered work" means either the unmodified Program or a work based 90 | on the Program. 91 | 92 | To "propagate" a work means to do anything with it that, without 93 | permission, would make you directly or secondarily liable for 94 | infringement under applicable copyright law, except executing it on a 95 | computer or modifying a private copy. Propagation includes copying, 96 | distribution (with or without modification), making available to the 97 | public, and in some countries other activities as well. 98 | 99 | To "convey" a work means any kind of propagation that enables other 100 | parties to make or receive copies. Mere interaction with a user through 101 | a computer network, with no transfer of a copy, is not conveying. 102 | 103 | An interactive user interface displays "Appropriate Legal Notices" 104 | to the extent that it includes a convenient and prominently visible 105 | feature that (1) displays an appropriate copyright notice, and (2) 106 | tells the user that there is no warranty for the work (except to the 107 | extent that warranties are provided), that licensees may convey the 108 | work under this License, and how to view a copy of this License. If 109 | the interface presents a list of user commands or options, such as a 110 | menu, a prominent item in the list meets this criterion. 111 | 112 | 1. Source Code. 113 | 114 | The "source code" for a work means the preferred form of the work 115 | for making modifications to it. "Object code" means any non-source 116 | form of a work. 117 | 118 | A "Standard Interface" means an interface that either is an official 119 | standard defined by a recognized standards body, or, in the case of 120 | interfaces specified for a particular programming language, one that 121 | is widely used among developers working in that language. 122 | 123 | The "System Libraries" of an executable work include anything, other 124 | than the work as a whole, that (a) is included in the normal form of 125 | packaging a Major Component, but which is not part of that Major 126 | Component, and (b) serves only to enable use of the work with that 127 | Major Component, or to implement a Standard Interface for which an 128 | implementation is available to the public in source code form. A 129 | "Major Component", in this context, means a major essential component 130 | (kernel, window system, and so on) of the specific operating system 131 | (if any) on which the executable work runs, or a compiler used to 132 | produce the work, or an object code interpreter used to run it. 133 | 134 | The "Corresponding Source" for a work in object code form means all 135 | the source code needed to generate, install, and (for an executable 136 | work) run the object code and to modify the work, including scripts to 137 | control those activities. However, it does not include the work's 138 | System Libraries, or general-purpose tools or generally available free 139 | programs which are used unmodified in performing those activities but 140 | which are not part of the work. For example, Corresponding Source 141 | includes interface definition files associated with source files for 142 | the work, and the source code for shared libraries and dynamically 143 | linked subprograms that the work is specifically designed to require, 144 | such as by intimate data communication or control flow between those 145 | subprograms and other parts of the work. 146 | 147 | The Corresponding Source need not include anything that users 148 | can regenerate automatically from other parts of the Corresponding 149 | Source. 150 | 151 | The Corresponding Source for a work in source code form is that 152 | same work. 153 | 154 | 2. Basic Permissions. 155 | 156 | All rights granted under this License are granted for the term of 157 | copyright on the Program, and are irrevocable provided the stated 158 | conditions are met. This License explicitly affirms your unlimited 159 | permission to run the unmodified Program. The output from running a 160 | covered work is covered by this License only if the output, given its 161 | content, constitutes a covered work. This License acknowledges your 162 | rights of fair use or other equivalent, as provided by copyright law. 163 | 164 | You may make, run and propagate covered works that you do not 165 | convey, without conditions so long as your license otherwise remains 166 | in force. You may convey covered works to others for the sole purpose 167 | of having them make modifications exclusively for you, or provide you 168 | with facilities for running those works, provided that you comply with 169 | the terms of this License in conveying all material for which you do 170 | not control copyright. Those thus making or running the covered works 171 | for you must do so exclusively on your behalf, under your direction 172 | and control, on terms that prohibit them from making any copies of 173 | your copyrighted material outside their relationship with you. 174 | 175 | Conveying under any other circumstances is permitted solely under 176 | the conditions stated below. Sublicensing is not allowed; section 10 177 | makes it unnecessary. 178 | 179 | 3. Protecting Users' Legal Rights From Anti-Circumvention Law. 180 | 181 | No covered work shall be deemed part of an effective technological 182 | measure under any applicable law fulfilling obligations under article 183 | 11 of the WIPO copyright treaty adopted on 20 December 1996, or 184 | similar laws prohibiting or restricting circumvention of such 185 | measures. 186 | 187 | When you convey a covered work, you waive any legal power to forbid 188 | circumvention of technological measures to the extent such circumvention 189 | is effected by exercising rights under this License with respect to 190 | the covered work, and you disclaim any intention to limit operation or 191 | modification of the work as a means of enforcing, against the work's 192 | users, your or third parties' legal rights to forbid circumvention of 193 | technological measures. 194 | 195 | 4. Conveying Verbatim Copies. 196 | 197 | You may convey verbatim copies of the Program's source code as you 198 | receive it, in any medium, provided that you conspicuously and 199 | appropriately publish on each copy an appropriate copyright notice; 200 | keep intact all notices stating that this License and any 201 | non-permissive terms added in accord with section 7 apply to the code; 202 | keep intact all notices of the absence of any warranty; and give all 203 | recipients a copy of this License along with the Program. 204 | 205 | You may charge any price or no price for each copy that you convey, 206 | and you may offer support or warranty protection for a fee. 207 | 208 | 5. Conveying Modified Source Versions. 209 | 210 | You may convey a work based on the Program, or the modifications to 211 | produce it from the Program, in the form of source code under the 212 | terms of section 4, provided that you also meet all of these conditions: 213 | 214 | a) The work must carry prominent notices stating that you modified 215 | it, and giving a relevant date. 216 | 217 | b) The work must carry prominent notices stating that it is 218 | released under this License and any conditions added under section 219 | 7. This requirement modifies the requirement in section 4 to 220 | "keep intact all notices". 221 | 222 | c) You must license the entire work, as a whole, under this 223 | License to anyone who comes into possession of a copy. This 224 | License will therefore apply, along with any applicable section 7 225 | additional terms, to the whole of the work, and all its parts, 226 | regardless of how they are packaged. This License gives no 227 | permission to license the work in any other way, but it does not 228 | invalidate such permission if you have separately received it. 229 | 230 | d) If the work has interactive user interfaces, each must display 231 | Appropriate Legal Notices; however, if the Program has interactive 232 | interfaces that do not display Appropriate Legal Notices, your 233 | work need not make them do so. 234 | 235 | A compilation of a covered work with other separate and independent 236 | works, which are not by their nature extensions of the covered work, 237 | and which are not combined with it such as to form a larger program, 238 | in or on a volume of a storage or distribution medium, is called an 239 | "aggregate" if the compilation and its resulting copyright are not 240 | used to limit the access or legal rights of the compilation's users 241 | beyond what the individual works permit. Inclusion of a covered work 242 | in an aggregate does not cause this License to apply to the other 243 | parts of the aggregate. 244 | 245 | 6. Conveying Non-Source Forms. 246 | 247 | You may convey a covered work in object code form under the terms 248 | of sections 4 and 5, provided that you also convey the 249 | machine-readable Corresponding Source under the terms of this License, 250 | in one of these ways: 251 | 252 | a) Convey the object code in, or embodied in, a physical product 253 | (including a physical distribution medium), accompanied by the 254 | Corresponding Source fixed on a durable physical medium 255 | customarily used for software interchange. 256 | 257 | b) Convey the object code in, or embodied in, a physical product 258 | (including a physical distribution medium), accompanied by a 259 | written offer, valid for at least three years and valid for as 260 | long as you offer spare parts or customer support for that product 261 | model, to give anyone who possesses the object code either (1) a 262 | copy of the Corresponding Source for all the software in the 263 | product that is covered by this License, on a durable physical 264 | medium customarily used for software interchange, for a price no 265 | more than your reasonable cost of physically performing this 266 | conveying of source, or (2) access to copy the 267 | Corresponding Source from a network server at no charge. 268 | 269 | c) Convey individual copies of the object code with a copy of the 270 | written offer to provide the Corresponding Source. This 271 | alternative is allowed only occasionally and noncommercially, and 272 | only if you received the object code with such an offer, in accord 273 | with subsection 6b. 274 | 275 | d) Convey the object code by offering access from a designated 276 | place (gratis or for a charge), and offer equivalent access to the 277 | Corresponding Source in the same way through the same place at no 278 | further charge. You need not require recipients to copy the 279 | Corresponding Source along with the object code. If the place to 280 | copy the object code is a network server, the Corresponding Source 281 | may be on a different server (operated by you or a third party) 282 | that supports equivalent copying facilities, provided you maintain 283 | clear directions next to the object code saying where to find the 284 | Corresponding Source. Regardless of what server hosts the 285 | Corresponding Source, you remain obligated to ensure that it is 286 | available for as long as needed to satisfy these requirements. 287 | 288 | e) Convey the object code using peer-to-peer transmission, provided 289 | you inform other peers where the object code and Corresponding 290 | Source of the work are being offered to the general public at no 291 | charge under subsection 6d. 292 | 293 | A separable portion of the object code, whose source code is excluded 294 | from the Corresponding Source as a System Library, need not be 295 | included in conveying the object code work. 296 | 297 | A "User Product" is either (1) a "consumer product", which means any 298 | tangible personal property which is normally used for personal, family, 299 | or household purposes, or (2) anything designed or sold for incorporation 300 | into a dwelling. In determining whether a product is a consumer product, 301 | doubtful cases shall be resolved in favor of coverage. For a particular 302 | product received by a particular user, "normally used" refers to a 303 | typical or common use of that class of product, regardless of the status 304 | of the particular user or of the way in which the particular user 305 | actually uses, or expects or is expected to use, the product. A product 306 | is a consumer product regardless of whether the product has substantial 307 | commercial, industrial or non-consumer uses, unless such uses represent 308 | the only significant mode of use of the product. 309 | 310 | "Installation Information" for a User Product means any methods, 311 | procedures, authorization keys, or other information required to install 312 | and execute modified versions of a covered work in that User Product from 313 | a modified version of its Corresponding Source. The information must 314 | suffice to ensure that the continued functioning of the modified object 315 | code is in no case prevented or interfered with solely because 316 | modification has been made. 317 | 318 | If you convey an object code work under this section in, or with, or 319 | specifically for use in, a User Product, and the conveying occurs as 320 | part of a transaction in which the right of possession and use of the 321 | User Product is transferred to the recipient in perpetuity or for a 322 | fixed term (regardless of how the transaction is characterized), the 323 | Corresponding Source conveyed under this section must be accompanied 324 | by the Installation Information. But this requirement does not apply 325 | if neither you nor any third party retains the ability to install 326 | modified object code on the User Product (for example, the work has 327 | been installed in ROM). 328 | 329 | The requirement to provide Installation Information does not include a 330 | requirement to continue to provide support service, warranty, or updates 331 | for a work that has been modified or installed by the recipient, or for 332 | the User Product in which it has been modified or installed. Access to a 333 | network may be denied when the modification itself materially and 334 | adversely affects the operation of the network or violates the rules and 335 | protocols for communication across the network. 336 | 337 | Corresponding Source conveyed, and Installation Information provided, 338 | in accord with this section must be in a format that is publicly 339 | documented (and with an implementation available to the public in 340 | source code form), and must require no special password or key for 341 | unpacking, reading or copying. 342 | 343 | 7. Additional Terms. 344 | 345 | "Additional permissions" are terms that supplement the terms of this 346 | License by making exceptions from one or more of its conditions. 347 | Additional permissions that are applicable to the entire Program shall 348 | be treated as though they were included in this License, to the extent 349 | that they are valid under applicable law. If additional permissions 350 | apply only to part of the Program, that part may be used separately 351 | under those permissions, but the entire Program remains governed by 352 | this License without regard to the additional permissions. 353 | 354 | When you convey a copy of a covered work, you may at your option 355 | remove any additional permissions from that copy, or from any part of 356 | it. (Additional permissions may be written to require their own 357 | removal in certain cases when you modify the work.) You may place 358 | additional permissions on material, added by you to a covered work, 359 | for which you have or can give appropriate copyright permission. 360 | 361 | Notwithstanding any other provision of this License, for material you 362 | add to a covered work, you may (if authorized by the copyright holders of 363 | that material) supplement the terms of this License with terms: 364 | 365 | a) Disclaiming warranty or limiting liability differently from the 366 | terms of sections 15 and 16 of this License; or 367 | 368 | b) Requiring preservation of specified reasonable legal notices or 369 | author attributions in that material or in the Appropriate Legal 370 | Notices displayed by works containing it; or 371 | 372 | c) Prohibiting misrepresentation of the origin of that material, or 373 | requiring that modified versions of such material be marked in 374 | reasonable ways as different from the original version; or 375 | 376 | d) Limiting the use for publicity purposes of names of licensors or 377 | authors of the material; or 378 | 379 | e) Declining to grant rights under trademark law for use of some 380 | trade names, trademarks, or service marks; or 381 | 382 | f) Requiring indemnification of licensors and authors of that 383 | material by anyone who conveys the material (or modified versions of 384 | it) with contractual assumptions of liability to the recipient, for 385 | any liability that these contractual assumptions directly impose on 386 | those licensors and authors. 387 | 388 | All other non-permissive additional terms are considered "further 389 | restrictions" within the meaning of section 10. If the Program as you 390 | received it, or any part of it, contains a notice stating that it is 391 | governed by this License along with a term that is a further 392 | restriction, you may remove that term. If a license document contains 393 | a further restriction but permits relicensing or conveying under this 394 | License, you may add to a covered work material governed by the terms 395 | of that license document, provided that the further restriction does 396 | not survive such relicensing or conveying. 397 | 398 | If you add terms to a covered work in accord with this section, you 399 | must place, in the relevant source files, a statement of the 400 | additional terms that apply to those files, or a notice indicating 401 | where to find the applicable terms. 402 | 403 | Additional terms, permissive or non-permissive, may be stated in the 404 | form of a separately written license, or stated as exceptions; 405 | the above requirements apply either way. 406 | 407 | 8. Termination. 408 | 409 | You may not propagate or modify a covered work except as expressly 410 | provided under this License. Any attempt otherwise to propagate or 411 | modify it is void, and will automatically terminate your rights under 412 | this License (including any patent licenses granted under the third 413 | paragraph of section 11). 414 | 415 | However, if you cease all violation of this License, then your 416 | license from a particular copyright holder is reinstated (a) 417 | provisionally, unless and until the copyright holder explicitly and 418 | finally terminates your license, and (b) permanently, if the copyright 419 | holder fails to notify you of the violation by some reasonable means 420 | prior to 60 days after the cessation. 421 | 422 | Moreover, your license from a particular copyright holder is 423 | reinstated permanently if the copyright holder notifies you of the 424 | violation by some reasonable means, this is the first time you have 425 | received notice of violation of this License (for any work) from that 426 | copyright holder, and you cure the violation prior to 30 days after 427 | your receipt of the notice. 428 | 429 | Termination of your rights under this section does not terminate the 430 | licenses of parties who have received copies or rights from you under 431 | this License. If your rights have been terminated and not permanently 432 | reinstated, you do not qualify to receive new licenses for the same 433 | material under section 10. 434 | 435 | 9. Acceptance Not Required for Having Copies. 436 | 437 | You are not required to accept this License in order to receive or 438 | run a copy of the Program. Ancillary propagation of a covered work 439 | occurring solely as a consequence of using peer-to-peer transmission 440 | to receive a copy likewise does not require acceptance. However, 441 | nothing other than this License grants you permission to propagate or 442 | modify any covered work. These actions infringe copyright if you do 443 | not accept this License. Therefore, by modifying or propagating a 444 | covered work, you indicate your acceptance of this License to do so. 445 | 446 | 10. Automatic Licensing of Downstream Recipients. 447 | 448 | Each time you convey a covered work, the recipient automatically 449 | receives a license from the original licensors, to run, modify and 450 | propagate that work, subject to this License. You are not responsible 451 | for enforcing compliance by third parties with this License. 452 | 453 | An "entity transaction" is a transaction transferring control of an 454 | organization, or substantially all assets of one, or subdividing an 455 | organization, or merging organizations. If propagation of a covered 456 | work results from an entity transaction, each party to that 457 | transaction who receives a copy of the work also receives whatever 458 | licenses to the work the party's predecessor in interest had or could 459 | give under the previous paragraph, plus a right to possession of the 460 | Corresponding Source of the work from the predecessor in interest, if 461 | the predecessor has it or can get it with reasonable efforts. 462 | 463 | You may not impose any further restrictions on the exercise of the 464 | rights granted or affirmed under this License. For example, you may 465 | not impose a license fee, royalty, or other charge for exercise of 466 | rights granted under this License, and you may not initiate litigation 467 | (including a cross-claim or counterclaim in a lawsuit) alleging that 468 | any patent claim is infringed by making, using, selling, offering for 469 | sale, or importing the Program or any portion of it. 470 | 471 | 11. Patents. 472 | 473 | A "contributor" is a copyright holder who authorizes use under this 474 | License of the Program or a work on which the Program is based. The 475 | work thus licensed is called the contributor's "contributor version". 476 | 477 | A contributor's "essential patent claims" are all patent claims 478 | owned or controlled by the contributor, whether already acquired or 479 | hereafter acquired, that would be infringed by some manner, permitted 480 | by this License, of making, using, or selling its contributor version, 481 | but do not include claims that would be infringed only as a 482 | consequence of further modification of the contributor version. For 483 | purposes of this definition, "control" includes the right to grant 484 | patent sublicenses in a manner consistent with the requirements of 485 | this License. 486 | 487 | Each contributor grants you a non-exclusive, worldwide, royalty-free 488 | patent license under the contributor's essential patent claims, to 489 | make, use, sell, offer for sale, import and otherwise run, modify and 490 | propagate the contents of its contributor version. 491 | 492 | In the following three paragraphs, a "patent license" is any express 493 | agreement or commitment, however denominated, not to enforce a patent 494 | (such as an express permission to practice a patent or covenant not to 495 | sue for patent infringement). To "grant" such a patent license to a 496 | party means to make such an agreement or commitment not to enforce a 497 | patent against the party. 498 | 499 | If you convey a covered work, knowingly relying on a patent license, 500 | and the Corresponding Source of the work is not available for anyone 501 | to copy, free of charge and under the terms of this License, through a 502 | publicly available network server or other readily accessible means, 503 | then you must either (1) cause the Corresponding Source to be so 504 | available, or (2) arrange to deprive yourself of the benefit of the 505 | patent license for this particular work, or (3) arrange, in a manner 506 | consistent with the requirements of this License, to extend the patent 507 | license to downstream recipients. "Knowingly relying" means you have 508 | actual knowledge that, but for the patent license, your conveying the 509 | covered work in a country, or your recipient's use of the covered work 510 | in a country, would infringe one or more identifiable patents in that 511 | country that you have reason to believe are valid. 512 | 513 | If, pursuant to or in connection with a single transaction or 514 | arrangement, you convey, or propagate by procuring conveyance of, a 515 | covered work, and grant a patent license to some of the parties 516 | receiving the covered work authorizing them to use, propagate, modify 517 | or convey a specific copy of the covered work, then the patent license 518 | you grant is automatically extended to all recipients of the covered 519 | work and works based on it. 520 | 521 | A patent license is "discriminatory" if it does not include within 522 | the scope of its coverage, prohibits the exercise of, or is 523 | conditioned on the non-exercise of one or more of the rights that are 524 | specifically granted under this License. You may not convey a covered 525 | work if you are a party to an arrangement with a third party that is 526 | in the business of distributing software, under which you make payment 527 | to the third party based on the extent of your activity of conveying 528 | the work, and under which the third party grants, to any of the 529 | parties who would receive the covered work from you, a discriminatory 530 | patent license (a) in connection with copies of the covered work 531 | conveyed by you (or copies made from those copies), or (b) primarily 532 | for and in connection with specific products or compilations that 533 | contain the covered work, unless you entered into that arrangement, 534 | or that patent license was granted, prior to 28 March 2007. 535 | 536 | Nothing in this License shall be construed as excluding or limiting 537 | any implied license or other defenses to infringement that may 538 | otherwise be available to you under applicable patent law. 539 | 540 | 12. No Surrender of Others' Freedom. 541 | 542 | If conditions are imposed on you (whether by court order, agreement or 543 | otherwise) that contradict the conditions of this License, they do not 544 | excuse you from the conditions of this License. If you cannot convey a 545 | covered work so as to satisfy simultaneously your obligations under this 546 | License and any other pertinent obligations, then as a consequence you may 547 | not convey it at all. For example, if you agree to terms that obligate you 548 | to collect a royalty for further conveying from those to whom you convey 549 | the Program, the only way you could satisfy both those terms and this 550 | License would be to refrain entirely from conveying the Program. 551 | 552 | 13. Use with the GNU Affero General Public License. 553 | 554 | Notwithstanding any other provision of this License, you have 555 | permission to link or combine any covered work with a work licensed 556 | under version 3 of the GNU Affero General Public License into a single 557 | combined work, and to convey the resulting work. The terms of this 558 | License will continue to apply to the part which is the covered work, 559 | but the special requirements of the GNU Affero General Public License, 560 | section 13, concerning interaction through a network will apply to the 561 | combination as such. 562 | 563 | 14. Revised Versions of this License. 564 | 565 | The Free Software Foundation may publish revised and/or new versions of 566 | the GNU General Public License from time to time. Such new versions will 567 | be similar in spirit to the present version, but may differ in detail to 568 | address new problems or concerns. 569 | 570 | Each version is given a distinguishing version number. If the 571 | Program specifies that a certain numbered version of the GNU General 572 | Public License "or any later version" applies to it, you have the 573 | option of following the terms and conditions either of that numbered 574 | version or of any later version published by the Free Software 575 | Foundation. If the Program does not specify a version number of the 576 | GNU General Public License, you may choose any version ever published 577 | by the Free Software Foundation. 578 | 579 | If the Program specifies that a proxy can decide which future 580 | versions of the GNU General Public License can be used, that proxy's 581 | public statement of acceptance of a version permanently authorizes you 582 | to choose that version for the Program. 583 | 584 | Later license versions may give you additional or different 585 | permissions. However, no additional obligations are imposed on any 586 | author or copyright holder as a result of your choosing to follow a 587 | later version. 588 | 589 | 15. Disclaimer of Warranty. 590 | 591 | THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY 592 | APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT 593 | HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY 594 | OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, 595 | THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 596 | PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM 597 | IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF 598 | ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 599 | 600 | 16. Limitation of Liability. 601 | 602 | IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 603 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS 604 | THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY 605 | GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE 606 | USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF 607 | DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD 608 | PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), 609 | EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF 610 | SUCH DAMAGES. 611 | 612 | 17. Interpretation of Sections 15 and 16. 613 | 614 | If the disclaimer of warranty and limitation of liability provided 615 | above cannot be given local legal effect according to their terms, 616 | reviewing courts shall apply local law that most closely approximates 617 | an absolute waiver of all civil liability in connection with the 618 | Program, unless a warranty or assumption of liability accompanies a 619 | copy of the Program in return for a fee. 620 | 621 | END OF TERMS AND CONDITIONS 622 | 623 | How to Apply These Terms to Your New Programs 624 | 625 | If you develop a new program, and you want it to be of the greatest 626 | possible use to the public, the best way to achieve this is to make it 627 | free software which everyone can redistribute and change under these terms. 628 | 629 | To do so, attach the following notices to the program. It is safest 630 | to attach them to the start of each source file to most effectively 631 | state the exclusion of warranty; and each file should have at least 632 | the "copyright" line and a pointer to where the full notice is found. 633 | 634 | 635 | Copyright (C) 636 | 637 | This program is free software: you can redistribute it and/or modify 638 | it under the terms of the GNU General Public License as published by 639 | the Free Software Foundation, either version 3 of the License, or 640 | (at your option) any later version. 641 | 642 | This program is distributed in the hope that it will be useful, 643 | but WITHOUT ANY WARRANTY; without even the implied warranty of 644 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 645 | GNU General Public License for more details. 646 | 647 | You should have received a copy of the GNU General Public License 648 | along with this program. If not, see . 649 | 650 | Also add information on how to contact you by electronic and paper mail. 651 | 652 | If the program does terminal interaction, make it output a short 653 | notice like this when it starts in an interactive mode: 654 | 655 | Copyright (C) 656 | This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 657 | This is free software, and you are welcome to redistribute it 658 | under certain conditions; type `show c' for details. 659 | 660 | The hypothetical commands `show w' and `show c' should show the appropriate 661 | parts of the General Public License. Of course, your program's commands 662 | might be different; for a GUI interface, you would use an "about box". 663 | 664 | You should also get your employer (if you work as a programmer) or school, 665 | if any, to sign a "copyright disclaimer" for the program, if necessary. 666 | For more information on this, and how to apply and follow the GNU GPL, see 667 | . 668 | 669 | The GNU General Public License does not permit incorporating your program 670 | into proprietary programs. If your program is a subroutine library, you 671 | may consider it more useful to permit linking proprietary applications with 672 | the library. If this is what you want to do, use the GNU Lesser General 673 | Public License instead of this License. But first, please read 674 | . 675 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Time-Optimal Planning for Quadrotor Waypoint Flight 2 | This is an **example implementation** of the paper "Time-Optimal Planning for Quadrotor Waypoint Flight" by Philipp Foehn, Angel Romero, Davide Scaramuzza 3 | published in Science Robotics, Volume 6, Issue 56, 2021. 4 | 5 | Check out the [video](https://www.youtube.com/watch?v=ZPI8U1uSJUs), the [paper](http://rpg.ifi.uzh.ch/docs/ScienceRobotics21_Foehn.pdf), and follow the [instructions](#instructions) below. 6 | 7 | 8 | # Video 9 | [![Time-Optimal-Planning](https://img.youtube.com/vi/ZPI8U1uSJUs/0.jpg)](https://www.youtube.com/watch?v=ZPI8U1uSJUs) 10 | 11 | # Paper 12 | If you use this code in an academic context, please cite the following [Science Robotics 2021 paper](http://rpg.ifi.uzh.ch/docs/ScienceRobotics21_Foehn.pdf). 13 | 14 | Philipp Foehn, Angel Romero, Davide Scaramuzza, 15 | "**Time-Optimal Planning for Quadrotor Waypoint Flight**," 16 | 2021, Science Robotics, Volume 6, Issue 56, DOI: 10.1126/scirobotics.abh1221 17 | 18 | Bibtex: 19 | ``` 20 | @article {foehn2021CPC, 21 | author = {Foehn, Philipp and Romero, Angel and Scaramuzza, Davide}, 22 | title = {Time-Optimal Planning for Quadrotor Waypoint Flight}, 23 | volume = {6}, 24 | number = {56}, 25 | elocation-id = {eabh1221}, 26 | year = {2021}, 27 | doi = {10.1126/scirobotics.abh1221}, 28 | publisher = {Science Robotics}, 29 | URL = {https://robotics.sciencemag.org/content/6/56/eabh1221}, 30 | eprint = {https://robotics.sciencemag.org/content/6/56/eabh1221.full.pdf}, 31 | journal = {Science Robotics} 32 | } 33 | ``` 34 | 35 | # Instructions 36 | 1. Make sure you have Python3 running. 37 | 4. Clone this repository with `git clone git@github.com:uzh-rpg/rpg_time_optimal.git`. 38 | 5. Navigate into the root folder of the clone repository `cd rpg_time_optimal`. 39 | 2. Install the requirements `pip3 install -r requirements.txt` 40 | 3. Download CasADi from [the official website](https://web.casadi.org) or with `pip install casadi`. 41 | 6. Run the example with `python3 example/optimization.py`. 42 | 7. Show some sparkly plots with `python3 example/plotting.py`. 43 | 44 | This will create output `.csv` files with the trajectory and some `.pdf` visualizing the result. 45 | -------------------------------------------------------------------------------- /example/optimization.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import os 3 | BASEPATH = os.path.abspath(__file__).split('rpg_time_optimal', 1)[0]+'rpg_time_optimal/' 4 | sys.path += [BASEPATH + 'src'] 5 | from track import Track 6 | from quad import Quad 7 | from integrator import RungeKutta4 8 | from planner import Planner 9 | from trajectory import Trajectory 10 | from plot import CallbackPlot 11 | 12 | track = Track(BASEPATH + "/tracks/track.yaml") 13 | quad = Quad(BASEPATH + "/quads/quad.yaml") 14 | 15 | cp = CallbackPlot(pos='xy', vel='xya', ori='xyzw', rate='xyz', inputs='u', prog='mn') 16 | planner = Planner(quad, track, RungeKutta4, {'tolerance': 0.3, 'nodes_per_gate': 40, 'vel_guess': 3.0}) 17 | planner.setup() 18 | planner.set_iteration_callback(cp) 19 | x = planner.solve() 20 | 21 | traj = Trajectory(x, NPW=planner.NPW, wp=planner.wp) 22 | traj.save(BASEPATH + '/example/result_cpc_format.csv', False) 23 | traj.save(BASEPATH + '/example/result.csv', True) 24 | -------------------------------------------------------------------------------- /example/plotting.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import os 3 | BASEPATH = os.path.abspath(__file__).split('rpg_time_optimal', 1)[0]+'rpg_time_optimal/' 4 | sys.path += [BASEPATH + 'src'] 5 | from trajectory import Trajectory 6 | import matplotlib.pyplot as plt 7 | 8 | path = './example' 9 | 10 | traj = Trajectory(path + '/result_cpc_format.csv') 11 | print('Total Time: %1.4fs' % traj.t_total) 12 | 13 | fig_pos_xy = plt.figure(0, (3,3)) 14 | axhxy = fig_pos_xy.gca() 15 | traj.plotPos(fig_pos_xy, '', plot_axis='xyq', arrow_size=3.0, arrow_nth=15, arrow_args={'color': 'k'}) 16 | axhxy.set_title('') 17 | axhxy.grid(True) 18 | fig_pos_xy.tight_layout() 19 | fig_pos_xy.savefig(path + '/pos_xy.pdf') 20 | 21 | fig_pos_xz = plt.figure(1, (3,3)) 22 | axhxz = fig_pos_xz.gca() 23 | traj.plotPos(fig_pos_xz, '', plot_axis='xzq', arrow_size=3.0, arrow_nth=15, arrow_args={'color': 'k'}) 24 | axhxz.set_title('') 25 | axhxz.grid(True) 26 | fig_pos_xz.tight_layout() 27 | fig_pos_xz.savefig(path + '/pos_xz.pdf') 28 | 29 | fig_vel = plt.figure(2, (6, 2)) 30 | axhv = fig_vel.gca() 31 | traj.plotVel(fig_vel, '', label=None) 32 | axhv.grid(True) 33 | fig_vel.legend(['$v_x$', '$v_y$', '$v_z$', '$\|v\|$'], loc='right') 34 | fig_vel.tight_layout() 35 | fig_vel.savefig(path + '/vel.pdf') 36 | 37 | fig_prog = plt.figure(3, (6, 2)) 38 | axhm = fig_prog.gca() 39 | traj.plotProgress(fig_prog, '') 40 | axhm.grid(True) 41 | fig_prog.tight_layout() 42 | fig_prog.savefig(path + '/prog.pdf') 43 | -------------------------------------------------------------------------------- /quads/quad.yaml: -------------------------------------------------------------------------------- 1 | 2 | mass: 0.85 # mass [kg] 3 | arm_length: 0.15 # body center to motor distance diagonal [m] 4 | inertia: [[0.001, 0, 0], 5 | [0, 0.001, 0], 6 | [0, 0, 0.0017]] 7 | thrust_min: 0.0 # min thrust per propeller [N] 8 | TWR_max: 3.3 # max thrust to weight ratio 9 | omega_max_xy: 15.0 # max bodyrate magnitude [rad/s] 10 | omega_max_z: 0.3 # max bodyrate magnitude [rad/s] 11 | torque_coeff: 0.05 # thrust to drag relation 12 | 13 | rampup_dist: 35 # distance over which we ramp up [m] 14 | TWR_ramp_start: 1.5 # start of TWR ramp 15 | omega_ramp_start: 4.5 # start of XY body rates ramp [rad/s] 16 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | casadi==3.5.5 2 | cycler==0.10.0 3 | kiwisolver==1.3.1 4 | matplotlib==3.4.1 5 | numpy==1.20.2 6 | pandas==1.2.4 7 | Pillow==8.2.0 8 | pyaml==20.4.0 9 | pyparsing==2.4.7 10 | python-dateutil==2.8.1 11 | pytz==2021.1 12 | PyYAML==5.4.1 13 | scipy==1.6.3 14 | six==1.16.0 15 | -------------------------------------------------------------------------------- /src/__ini__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_time_optimal/f5541b6d9d3dee563e01a64ec1994b8ed6c0076c/src/__ini__.py -------------------------------------------------------------------------------- /src/integrator.py: -------------------------------------------------------------------------------- 1 | from casadi import MX, Function 2 | 3 | 4 | def Euler(dynamics): 5 | dt = MX.sym('dt', 1) 6 | x = MX.sym('x', dynamics.size1_in(0)) 7 | u = MX.sym('u', dynamics.size1_in(1)) 8 | integrator = Function('integrator', 9 | [x, u, dt], 10 | [x + dt * dynamics(x, u)], 11 | ['x', 'u', 'dt'], ['xn']) 12 | return integrator 13 | 14 | def RungeKutta4(dynamics): 15 | dt = MX.sym('dt', 1) 16 | x = MX.sym('x', dynamics.size1_in(0)) 17 | u = MX.sym('u', dynamics.size1_in(1)) 18 | k1 = dynamics(x, u) 19 | k2 = dynamics(x + dt/2 * k1, u) 20 | k3 = dynamics(x + dt/2 * k2, u) 21 | k4 = dynamics(x + dt * k3, u) 22 | integrator = Function('integrator', 23 | [x, u, dt], 24 | [x + dt/6 * (k1 + 2*k2 + 2*k3 + k4)], 25 | ['x', 'u', 'dt'], ['xn']) 26 | return integrator -------------------------------------------------------------------------------- /src/planner.py: -------------------------------------------------------------------------------- 1 | from casadi import MX, DM, vertcat, horzcat, veccat, norm_2, dot, mtimes, nlpsol, diag, repmat, sum1 2 | import numpy as np 3 | import inspect 4 | 5 | class Planner: 6 | def __init__(self, quad, track, Integrator, options = {}): 7 | # Essentials 8 | self.quad = quad 9 | self.track = track 10 | self.options = options 11 | 12 | # Track 13 | self.wp = DM(track.gates).T 14 | if track.end_pos is not None: 15 | if len(track.gates) > 0: 16 | self.wp = horzcat(self.wp, DM(track.end_pos)) 17 | else: 18 | self.wp = DM(track.end_pos) 19 | 20 | if track.init_pos is not None: 21 | self.p_init = DM(track.init_pos) 22 | else: 23 | self.p_init = self.wp[:,-1] 24 | if track.init_att is not None: 25 | self.q_init = DM(track.init_att) 26 | else: 27 | self.q_init = DM([1, 0, 0, 0]).T 28 | 29 | # Dynamics 30 | dynamics = quad.dynamics() 31 | self.fdyn = Integrator(dynamics) 32 | 33 | # Sizes 34 | self.NX = dynamics.size1_in(0) 35 | self.NU = dynamics.size1_in(1) 36 | self.NW = self.wp.shape[1] 37 | 38 | dist = [np.linalg.norm(self.wp[:,0] - self.p_init)] 39 | for i in range(self.NW-1): 40 | dist += [dist[i] + np.linalg.norm(self.wp[:,i+1] - self.wp[:,i])] 41 | 42 | if 'nodes_per_gate' in options: 43 | self.NPW = options['nodes_per_gate'] 44 | else: 45 | self.NPW = 30 46 | 47 | if 'tolerance' in options: 48 | self.tol = options['tolerance'] 49 | else: 50 | self.tol = 0.3 51 | 52 | self.N = self.NPW * self.NW 53 | self.dpn = dist[-1] / self.N 54 | if self.dpn < self.tol: 55 | suff_str = "sufficient" 56 | else: 57 | suff_str = "insufficient" 58 | print("Discretization over %d nodes and %1.1fm" % (self.N, dist[-1])) 59 | print("results in %1.3fm per node, %s for tolerance of %1.3fm." % (self.dpn, suff_str, self.tol)) 60 | 61 | self.i_switch = np.array(self.N * np.array(dist) / dist[-1], dtype=int) 62 | 63 | # Problem variables 64 | self.x = [] 65 | self.xg = [] 66 | self.g = [] 67 | self.lb = [] 68 | self.ub = [] 69 | self.J = [] 70 | 71 | # Solver 72 | if 'solver_options' in options: 73 | self.solver_options = options['solver_options'] 74 | else: 75 | self.solver_options = {} 76 | ipopt_options = {} 77 | ipopt_options['max_iter'] = 10000 78 | self.solver_options['ipopt'] = ipopt_options 79 | if 'solver_type' in options: 80 | self.solver_type = options['solver_type'] 81 | else: 82 | self.solver_type = 'ipopt' 83 | self.iteration_plot = [] 84 | 85 | # Guesses 86 | if 't_guess' in options: 87 | self.t_guess = options['t_guess'] 88 | self.vel_guess = dist[-1] / self.t_guess 89 | elif 'vel_guess' in options: 90 | self.vel_guess = options['vel_guess'] 91 | self.t_guess = dist[-1] / self.vel_guess 92 | else: 93 | self.vel_guess = 2.5 94 | self.t_guess = dist[-1] / self.vel_guess 95 | 96 | if 'legacy_init' in options: 97 | if options['legacy_init']: 98 | self.i_switch = range(self.NPW,self.N+1,self.NPW) 99 | 100 | def set_iteration_callback(self, callback): 101 | self.iteration_callback = callback 102 | 103 | def set_init_pos(self, position): 104 | self.p_init = DM(position) 105 | 106 | def set_init_att(self, quaternion): 107 | self.q_init = DM(quaternion) 108 | 109 | def set_t_guess(self, t): 110 | self.t_guess 111 | 112 | def set_initial_guess(self, x_guess): 113 | if len(x_guess) > 0 and (self.xg.shape[0] == len(x_guess) or self.xg.shape[0] == 0): 114 | self.xg = veccat(*x_guess) 115 | 116 | def setup(self): 117 | x = [] 118 | xg = [] 119 | g = [] 120 | lb = [] 121 | ub = [] 122 | J = 0 123 | 124 | # Total time variable 125 | t = MX.sym('t', 1) 126 | x += [t] 127 | xg += [self.t_guess] 128 | g += [t] 129 | lb += [0.1] 130 | ub += [150] 131 | J = t 132 | 133 | # Bound initial state to x0 134 | xk = MX.sym('x_init', self.NX) 135 | vel_guess = (self.wp[:,0] - self.p_init) 136 | vel_guess = self.vel_guess * vel_guess / norm_2(vel_guess) 137 | x0 = [self.p_init[0], self.p_init[1], self.p_init[2], vel_guess[0], vel_guess[1], vel_guess[2], self.q_init[0], self.q_init[1], self.q_init[2], self.q_init[3], 0, 0, 0] 138 | pos_guess = self.p_init 139 | if self.track.init_vel is not None: 140 | x0[3:6] = self.track.init_vel 141 | x += [xk] 142 | xg += x0 143 | if self.track.init_pos is not None: 144 | # if self.track.init_pos is not None and not self.track.ring: 145 | print('Using start position constraint') 146 | g += [xk[0:3]] 147 | ub += [self.track.init_pos] 148 | lb += [self.track.init_pos] 149 | if self.track.init_vel is not None: 150 | print('Using start velocity constraint') 151 | g += [xk[3:6]] 152 | ub += [self.track.init_vel] 153 | lb += [self.track.init_vel] 154 | if self.track.init_att is not None: 155 | print('Using start attitude constraint') 156 | g += [xk[6:10]] 157 | ub += [self.track.init_att] 158 | lb += [self.track.init_att] 159 | else: 160 | g += [dot(xk[6:10], xk[6:10])] 161 | ub += [1.0] 162 | lb += [1.0] 163 | if self.track.init_omega is not None: 164 | print('Using start bodyrate constraint') 165 | g += [xk[10:13]] 166 | ub += [self.track.init_omega] 167 | lb += [self.track.init_omega] 168 | x_init = xk 169 | 170 | # Bound inital progress variable to 1 171 | muk = MX.sym('mu_init', self.NW) 172 | x += [muk] 173 | xg += [1]*(self.NW) 174 | g += [muk] 175 | ub += [1]*(self.NW) 176 | lb += [1]*(self.NW) 177 | 178 | # For each node ... 179 | i_wp = 0 180 | for i in range(self.N): 181 | T_max = self.quad.T_max 182 | omega_max_xy = self.quad.omega_max_xy 183 | 184 | # linearly interpolate max thrust and max omegas 185 | if self.quad.rampup_dist > 0: 186 | T_max = max(min(self.interpolate(0, self.quad.T_ramp_start, self.quad.rampup_dist, self.quad.T_max, i * self.dpn), self.quad.T_max), self.quad.T_ramp_start) 187 | omega_max_xy = max(min(self.interpolate(0, self.quad.omega_ramp_start, self.quad.rampup_dist, self.quad.omega_max_xy, i * self.dpn), self.quad.omega_max_xy), self.quad.omega_ramp_start) 188 | 189 | # ... add inputs 190 | uk = MX.sym('u'+str(i), self.NU) 191 | x += [uk] 192 | xg += [T_max]*self.NU 193 | g += [uk] 194 | lb += [self.quad.T_min]*self.NU 195 | ub += [T_max]*self.NU 196 | 197 | # ... add next state 198 | Fnext = self.fdyn(x = xk, u = uk, dt = t/self.N) 199 | xn = Fnext['xn'] 200 | 201 | xk = MX.sym('x'+str(i), self.NX) 202 | x += [xk] 203 | g += [xk - xn] 204 | lb += [0] * self.NX 205 | ub += [0] * self.NX 206 | 207 | # if i >= (1 + i_wp) * self.NPW: 208 | if i > self.i_switch[i_wp]: 209 | i_wp += 1 210 | if i_wp == 0: 211 | wp_last = self.p_init 212 | else: 213 | wp_last = self.wp[:, i_wp-1] 214 | wp_next = self.wp[:, i_wp] 215 | if i_wp > 0: 216 | interp = (i - self.i_switch[i_wp-1]) / (self.i_switch[i_wp] - self.i_switch[i_wp-1]) 217 | else: 218 | interp = i / self.i_switch[0] 219 | pos_guess = (1-interp) * wp_last + interp * wp_next 220 | vel_guess = self.vel_guess * (wp_next - wp_last)/norm_2(wp_next - wp_last) 221 | # direction = (wp_next - wp_last)/norm_2(wp_next - wp_last) 222 | # if interp < 0.5: 223 | # vel_guess = interp * 4 * self.vel_guess * direction 224 | # else: 225 | # vel_guess = (1 - interp) * 4 * self.vel_guess * direction 226 | # pos_guess += self.t_guess / self.N * vel_guess 227 | xg += [pos_guess, vel_guess, self.q_init, [0]*3] 228 | 229 | # Progress Variables 230 | lam = MX.sym('lam'+str(i), self.NW) 231 | x += [lam] 232 | g += [lam] 233 | lb += [0]*self.NW 234 | ub += [1]*self.NW 235 | if ((i_wp == 0) and (i + 1 >= self.i_switch[0])) or i + 1 - self.i_switch[i_wp-1] >= self.i_switch[i_wp]: 236 | lamg = [0] * self.NW 237 | lamg[i_wp] = 1.0 238 | xg += lamg 239 | else: 240 | xg += [0] * self.NW 241 | 242 | tau = MX.sym('tau'+str(i), self.NW) 243 | x += [tau] 244 | g += [tau] 245 | lb += [0]*self.NW 246 | ub += [self.tol**2]*(self.NW) 247 | xg += [0] * self.NW 248 | 249 | for j in range(self.NW): 250 | diff = xk[0:3] - self.wp[:,j] 251 | g += [lam[j] * (dot(diff, diff)-tau[j])] 252 | lb += [0]*self.NW 253 | ub += [0.01]*self.NW 254 | 255 | mul = muk 256 | muk = MX.sym('mu'+str(i), self.NW) 257 | x += [muk] 258 | g += [mul - lam - muk] 259 | lb += [0]*self.NW 260 | ub += [0]*self.NW 261 | 262 | for j in range(self.NW): 263 | # if i >= ((j+1)*self.NPW - 1): 264 | if i+1 >= self.i_switch[j]: 265 | xg += [0] 266 | else: 267 | xg += [1] 268 | 269 | # Bind rates 270 | g += [xk[10:13]] 271 | lb += [-omega_max_xy, -omega_max_xy, -self.quad.omega_max_z] 272 | ub += [omega_max_xy, omega_max_xy, self.quad.omega_max_z] 273 | 274 | # z constraint 275 | g += [xk[2]] 276 | lb += [0.5] 277 | ub += [100.0] # infinity 278 | 279 | for j in range(self.NW-1): 280 | g += [muk[j+1]-muk[j]] 281 | lb += [0] 282 | ub += [1] 283 | # end for loop ############################################################# 284 | 285 | g += [muk] 286 | lb += [0]*self.NW 287 | ub += [0]*self.NW 288 | 289 | if self.track.ring: 290 | print('Using ring constraint') 291 | g += [xk[3:6] - x_init[3:6]] 292 | lb += [0] * 3 #self.NX 293 | ub += [0] * 3 #self.NX 294 | 295 | if self.track.end_att is not None: 296 | print('Using end attitude constraint') 297 | g += [xk[6:10]] 298 | lb += [self.track.end_att] 299 | ub += [self.track.end_att] 300 | 301 | if self.track.end_vel is not None: 302 | print('Using end velocity constraint') 303 | g += [xk[3:6]] 304 | lb += [self.track.end_vel] 305 | ub += [self.track.end_vel] 306 | 307 | if self.track.end_omega is not None: 308 | print('Using end bodyrate constraint') 309 | g += [xk[10:13]] 310 | lb += [self.track.end_omega] 311 | ub += [self.track.end_omega] 312 | 313 | # Reformat 314 | self.x = vertcat(*x) 315 | if not self.xg: 316 | self.xg = xg 317 | self.xg = veccat(*self.xg) 318 | self.g = vertcat(*g) 319 | self.lb = veccat(*lb) 320 | self.ub = veccat(*ub) 321 | self.J = J 322 | 323 | # Construct Non-Linear Program 324 | self.nlp = {'f': self.J, 'x': self.x, 'g': self.g} 325 | 326 | def solve(self, x_guess = []): 327 | self.set_initial_guess(x_guess) 328 | 329 | if hasattr(self, 'iteration_callback') and self.iteration_callback is not None: 330 | if inspect.isclass(self.iteration_callback): 331 | callback = self.iteration_callback("IterationCallback") 332 | elif self.iteration_callback: 333 | callback = self.iteration_callback 334 | 335 | callback.set_size(self.x.shape[0], self.g.shape[0], self.NPW) 336 | callback.set_wp(self.wp) 337 | self.solver_options['iteration_callback'] = callback 338 | 339 | self.solver = nlpsol('solver', self.solver_type, self.nlp, self.solver_options) 340 | 341 | self.solution = self.solver(x0=self.xg, lbg=self.lb, ubg=self.ub) 342 | self.x_sol = self.solution['x'].full().flatten() 343 | return self.x_sol 344 | 345 | def interpolate(self, x1, y1, x2, y2, x): 346 | if (abs(x2 - x1) < 1e-5): 347 | return 0 348 | 349 | return y1 + (y2 - y1)/(x2 - x1) * (x - x1) 350 | -------------------------------------------------------------------------------- /src/plot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | import matplotlib.pyplot as plt 3 | import argparse 4 | import math 5 | import casadi as ca 6 | from trajectory import Trajectory 7 | 8 | class CallbackPlot(ca.Callback): 9 | def __init__(self, 10 | pos='xy', vel=None, ori=None, rate=None, 11 | inputs=None, prog=None, save=None, fig=None, opts={}): 12 | ca.Callback.__init__(self) 13 | 14 | self.nx = None 15 | self.ng = None 16 | self.NPW = None 17 | self.wp = None 18 | self.i = 0 19 | self.opts = opts 20 | 21 | if pos is not None: assert type(pos) == str 22 | if vel is not None: assert type(vel) == str 23 | if ori is not None: assert type(ori) == str 24 | if rate is not None: assert type(rate) == str 25 | if inputs is not None: assert type(inputs) == str 26 | if prog is not None: assert type(prog) == str 27 | if save is not None: assert type(save) == str 28 | 29 | self.pos = pos 30 | self.vel = vel 31 | self.ori = ori 32 | self.rate = rate 33 | self.inputs = inputs 34 | self.prog = prog 35 | self.save = save 36 | self.n_plots = 0 37 | 38 | for plot in [self.pos, self.vel, self.ori, self.rate, self.inputs, self.prog]: 39 | self.n_plots += int((plot is not None) & (type(plot)==str)) 40 | assert self.n_plots > 0 41 | 42 | if self.n_plots<=3: 43 | self.plot_r = self.n_plots 44 | self.plot_c = 1 45 | else: 46 | self.plot_r = 2 47 | self.plot_c = int((self.n_plots+1)/2) 48 | 49 | if fig is None: 50 | self.fig = plt.figure() 51 | elif type(fig) == int: 52 | self.fig = plt.figure(fig) 53 | elif type(fig) == plt.Figure: 54 | self.fig = plt.figure(fig.number) 55 | else: 56 | raise Exception('No valid figure passed!') 57 | 58 | print(self.plot_r) 59 | print(self.plot_c) 60 | self.axes = [] 61 | if self.n_plots>1: 62 | for i in range(self.n_plots): 63 | self.axes.append(plt.subplot(self.plot_r, self.plot_c, 1+i)) 64 | else: 65 | self.axes.append(self.fig.gca()) 66 | 67 | self.fig.show() 68 | 69 | def set_size(self, nx, ng, NPW): 70 | self.nx = nx 71 | self.ng = ng 72 | self.NPW = NPW 73 | self.construct('CallbackPlot', self.opts) 74 | 75 | def set_wp(self, wp): 76 | self.wp = wp 77 | 78 | def get_n_in(self): return ca.nlpsol_n_out() 79 | def get_n_out(self): return 1 80 | def get_name_in(self, i): return ca.nlpsol_out(i) 81 | def get_name_out(self, i): return "ret" 82 | 83 | def get_sparsity_in(self, i): 84 | n = ca.nlpsol_out(i) 85 | if n=='f': 86 | return ca.Sparsity.scalar() 87 | elif n in ('x', 'lam_x'): 88 | return ca.Sparsity.dense(self.nx) 89 | elif n in ('g', 'lam_g'): 90 | return ca.Sparsity.dense(self.ng) 91 | else: 92 | return ca.Sparsity(0,0) 93 | 94 | def eval(self, arg): 95 | # Create dictionary 96 | darg = {} 97 | for (i,s) in enumerate(ca.nlpsol_out()): darg[s] = arg[i] 98 | 99 | X_opt = darg['x'].full().flatten() 100 | traj = Trajectory(X_opt, NPW=self.NPW, wp=self.wp) 101 | 102 | i_plot = 0 103 | if self.pos is not None: 104 | self.axes[i_plot].cla() 105 | traj.plotPos(self.axes[i_plot], plot_axis=self.pos) 106 | i_plot += 1 107 | 108 | if self.ori is not None: 109 | self.axes[i_plot].cla() 110 | traj.plotOrientation(self.axes[i_plot], plot_axis=self.ori) 111 | i_plot += 1 112 | 113 | if self.prog is not None: 114 | self.axes[i_plot].cla() 115 | traj.plotProgress(self.axes[i_plot], plot_axis=self.prog) 116 | i_plot += 1 117 | 118 | if self.vel is not None: 119 | self.axes[i_plot].cla() 120 | traj.plotVel(self.axes[i_plot], plot_axis=self.vel) 121 | i_plot += 1 122 | 123 | if self.rate is not None: 124 | self.axes[i_plot].cla() 125 | traj.plotOmega(self.axes[i_plot], plot_axis=self.rate) 126 | i_plot += 1 127 | 128 | if self.inputs is not None: 129 | self.axes[i_plot].cla() 130 | traj.plotThrust(self.axes[i_plot], plot_axis=self.inputs) 131 | i_plot += 1 132 | 133 | if self.save is not None: 134 | traj.save(self.save+'/iteration_%05d.csv' % self.i) 135 | else: 136 | plt.draw() 137 | self.fig.canvas.start_event_loop(0.0002) 138 | 139 | self.i += 1 140 | return [0] 141 | 142 | 143 | if __name__ == '__main__': 144 | parser = argparse.ArgumentParser(description='Plots a given trajectory from .csv format.') 145 | parser.add_argument('filename', metavar='file', type=str, 146 | help='filename of the trajectory csv file') 147 | parser.add_argument('-p', '--position', dest='pos', type=str, 148 | help='plot position over specified axes') 149 | parser.add_argument('-v', '--velocity', dest='vel', type=str, 150 | help='plot velocity over specified axes') 151 | parser.add_argument('-w', '--omega', dest='omega', type=str, 152 | help='plot omega over specified axes') 153 | parser.add_argument('-q', '--orientation', dest='ori', type=str, 154 | help='plot orientation quaternion over specified axes') 155 | parser.add_argument('-u', '--thrust', dest='thrust', type=str, 156 | help='plot thrust at rotors or acceleration over specified axes') 157 | parser.add_argument('-m', '--progress', dest='prog', type=str, 158 | help='plot progress variables') 159 | parser.add_argument('-s', '--sub', dest='subp', metavar=('rows', 'cols'), type=int, nargs=2, 160 | help='plot all in one figure arrange by rows*cols plots') 161 | parser.add_argument('-o', '--output', dest='output', type=str, 162 | help='filename to output figure') 163 | args = parser.parse_args() 164 | 165 | traj = Trajectory(args.filename) 166 | print("Plotting trajectory with %d waypoints and %d nodes." % (traj.NW, traj.N)) 167 | print("Overall time: %1.3f" % traj.t_total) 168 | 169 | if all(arg==None for arg in [ 170 | args.pos, args.vel, args.omega, args.ori, args.thrust, args.prog]): 171 | args.subp = [2, 3] 172 | args.pos = 'xy' 173 | args.vel = 'xyza' 174 | args.omega = 'xyz' 175 | args.ori = 'wxyza' 176 | args.thrust = 'u' 177 | args.prog = 'mn' 178 | 179 | n_plots = 0 180 | if args.pos is not None: n_plots += 1 181 | if args.vel is not None: n_plots += 1 182 | if args.omega is not None: n_plots += 1 183 | if args.ori is not None: n_plots += 1 184 | if args.thrust is not None: n_plots += 1 185 | if args.prog is not None: n_plots += 1 186 | 187 | use_subplot = False 188 | if args.subp is not None: 189 | subr = args.subp[0] 190 | subc = args.subp[1] 191 | use_subplot = True 192 | if subr * subc < n_plots: 193 | Warning('Subplot too small') 194 | subc = math.ceil(n_plots/subr) 195 | 196 | if use_subplot: 197 | fig = plt.figure(0) 198 | 199 | axes = [] 200 | for i in range(n_plots): 201 | if use_subplot: 202 | axes += [plt.subplot(subr, subc, i+1)] 203 | else: 204 | axes += [plt.figure(i)] 205 | 206 | i_fig = 0 207 | if args.pos is not None: 208 | traj.plotPos(axes[i_fig], plot_axis=args.pos, wp_style='rx') 209 | i_fig += 1 210 | 211 | if args.ori is not None: 212 | traj.plotOrientation(axes[i_fig], plot_axis=args.ori) 213 | i_fig += 1 214 | 215 | if args.prog is not None: 216 | traj.plotProgress(axes[i_fig], plot_axis=args.prog) 217 | i_fig += 1 218 | 219 | if args.vel is not None: 220 | traj.plotVel(axes[i_fig], plot_axis=args.vel) 221 | i_fig += 1 222 | 223 | if args.omega is not None: 224 | traj.plotOmega(axes[i_fig], plot_axis=args.omega) 225 | i_fig += 1 226 | 227 | if args.thrust is not None: 228 | traj.plotThrust(axes[i_fig], plot_axis=args.thrust) 229 | i_fig += 1 230 | 231 | if args.output is not None: 232 | plt.savefig(fig, args.output) 233 | else: 234 | plt.show() 235 | -------------------------------------------------------------------------------- /src/progress.py: -------------------------------------------------------------------------------- 1 | import casadi as ca 2 | 3 | class Progress4: 4 | def __init__(self, waypoints, options = {}): 5 | self.wp = waypoints 6 | self.NW = waypoints.shape[1] 7 | if 'distance_threshold' in options: 8 | self.dist = options['distance_threshold'] 9 | else: 10 | self.dist = 0.3 11 | self.th = 5/3*self.dist**4 12 | 13 | def step(self): 14 | p_gate = ca.MX.sym('p_gate', 3) 15 | p_adj = ca.MX.sym('p_adj', 3) 16 | p = ca.MX.sym('p', 3) 17 | v = ca.MX.sym('v', 3) 18 | q = ca.MX.sym('q', 4) 19 | w = ca.MX.sym('w', 3) 20 | x = ca.vertcat(p, v, q, w) 21 | 22 | f_prog = ca.Function('f_prog', 23 | [x, p_gate, p_adj], 24 | [1-self.th/(ca.dot((p-p_gate-p_adj), (p-p_gate-p_adj))**2+self.th)]) 25 | 26 | mu = ca.MX.sym('mu', self.NW) 27 | tau = ca.MX.sym('tau', 3, self.NW) 28 | mu_step = ca.Function('mu_step', 29 | [x, mu, tau], 30 | [ca.mtimes(ca.diag(f_prog(ca.repmat(x,1,self.NW),self.wp[:,0:self.NW], tau)) 31 | ,mu)], ['x', 'mu', 'tau'], ['mun']) 32 | return mu_step 33 | 34 | class Progress2: 35 | def __init__(self, waypoints, options = {}): 36 | self.wp = waypoints 37 | self.NW = waypoints.shape[1] 38 | if 'distance_threshold' in options: 39 | self.dist = options['distance_threshold'] 40 | else: 41 | self.dist = 0.05 42 | self.th = 3*self.dist**2 43 | 44 | def step(self): 45 | p_gate = ca.MX.sym('p_gate', 3) 46 | p_adj = ca.MX.sym('p_adj', 3) 47 | p = ca.MX.sym('p', 3) 48 | v = ca.MX.sym('v', 3) 49 | q = ca.MX.sym('q', 4) 50 | w = ca.MX.sym('w', 3) 51 | x = ca.vertcat(p, v, q, w) 52 | 53 | f_prog = ca.Function('f_prog', 54 | [x, p_gate, p_adj], 55 | [1-self.th/(ca.dot((p-p_gate-p_adj), (p-p_gate-p_adj))+self.th)]) 56 | 57 | mu = ca.MX.sym('mu', self.NW) 58 | tau = ca.MX.sym('tau', 3, self.NW) 59 | mu_step = ca.Function('mu_step', 60 | [x, mu, tau], 61 | [ca.mtimes(ca.diag(f_prog(ca.repmat(x,1,self.NW),self.wp[:,0:self.NW], tau)) 62 | ,mu)], ['x', 'mu', 'tau'], ['mun']) 63 | return mu_step 64 | -------------------------------------------------------------------------------- /src/quad.py: -------------------------------------------------------------------------------- 1 | from casadi import MX, DM, vertcat, mtimes, Function, inv, cross, sqrt, norm_2 2 | import yaml 3 | from quaternion import * 4 | 5 | 6 | class Quad: 7 | def __init__(self, filename = ""): 8 | self.m = 1 # mass in [kg] 9 | self.l = 1 # arm length 10 | self.I = DM([(1, 0, 0), (0, 1, 0), (0, 0, 1)]) # Inertia 11 | self.I_inv = inv(self.I) # Inertia inverse 12 | self.T_max = 5 # max thrust [N] 13 | self.T_min = 0 # min thrust [N] 14 | self.omega_max = 3 # max bodyrate [rad/s] 15 | self.ctau = 0.5 # thrust torque coeff. 16 | self.rampup_dist = 0 17 | self.T_ramp_start = 5 18 | self.omega_ramp_start = 3 19 | 20 | self.v_max = None 21 | self.cd = 0.0 22 | 23 | self.g = 9.801 24 | 25 | if filename: 26 | self.load(filename) 27 | 28 | def load(self, filename): 29 | print("Loading track from " + filename) 30 | with open(filename, 'r') as file: 31 | quad = yaml.load(file, Loader=yaml.FullLoader) 32 | 33 | if 'mass' in quad: 34 | self.m = quad['mass'] 35 | else: 36 | print("No mass specified in " + filename) 37 | 38 | if 'arm_length' in quad: 39 | self.l = quad['arm_length'] 40 | else: 41 | print("No arm length specified in " + filename) 42 | 43 | if 'inertia' in quad: 44 | self.I = DM(quad['inertia']) 45 | self.I_inv = inv(self.I) 46 | else: 47 | print("No inertia specified in " + filename) 48 | 49 | 50 | if 'TWR_max' in quad: 51 | self.T_max = quad['TWR_max'] * 9.81 * self.m / 4 52 | elif 'thrust_max' in quad: 53 | self.T_max = quad['thrust_max'] 54 | else: 55 | print("No max thrust specified in " + filename) 56 | 57 | if 'TWR_min' in quad: 58 | self.T_min = quad['TWR_min'] * 9.81 * self.m / 4 59 | elif 'thrust_min' in quad: 60 | self.T_min = quad['thrust_min'] 61 | else: 62 | print("No min thrust specified in " + filename) 63 | 64 | if 'omega_max_xy' in quad: 65 | self.omega_max_xy = quad['omega_max_xy'] 66 | else: 67 | print("No max omega_xy specified in " + filename) 68 | 69 | if 'omega_max_z' in quad: 70 | self.omega_max_z = quad['omega_max_z'] 71 | else: 72 | print("No max omega_z specified in " + filename) 73 | 74 | if 'torque_coeff' in quad: 75 | self.ctau = quad['torque_coeff'] 76 | else: 77 | print("No thrust to drag coefficient specified in " + filename) 78 | 79 | if 'v_max' in quad: 80 | self.v_max = quad['v_max'] 81 | a_max = 4 * self.T_max / self.m 82 | a_hmax = sqrt(a_max**2 - self.g**2) 83 | self.cd = a_hmax / self.v_max 84 | if 'drag_coeff' in quad: 85 | self.cd = quad['drag_coeff'] 86 | 87 | if 'rampup_dist' in quad: 88 | self.rampup_dist = quad['rampup_dist'] 89 | if 'TWR_ramp_start' in quad and 'omega_ramp_start' in quad: 90 | self.T_ramp_start = min(quad['TWR_ramp_start'] * 9.81 * self.m / 4, self.T_max) 91 | self.omega_ramp_start = min(quad['omega_ramp_start'], self.omega_max_xy) 92 | else: 93 | print("No TWR_ramp_start or omega_ramp_start specified. Disabling rampup") 94 | rampup_dist = 0 95 | 96 | 97 | def dynamics(self): 98 | p = MX.sym('p', 3) 99 | v = MX.sym('v', 3) 100 | q = MX.sym('q', 4) 101 | w = MX.sym('w', 3) 102 | T = MX.sym('thrust', 4) 103 | 104 | x = vertcat(p, v, q, w) 105 | u = vertcat(T) 106 | 107 | g = DM([0, 0, -self.g]) 108 | 109 | x_dot = vertcat( 110 | v, 111 | rotate_quat(q, vertcat(0, 0, (T[0]+T[1]+T[2]+T[3])/self.m)) + g - v * self.cd, 112 | 0.5*quat_mult(q, vertcat(0, w)), 113 | mtimes(self.I_inv, vertcat( 114 | self.l*(T[0]-T[1]-T[2]+T[3]), 115 | self.l*(-T[0]-T[1]+T[2]+T[3]), 116 | self.ctau*(T[0]-T[1]+T[2]-T[3])) 117 | -cross(w,mtimes(self.I,w))) 118 | ) 119 | fx = Function('f', [x, u], [x_dot], ['x', 'u'], ['x_dot']) 120 | return fx 121 | -------------------------------------------------------------------------------- /src/quaternion.py: -------------------------------------------------------------------------------- 1 | from casadi import vertcat 2 | import numpy as np 3 | 4 | # For casadi 5 | # Quaternion Multiplication 6 | def quat_mult(q1,q2): 7 | ans = vertcat(q2[0,:] * q1[0,:] - q2[1,:] * q1[1,:] - q2[2,:] * q1[2,:] - q2[3,:] * q1[3,:], 8 | q2[0,:] * q1[1,:] + q2[1,:] * q1[0,:] - q2[2,:] * q1[3,:] + q2[3,:] * q1[2,:], 9 | q2[0,:] * q1[2,:] + q2[2,:] * q1[0,:] + q2[1,:] * q1[3,:] - q2[3,:] * q1[1,:], 10 | q2[0,:] * q1[3,:] - q2[1,:] * q1[2,:] + q2[2,:] * q1[1,:] + q2[3,:] * q1[0,:]) 11 | return ans 12 | 13 | # Quaternion-Vector Rotation 14 | def rotate_quat(q1,v1): 15 | ans = quat_mult(quat_mult(q1, vertcat(0, v1)), vertcat(q1[0,:],-q1[1,:], -q1[2,:], -q1[3,:])) 16 | return vertcat(ans[1,:], ans[2,:], ans[3,:]) # to covert to 3x1 vec 17 | 18 | # For Numpy 19 | def skew(v): 20 | return np.array([[0, -v[2], v[1]], 21 | [v[2], 0, -v[0]], 22 | [-v[1], v[0], 0]]) 23 | 24 | def conj(q): 25 | return np.array([q[0], -q[1], -q[2], -q[3]], ndmin=2).T 26 | 27 | def Ql(q): 28 | if q.ndim == 1: q = np.array(q, ndmin=2).T 29 | QL = np.zeros((4,4)) 30 | QL[0, 1:4] = -q[1:4].flatten() 31 | QL[1:4, 0] = q[1:4].flatten() 32 | QL[1:4, 1:4] = skew(q[1:4]) 33 | return q[0] * np.eye(4) + QL 34 | 35 | def Qr(q): 36 | if q.ndim == 1: q = np.array(q, ndmin=2).T 37 | QR = np.zeros((4,4)) 38 | QR[0, 1:4] = -q[1:4].flatten() 39 | QR[1:4, 0] = q[1:4].flatten() 40 | QR[1:4, 1:4] = skew(q[1:4]).T 41 | return q[0] * np.eye(4) + QR 42 | 43 | def qtimes(q1, q2): 44 | return np.matmul(Ql(q1), q2) 45 | 46 | def qRot(q): 47 | R = np.matmul(Ql(q), Qr(conj(q))) 48 | return R[1:4, 1:4] 49 | 50 | def qRotv(q, v): 51 | return np.matmul(qRot(q), v) 52 | 53 | def angleAxisToQuaternion(angle = None, axis = None): 54 | assert axis is not None 55 | anorm = np.linalg.norm(axis) 56 | if angle is None: 57 | angle = anorm 58 | ca2 = np.cos(angle/2.0) 59 | sa2 = np.sin(angle/2.0) 60 | dir = axis/anorm 61 | return np.array([ca2, sa2 * dir[0], sa2 * dir[1], sa2 * dir[2]], ndmin=2) 62 | 63 | def eulerToQuaternion(angle): 64 | qz = np.array([np.cos(angle[2]/2), 0, 0, np.sin(angle[2]/2)], ndmin=2).T 65 | qy = np.array([np.cos(angle[1]/2), 0, np.sin(angle[1]/2), 0], ndmin=2).T 66 | qx = np.array([np.cos(angle[0]/2), np.sin(angle[0]/2), 0, 0], ndmin=2).T 67 | 68 | return qtimes(qz, qtimes(qy, qx)) -------------------------------------------------------------------------------- /src/track.py: -------------------------------------------------------------------------------- 1 | import yaml 2 | 3 | class Track: 4 | def __init__(self, filename = ""): 5 | self.init_pos = None 6 | self.init_att = None 7 | self.init_vel = None 8 | self.init_omega = None 9 | self.end_pos = None 10 | self.end_att = None 11 | self.end_vel = None 12 | self.end_omega = None 13 | self.gates = [] 14 | self.ring = False 15 | if filename: 16 | self.load(filename) 17 | 18 | def addGate(self, gate): 19 | self.gates += [gate] 20 | print(self.gates) 21 | 22 | def load(self, filename): 23 | print("Loading track from " + filename) 24 | with open(filename, 'r') as file: 25 | track = yaml.load(file, Loader=yaml.FullLoader) 26 | 27 | if 'gates' in track: 28 | self.gates = track['gates'] 29 | else: 30 | print("No gates specified in " + filename) 31 | if 'initial' in track: 32 | initial = track['initial'] 33 | else: 34 | initial = track 35 | if 'position' in initial: 36 | self.init_pos = initial['position'] 37 | if 'attitude' in initial: 38 | self.init_att = initial['attitude'] 39 | if 'velocity' in initial: 40 | self.init_vel = initial['velocity'] 41 | if 'omega' in initial: 42 | self.init_omega = initial['omega'] 43 | if 'end' in track: 44 | end = track['end'] 45 | if 'position' in end: 46 | self.end_pos = end['position'] 47 | if 'attitude' in end: 48 | self.end_att = end['attitude'] 49 | if 'velocity' in end: 50 | self.end_vel = end['velocity'] 51 | if 'omega' in end: 52 | self.end_omega = end['omega'] 53 | if 'ring' in track: 54 | self.ring = track['ring'] -------------------------------------------------------------------------------- /src/trajectory.py: -------------------------------------------------------------------------------- 1 | import casadi as ca 2 | import numpy as np 3 | import csv 4 | import matplotlib.pyplot as plt 5 | from quaternion import rotate_quat 6 | from mpl_toolkits.mplot3d import Axes3D 7 | from quad import Quad 8 | import warnings 9 | 10 | # Plot and analyse trajectories 11 | class Trajectory: 12 | def __init__(self, x=None, NPW=None, wp = [], NW = 0): 13 | assert type(wp) == ca.MX or type(wp) == ca.DM or type(wp) == np.ndarray or type(wp)==list 14 | if x is not None: 15 | assert isinstance(x, (ca.MX, ca.DM, np.ndarray, list, str)) 16 | 17 | if x is None: 18 | self.NX = 0 19 | self.NU = 0 20 | self.x = [] 21 | self.NPW = 0 22 | return 23 | elif isinstance(x, (ca.MX, ca.DM)): 24 | assert x.shape[1] == 1 25 | self.x = x.full().flatten() 26 | elif isinstance(x, (np.ndarray, list)): 27 | self.x = x 28 | elif isinstance(x, str): 29 | self.load(x) 30 | self.parse() 31 | return 32 | else: 33 | raise Exception('Unknown state vector x passed.') 34 | 35 | if NPW is None: 36 | raise Exception('Number of nodes per waypoint (NPW) must be provided!') 37 | 38 | self.NX = 13 39 | self.NU = 4 40 | self.NPW = NPW 41 | 42 | if type(wp) == ca.MX or type(wp) == ca.DM: 43 | self.wp = wp 44 | self.NW = self.wp.shape[1] 45 | elif NW>0: 46 | self.NW = NW 47 | else: 48 | raise Exception('need valid waypointlist [wp] or number of waypoints [NW]!') 49 | 50 | self.N = self.NW * self.NPW 51 | 52 | self.parse() 53 | 54 | def parse(self): 55 | n_slice = self.NX + self.NU + 3 * self.NW 56 | n_start = 1 + self.NX + self.NW 57 | 58 | self.t_total = self.x[0] 59 | self.t_x = ca.DM(np.linspace(0, self.t_total, self.N+1, True)) 60 | self.t_u = self.t_x[0:self.N] 61 | 62 | idx = np.array([0, *list(range(n_start+self.NU-1, len(self.x), n_slice))]) 63 | 64 | self.p = np.array([ 65 | self.x[1+idx], 66 | self.x[2+idx], 67 | self.x[3+idx] 68 | ]) 69 | 70 | self.v = np.array([ 71 | self.x[4+idx], 72 | self.x[5+idx], 73 | self.x[6+idx] 74 | ]) 75 | 76 | self.q = np.array([ 77 | self.x[7+idx], 78 | self.x[8+idx], 79 | self.x[9+idx], 80 | self.x[10+idx] 81 | ]) 82 | 83 | self.w = np.array([ 84 | self.x[11+idx], 85 | self.x[12+idx], 86 | self.x[13+idx] 87 | ]) 88 | 89 | self.u = np.array([ 90 | self.x[n_start+0::n_slice], 91 | self.x[n_start+1::n_slice], 92 | self.x[n_start+2::n_slice], 93 | self.x[n_start+3::n_slice] 94 | ]) 95 | 96 | dt = self.t_total / self.N 97 | self.a_lin = np.zeros((3, self.N+1)) 98 | self.a_rot = np.zeros((3, self.N+1)) 99 | 100 | self.a_lin[:,0:-1] = np.diff(self.v) / dt 101 | self.a_rot[:,0:-1] = np.diff(self.w) / dt 102 | 103 | self.mu = np.zeros((self.NW, self.N+1)) 104 | self.nu = np.zeros((self.NW, self.N)) 105 | self.tau = np.zeros((self.NW, self.N)) 106 | for i in range(self.NW): 107 | self.mu[i,:] = self.x[1+self.NX+i::n_slice] 108 | self.nu[i,:] = self.x[n_start+self.NU+self.NX+i::n_slice] 109 | self.tau[i,:] = self.x[n_start+self.NU+self.NX+self.NW+i::n_slice] 110 | 111 | self.thrust = np.zeros((3,self.N)) 112 | self.dir = np.zeros((3,self.N)) 113 | for i in range(self.N): 114 | self.thrust[:,i] = np.array( 115 | rotate_quat(ca.DM(self.q[:,i]), ca.vertcat(0, 0, ca.cumsum(self.u[:,i]))))[:,0] 116 | self.dir[:,i] = self.thrust[:,i] / np.linalg.norm(self.thrust[:,i]) 117 | 118 | def unparse(self): 119 | n_slice = self.NX + self.NU + 3 * self.NW 120 | n_start = 1 + self.NX + self.NW 121 | 122 | self.x = np.zeros(n_start + self.N * n_slice) 123 | idx = np.array([0, *list(range(n_start+self.NU-1, len(self.x)-1, n_slice))]) 124 | 125 | self.x[0] = self.t_total 126 | self.x[1+idx] = self.p[0,:] 127 | self.x[2+idx] = self.p[1,:] 128 | self.x[3+idx] = self.p[2,:] 129 | 130 | self.x[4+idx] = self.v[0,:] 131 | self.x[5+idx] = self.v[1,:] 132 | self.x[6+idx] = self.v[2,:] 133 | 134 | self.x[7+idx] = self.q[0,:] 135 | self.x[8+idx] = self.q[1,:] 136 | self.x[9+idx] = self.q[2,:] 137 | self.x[10+idx] = self.q[3,:] 138 | 139 | self.x[11+idx] = self.w[0,:] 140 | self.x[12+idx] = self.w[1,:] 141 | self.x[13+idx] = self.w[2,:] 142 | 143 | self.x[n_start+0::n_slice] = self.u[0,:] 144 | self.x[n_start+1::n_slice] = self.u[1,:] 145 | self.x[n_start+2::n_slice] = self.u[2,:] 146 | self.x[n_start+3::n_slice] = self.u[3,:] 147 | 148 | 149 | def getAxesHandle(self, fig, plot3d=False): 150 | kwargs = {} 151 | if plot3d: 152 | kwargs = {'projection': '3d'} 153 | if fig is None: 154 | return plt.figure(**kwargs).gca() 155 | elif type(fig) == int or type(fig) == str: 156 | return plt.figure(fig, **kwargs).gca() 157 | elif type(fig) == plt.Figure: 158 | return fig.gca(**kwargs) 159 | elif type(fig) == plt.Axes or type(fig) == plt.Subplot: 160 | return fig 161 | else: 162 | raise Exception('Provided figure or axis handle is invalid') 163 | 164 | def getDataAxes(self, axes_str, cset="xyz"): 165 | assert type(cset) == str 166 | assert type(axes_str) == str 167 | axes_str = axes_str.lower() 168 | cset = cset.lower() 169 | axes = [] 170 | for i in range(len(axes_str)): 171 | axes += [cset.find(axes_str[i])] 172 | if axes[-1] < 0: 173 | raise Exception('Invalid axes %c specified' % axes_str[i]) 174 | 175 | return axes 176 | 177 | def plotWaypoints(self, wp, fig=None, plot_axis='xy', style='rx', **kwargs): 178 | if wp is None: wp = self.wp 179 | p = [] 180 | o = [] 181 | 182 | if type(wp) == list: 183 | if len(wp) > 0: 184 | wp = ca.DM(wp) 185 | else: 186 | return p 187 | if type(wp) == ca.DM: 188 | if wp.shape[0]!=3: 189 | if wp.shape[1]==3: 190 | wp = wp.T 191 | else: 192 | raise Exception('waypoints have incorect format') 193 | elif wp.shape[1]<1: 194 | return p 195 | 196 | data = self.getDataAxes(plot_axis) 197 | ax = self.getAxesHandle(fig, plot3d=(len(data)>2)) 198 | 199 | if len(data) == 2: 200 | p = ax.plot(wp[data[0],:], wp[data[1],:], style, **kwargs) 201 | o = ax.plot(self.p[data[0],0], self.p[data[1],0], '.k') 202 | elif len(data) == 3: 203 | p = ax.plot(wp[data[0],:], wp[data[1],:], wp[data[2],:], style, **kwargs) 204 | o = ax.plot(self.p[data[0],[0]], self.p[data[1],[0]], self.p[data[2],[0]], '.k') 205 | else: 206 | raise Exception('No valid data axes specified to plot') 207 | plt.draw() 208 | return [o, p] 209 | 210 | def plotPos(self, 211 | fig=None, title=None, plot_axis='xy', 212 | wp=None, wp_style='rx', arrow_nth=None, arrow_size=0.5, arrow_args=None, **kwargs): 213 | if 'color' not in kwargs: 214 | kwargs['color'] = 'b' 215 | 216 | if arrow_args is None: 217 | arrow_args = {} 218 | arrow_args['color'] = kwargs['color'] 219 | 220 | if not 'width' in arrow_args: 221 | arrow_args['width'] = 0.01 222 | arrow_args['zorder'] = 100 223 | 224 | if title is None: 225 | title = 'Position%' 226 | else: 227 | assert type(title) == str 228 | 229 | if arrow_nth is None: 230 | arrow_nth = int(self.N/10) 231 | 232 | title = title.replace('%', ' $t_{N}= %1.3fs$' % self.t_total) 233 | 234 | plot_ori = False 235 | if 'q' in plot_axis: 236 | plot_ori = True 237 | plot_axis = plot_axis.replace('q', '') 238 | 239 | data = self.getDataAxes(plot_axis) 240 | ax = self.getAxesHandle(fig, plot3d=(len(data)>2)) 241 | p = [] 242 | pdir = [] 243 | if len(data)==2: 244 | p += ax.plot(self.p[data[0],:], self.p[data[1],:], **kwargs) 245 | if plot_ori: 246 | idx = [i for i in range(0, self.N, arrow_nth)] 247 | px = [self.p[data[0], i] for i in idx] 248 | py = [self.p[data[1], i] for i in idx] 249 | dirx = [self.dir[data[0], i] for i in idx] 250 | diry = [self.dir[data[1], i] for i in idx] 251 | for i in range(len(idx)): 252 | pdir += [ax.arrow(px[i], py[i], arrow_size * dirx[i], arrow_size * diry[i], **arrow_args)] 253 | ax.set_xlabel('$p_%c$ $[m]$' % plot_axis[0]) 254 | ax.set_ylabel('$p_%c$ $[m]$' % plot_axis[1]) 255 | ax.axis('equal') 256 | elif len(data)==3: 257 | assert type(fig) == plt.Figure 258 | p += ax.plot(self.p[data[0],:], self.p[data[1],:], self.p[data[2],:], **kwargs) 259 | smax = np.max(self.p) 260 | smin = np.min(self.p) 261 | if plot_ori: 262 | idx = [i for i in range(0, self.N+1, arrow_nth)] 263 | dist = smax-smin 264 | px = [self.p[data[0], i] for i in idx] 265 | py = [self.p[data[1], i] for i in idx] 266 | pz = [self.p[data[2], i] for i in idx] 267 | dirx = [self.dir[data[0], i] for i in idx] 268 | diry = [self.dir[data[1], i] for i in idx] 269 | dirz = [self.dir[data[2], i] for i in idx] 270 | pdir += [ax.quiver(px, py, pz, dirx, diry, dirz)] 271 | # for i in range(len(idx)): 272 | # pdir += [ax.arrow(px[i], py[i], dirx[i], diry[i], width=0.003*dist, 273 | # ec='k', fc='k')] 274 | 275 | ax.set_xlabel('$p_%c$ $[m]$' % plot_axis[0]) 276 | ax.set_ylabel('$p_%c$ $[m]$' % plot_axis[1]) 277 | ax.set_zlabel('$p_%c$ $[m]$' % plot_axis[2]) 278 | ax.set_xlim(smin, smax) 279 | ax.set_ylim(smin, smax) 280 | ax.set_zlim(smin, smax) 281 | else: 282 | raise Exception('No valid axes specified to plot') 283 | 284 | wpp = self.plotWaypoints(wp, fig, plot_axis, wp_style, ms=5) 285 | if len(title) > 0: ax.set_title(title) 286 | plt.draw() 287 | if len(pdir)>0: p += pdir 288 | if len(wpp)>0: p += wpp 289 | return p 290 | 291 | 292 | def plotVel(self, fig=None, title='Velocity', plot_axis='xyza', **kwargs): 293 | ax = self.getAxesHandle(fig) 294 | plot_abs = False 295 | if 'a' in plot_axis: 296 | plot_abs = True 297 | plot_axis = plot_axis.replace('a', '') 298 | data = self.getDataAxes(plot_axis) 299 | 300 | if len(data)<1 and not plot_abs: return ax 301 | if len(data)>3: 302 | warnings.warn('Can only print 3 axes for velocity') 303 | 304 | p = [] 305 | for i in range(min(len(data), 3)): 306 | if not 'label' in kwargs: 307 | kwargs['label'] = '$v_%c$' % plot_axis[i] 308 | p += ax.plot(self.t_x, self.v[data[i]], **kwargs) 309 | 310 | if plot_abs: 311 | if not 'label' in kwargs: 312 | kwargs['label'] = '$\|v\|$' 313 | p += ax.plot(self.t_x, np.linalg.norm(self.v, axis=0), **kwargs) 314 | 315 | if len(title) > 0: 316 | ax.set_title(title) 317 | ax.legend(loc='right') 318 | ax.set_xlabel('$t$ $[s]$') 319 | ax.set_ylabel("$v$ $[m/s]$") 320 | plt.draw() 321 | return p 322 | 323 | def plotOmega(self, fig=None, title='Bodyrate', plot_axis='xyz', **kwargs): 324 | ax = self.getAxesHandle(fig) 325 | data = self.getDataAxes(plot_axis) 326 | 327 | if len(data)<1: return fig 328 | if len(data)>3: 329 | warnings.warn('Can only print 3 axes for bodyrate') 330 | 331 | p = [] 332 | lgnd = [] 333 | for i in range(min(len(data), 3)): 334 | p += ax.plot(self.t_x, self.w[data[i]], **kwargs) 335 | lgnd += ['$\omega_%c$' % plot_axis[i]] 336 | 337 | if len(title) > 0: 338 | ax.set_title(title) 339 | ax.legend(lgnd) 340 | ax.set_xlabel('$t$ $[s]$') 341 | ax.set_ylabel("$\omega$ $[rad/s]$") 342 | plt.draw() 343 | return p 344 | 345 | def plotOrientation(self, fig=None, title='Orientation', plot_axis='wxyz', **kwargs): 346 | ax = self.getAxesHandle(fig) 347 | plot_abs = False 348 | if 'a' in plot_axis: 349 | plot_abs = True 350 | plot_axis = plot_axis.replace('a', '') 351 | data = self.getDataAxes(plot_axis, 'wxyz') 352 | 353 | if len(data)<1: return fig 354 | if len(data)>4: 355 | warnings.warn('Can only print 4 axes for bodyrate') 356 | 357 | p = [] 358 | lgnd = [] 359 | for i in range(len(data)): 360 | p += ax.plot(self.t_x, self.q[data[i]], **kwargs) 361 | lgnd += ['$q_%c$' % plot_axis[i]] 362 | 363 | if plot_abs: 364 | p += ax.plot(self.t_x, np.linalg.norm(self.q, axis=0), **kwargs) 365 | lgnd += ['$\|q\|$'] 366 | 367 | if len(title) > 0: 368 | ax.set_title(title) 369 | ax.legend(lgnd) 370 | ax.set_xlabel('$t$ $[s]$') 371 | ax.set_ylabel("$q$ $[unit]$") 372 | plt.draw() 373 | return p 374 | 375 | def plotThrust(self, 376 | fig=None, title='Thrust', plot_axis='xyza', astyle='-', ustyle=None, **kwargs): 377 | if ustyle is None: ustyle = astyle 378 | 379 | ax = self.getAxesHandle(fig) 380 | plot_abs = False 381 | if 'a' in plot_axis: 382 | plot_abs = True 383 | plot_axis = plot_axis.replace('a', '') 384 | plot_thrusts = False 385 | if 'u' in plot_axis: 386 | plot_thrusts = True 387 | plot_axis = plot_axis.replace('u', '') 388 | data = self.getDataAxes(plot_axis, 'xyz') 389 | 390 | if len(data)>3: 391 | warnings.warn('Can only print 4 axes for directional force') 392 | 393 | p = [] 394 | lgnd = [] 395 | if plot_thrusts: 396 | for i in range(self.NU): 397 | p += ax.plot(self.t_u, self.u[i,:], ustyle, label='$u_%d$' % i, **kwargs) 398 | 399 | for i in range(min(len(data), 3)): 400 | p += ax.plot(self.t_u, self.m * self.a[data[i]], astyle, label='$f_%c$' % plot_axis[i], **kwargs) 401 | 402 | if plot_abs: 403 | if len(data) < 1: 404 | label = '$\|u\|$' 405 | else: 406 | label = '$\|f\|$' 407 | p += ax.plot(self.t_u, np.sum(self.u, axis=0), astyle, label=label, **kwargs) 408 | 409 | if len(title) > 0: 410 | ax.set_title(title) 411 | # ax.legend(lgnd) 412 | ax.set_xlabel('$t$ $[s]$') 413 | if plot_thrusts: 414 | ax.set_ylabel("$u$ $[N]$") 415 | else: 416 | ax.set_ylabel("$f$ $[N]$") 417 | plt.draw() 418 | return p 419 | 420 | def plotProgress(self, 421 | fig=None, title='Progress', plot_axis='mn', mstyle='-', nstyle='--', tstyle='.', **kwargs): 422 | 423 | lgnd = [] 424 | ax = self.getAxesHandle(fig) 425 | 426 | plot_mu = 'm' in plot_axis 427 | plot_nu = 'n' in plot_axis 428 | plot_tau = 't' in plot_axis 429 | 430 | all_labels = (plot_mu ^ plot_nu ^ plot_tau) and self.NW <=6 431 | 432 | p = [] 433 | for i in range(self.NW): 434 | prev = None 435 | if plot_mu: 436 | prev = ax.plot(self.t_x, self.mu[i,:], mstyle, **kwargs) 437 | if all_labels: 438 | lgnd +=['$\mu_%d$' % i] 439 | elif i==0: 440 | lgnd +=['$\mu$'] 441 | p += prev 442 | if plot_nu: 443 | if prev is None: 444 | prev = ax.plot(self.t_u, self.nu[i,:], nstyle, **kwargs) 445 | else: 446 | prev = ax.plot(self.t_u, self.nu[i,:], nstyle, color=prev[0].get_color(), **kwargs) 447 | if all_labels: 448 | lgnd +=['$\\nu_%d $' % i] 449 | elif i==0: 450 | lgnd +=['$\\nu $'] 451 | p += prev 452 | if plot_tau: 453 | if prev is None: 454 | prev = ax.plot(self.t_u, self.tau[i,:], tstyle, **kwargs) 455 | else: 456 | prev = ax.plot(self.t_u, self.tau[i,:], tstyle, color=prev[0].get_color(), **kwargs) 457 | if all_labels: 458 | lgnd +=['$\\tau_%d$' % i] 459 | elif i==0: 460 | lgnd +=['$\\tau$'] 461 | p += prev 462 | 463 | if len(title) > 0: 464 | ax.set_title(title) 465 | ax.legend(lgnd, loc='right') 466 | ax.set_xlabel('$t$ $[s]$') 467 | ax.set_ylabel("progress") 468 | plt.draw() 469 | return p 470 | 471 | def save(self, filename, readable=False): 472 | assert type(filename) == str 473 | with open(filename, 'w') as csvfile: 474 | traj_writer = csv.writer(csvfile, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL) 475 | if not readable: 476 | traj_writer.writerow([self.NW, self.NPW, self.NX, self.NU, self.N]) 477 | traj_writer.writerow(self.x) 478 | if type(self.wp) == np.ndarray: 479 | wp = self.wp.T.flatten() 480 | else: 481 | wp = self.wp.T.full().flatten() 482 | traj_writer.writerow(wp) 483 | if readable: 484 | labels = ['t', 'p_x', 'p_y', 'p_z', 485 | 'q_w', 'q_x', 'q_y', 'q_z', 486 | 'v_x', 'v_y', 'v_z', 487 | 'w_x', 'w_y', 'w_z', 488 | 'a_lin_x', 'a_lin_y', 'a_lin_z', 489 | 'a_rot_x', 'a_rot_y', 'a_rot_z', 490 | 'u_1', 'u_2', 'u_3', 'u_4'] 491 | for i in range(self.NW): 492 | labels += ['mu_' + str(i)] 493 | labels += ['nu_' + str(i)] 494 | labels += ['tau_' + str(i)] 495 | 496 | traj_writer.writerow(labels) 497 | for i in range(self.N+1): 498 | # States 499 | row = [self.t_x[i], 500 | self.p[0,i], self.p[1,i], self.p[2,i], 501 | self.q[0,i], self.q[1,i], self.q[2,i], self.q[3,i], 502 | self.v[0,i], self.v[1,i], self.v[2,i], 503 | self.w[0,i], self.w[1,i], self.w[2,i], 504 | self.a_lin[0, i], self.a_lin[1, i], self.a_lin[2, i], 505 | self.a_rot[0, i], self.a_rot[1, i], self.a_rot[2, i]] 506 | 507 | # Inputs 508 | if i0: 518 | row += [self.nu[j,i-1]] 519 | row += [self.tau[j,i-1]] 520 | else: 521 | row += [0]*2 522 | 523 | traj_writer.writerow(row) 524 | 525 | def load(self, filename): 526 | assert type(filename) == str 527 | with open(filename, 'r') as csvfile: 528 | traj_reader = csv.reader(csvfile, delimiter=',', quotechar='"', quoting=csv.QUOTE_NONNUMERIC) 529 | params = next(traj_reader) 530 | [self.NW, self.NPW, self.NX, self.NU, self.N] = params[:5] 531 | if len(params)>5: 532 | self.m = params[5] 533 | self.NW = int(self.NW) 534 | self.NPW = int(self.NPW) 535 | self.NX = int(self.NX) 536 | self.NU = int(self.NU) 537 | self.N = int(self.N) 538 | self.x = np.array(next(traj_reader)) 539 | wp = np.array(next(traj_reader)) 540 | self.wp = wp.reshape(self.NW, 3).T 541 | 542 | self.parse() 543 | return self 544 | 545 | -------------------------------------------------------------------------------- /src/trajectory_conversion.py: -------------------------------------------------------------------------------- 1 | from trajectory import Trajectory 2 | from quaternion import * 3 | from pandas import read_csv 4 | 5 | def viconToTrajectory(filename, interest=None, dt=0.005, wp=None): 6 | data = read_csv(filename) 7 | keys = data.keys() 8 | t = a = w = None 9 | if 'ts' in keys: 10 | t = data['ts'].to_numpy().T 11 | else: 12 | t = dt * np.arange(len(data)) 13 | 14 | p = data[['TX', 'TY', 'TZ']].to_numpy().T 15 | r = data[['RX', 'RY', 'RZ']].to_numpy().T 16 | if ' accSmooth[0] (m/s/s)' in keys: 17 | a = data[[' accSmooth[0] (m/s/s)', ' accSmooth[1] (m/s/s)', ' accSmooth[2] (m/s/s)']].to_numpy().T 18 | if ' gyroADC[0] (deg/s)' in keys: 19 | w = data[[' gyroADC[0] (deg/s)', ' gyroADC[1] (deg/s)', ' gyroADC[2] (deg/s)']].to_numpy().T 20 | 21 | if interest is not None: 22 | p = p[:,interest[0]:interest[1]] 23 | r = r[:,interest[0]:interest[1]] 24 | if a is not None: a = a[:,interest[0]:interest[1]] 25 | if w is not None: w = w[:,interest[0]:interest[1]] 26 | if t is not None: t = t[interest[0]:interest[1]] 27 | 28 | ## Selecting segment 29 | if interest is not None: 30 | idx = ~(np.isnan(p).any(axis=0) | np.isnan(r).any(axis=0)) 31 | t = t[idx] 32 | p = p[:, idx] 33 | r = r[:, idx] 34 | if a is not None: a = a[:, idx] 35 | if w is not None: w = w[:, idx] 36 | 37 | n = len(t) 38 | 39 | # Polishing Data 40 | if (p>100.0).any(): p *= 1e-3 41 | q = np.zeros((4, n)) 42 | for i in range(n): 43 | q[:,i] = angleAxisToQuaternion(axis=r[:,i]).flatten() 44 | 45 | wind = 11 46 | k = np.ones(wind, 'd') 47 | k /= np.linalg.norm(k) 48 | dt = np.diff(t) 49 | 50 | pf = np.apply_along_axis(lambda m: np.convolve(m, k, mode='same'), axis=1, arr=p) 51 | v = np.zeros((3, n)) 52 | for i in range(n-1): 53 | v[:,i] = (pf[:,i+1] - pf[:,i]) / dt[i] 54 | inv_wind = int(wind-1) 55 | v[:,0:inv_wind] = np.repeat(v[:,[inv_wind]], inv_wind, axis=1) 56 | v[:,n-inv_wind:n] = np.repeat(v[:,[-1-inv_wind]], inv_wind, axis=1) 57 | 58 | 59 | if a is None: 60 | a = np.diff(v) 61 | for i in range(n-1): 62 | a[:,i] /= dt[i] 63 | a = np.hstack((a, a[:,[-1]])) 64 | 65 | n -= 1-(n%2) 66 | traj = Trajectory() 67 | traj.N = n-1 68 | traj.NX = 13 69 | traj.NU = 4 70 | if wp is not None: 71 | traj.NW = wp.shape[1] 72 | else: 73 | traj.NW = 1 74 | traj.NPW = int(n/traj.NW) 75 | if wp is not None: 76 | traj.wp = wp 77 | else: 78 | traj.wp = p[:,[-1]] 79 | 80 | traj.t_x = t[:n] - t[0] 81 | traj.t_u = t[:n-1] - t[0] 82 | traj.t_total = traj.t_x[-1] - traj.t_x[0] 83 | traj.p = p[:,:n] 84 | traj.v = v[:,:n] 85 | traj.q = q[:,:n] 86 | traj.a = a[:,:n] 87 | if w is not None: traj.w = w[:,:n] 88 | traj.dir = np.zeros((3, n)) 89 | for i in range(n): 90 | traj.dir[:,i] = qRotv(traj.q[:,i], np.array([0, 0, 1])) 91 | traj.u = np.zeros((4,n-1)) 92 | traj.mu = np.zeros((traj.NW, n)) 93 | traj.nu = np.zeros((traj.NW, n)) 94 | traj.tau = np.zeros((traj.NW, n-1)) 95 | return traj 96 | 97 | -------------------------------------------------------------------------------- /src/utils.py: -------------------------------------------------------------------------------- 1 | import casadi as ca 2 | from trajectory import Trajectory 3 | 4 | def progressBar (iteration, total, prefix = '', suffix = '', decimals = 1, length = 100, fill = '█', printEnd = "\r"): 5 | percent = ("{0:." + str(decimals) + "f}").format(100 * (iteration / float(total))) 6 | filledLength = int(length * iteration // total) 7 | bar = fill * filledLength + '-' * (length - filledLength) 8 | print('\r%s |%s| %s%% %s' % (prefix, bar, percent, suffix), end = printEnd) 9 | if iteration == total: print() 10 | 11 | class CallbackSaver(ca.Callback): 12 | def __init__(self, folderpath, opts={}): 13 | ca.Callback.__init__(self) 14 | 15 | self.nx = None 16 | self.ng = None 17 | self.NPW = None 18 | self.opts = opts 19 | 20 | assert type(folderpath)==str 21 | self.folderpath = folderpath 22 | self.i = 0 23 | 24 | def set_size(self, nx, ng, NPW): 25 | self.nx = nx 26 | self.ng = ng 27 | self.NPW = NPW 28 | self.construct('CallbackSaver', self.opts) 29 | 30 | def set_wp(self, wp): 31 | self.wp = wp 32 | 33 | def get_n_in(self): return ca.nlpsol_n_out() 34 | def get_n_out(self): return 1 35 | def get_name_in(self, i): return ca.nlpsol_out(i) 36 | def get_name_out(self, i): return "ret" 37 | 38 | def get_sparsity_in(self, i): 39 | n = ca.nlpsol_out(i) 40 | if n=='f': 41 | return ca.Sparsity.scalar() 42 | elif n in ('x', 'lam_x'): 43 | return ca.Sparsity.dense(self.nx) 44 | elif n in ('g', 'lam_g'): 45 | return ca.Sparsity.dense(self.ng) 46 | else: 47 | return ca.Sparsity(0,0) 48 | 49 | def eval(self, arg): 50 | # Create dictionary 51 | darg = {} 52 | for (i,s) in enumerate(ca.nlpsol_out()): darg[s] = arg[i] 53 | 54 | X_opt = darg['x'].full().flatten() 55 | traj = Trajectory(X_opt, NPW=self.NPW, wp=self.wp) 56 | 57 | traj.save(self.folderpath+'/iteration_%05d.csv' % self.i) 58 | 59 | self.i += 1 60 | return [0] -------------------------------------------------------------------------------- /tracks/track.yaml: -------------------------------------------------------------------------------- 1 | gates: [[-1.1, -1.6, 3.6], 2 | [9.2, 6.6, 1.0], 3 | [9.2, -4.0, 1.2], 4 | [-4.5, -6.0, 3.5], 5 | [-4.5, -6.0, 0.8], 6 | [4.75, -0.9, 1.2], 7 | [-2.8, 6.8, 1.2], 8 | [-1.1, -1.6, 3.6], 9 | [9.2, 6.6, 1.0], 10 | [9.2, -4.0, 1.2], 11 | [-4.5, -6.0, 3.5], 12 | [-4.5, -6.0, 0.8], 13 | [4.75, -0.9, 1.2], 14 | [-2.8, 6.8, 1.2], 15 | [-1.1, -1.6, 3.6], 16 | [9.2, 6.6, 1.0], 17 | [9.2, -4.0, 1.2], 18 | [-4.5, -6.0, 3.5], 19 | [-4.5, -6.0, 0.8]] 20 | initial: 21 | position: [-5, 4.5, 1.2] 22 | attitude: [1, 0, 0, 0] 23 | velocity: [0, 0, 0] 24 | omega: [0, 0, 0] 25 | 26 | end: 27 | position: [4.75, -0.9, 1.2] 28 | --------------------------------------------------------------------------------