diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..7cad6bf --- /dev/null +++ b/.clang-format @@ -0,0 +1,87 @@ +--- +Language: Cpp +# BasedOnStyle: Google +AccessModifierOffset: -2 +AlignAfterOpenBracket: Align +AlignConsecutiveAssignments: true +AlignConsecutiveDeclarations: false +AlignEscapedNewlinesLeft: true +AlignOperands: true +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: true +AllowShortBlocksOnASingleLine: false +AllowShortCaseLabelsOnASingleLine: false +AllowShortFunctionsOnASingleLine: None +AllowShortIfStatementsOnASingleLine: true +AllowShortLoopsOnASingleLine: true +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakBeforeMultilineStrings: true +AlwaysBreakTemplateDeclarations: true +BinPackArguments: false +BinPackParameters: false +BraceWrapping: + AfterClass: false + AfterControlStatement: false + AfterEnum: false + AfterFunction: false + AfterNamespace: false + AfterObjCDeclaration: false + AfterStruct: false + AfterUnion: false + BeforeCatch: false + BeforeElse: false + IndentBraces: false +BreakBeforeBinaryOperators: None +BreakBeforeBraces: Attach +BreakBeforeTernaryOperators: true +BreakConstructorInitializersBeforeComma: false +ColumnLimit: 100 +CommentPragmas: '(.+\n*.+)*' +ConstructorInitializerAllOnOneLineOrOnePerLine: true +ConstructorInitializerIndentWidth: 4 +ContinuationIndentWidth: 4 +Cpp11BracedListStyle: true +DerivePointerAlignment: false +DisableFormat: false +ExperimentalAutoDetectBinPacking: false +ForEachMacros: [ foreach, Q_FOREACH, BOOST_FOREACH ] +IncludeCategories: + - Regex: '^<.*\.h>' + Priority: 2 + - Regex: '^<.*' + Priority: 1 + - Regex: '.*' + Priority: 3 +IndentCaseLabels: true +IndentWidth: 4 +IndentWrappedFunctionNames: false +KeepEmptyLinesAtTheStartOfBlocks: true +MacroBlockBegin: '' +MacroBlockEnd: '' +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCBlockIndentWidth: 2 +ObjCSpaceAfterProperty: false +ObjCSpaceBeforeProtocolList: false +PenaltyBreakBeforeFirstCallParameter: 1 +PenaltyBreakComment: 300 +PenaltyBreakFirstLessLess: 120 +PenaltyBreakString: 1000 +PenaltyExcessCharacter: 1000000 +PenaltyReturnTypeOnItsOwnLine: 200 +SortIncludes: true +PointerAlignment: Left +SpaceAfterCStyleCast: false +SpaceBeforeAssignmentOperators: true +SpaceBeforeParens: ControlStatements +SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 2 +SpacesInAngles: false +SpacesInContainerLiterals: true +SpacesInCStyleCastParentheses: false +SpacesInParentheses: false +SpacesInSquareBrackets: false +Standard: Auto +TabWidth: 4 +UseTab: Never +... diff --git a/.github/workflows/ungar-ci.yml b/.github/workflows/ungar-ci.yml new file mode 100644 index 0000000..2c51851 --- /dev/null +++ b/.github/workflows/ungar-ci.yml @@ -0,0 +1,45 @@ +name: CI + +on: + push: + pull_request: + +env: + BUILD_TYPE: Release + +jobs: + ci: + strategy: + fail-fast: false + matrix: + os: [ ubuntu-latest ] + compiler: [ gcc-11, gcc-13 ] + + runs-on: ${{ matrix.os }} + steps: + - name: Check out Ungar + uses: actions/checkout@v3 + + - name: Install compiler + id: install_compiler + uses: rlalik/setup-cpp-compiler@v1.2 + with: + compiler: ${{ matrix.compiler }} + + - name: Configure CMake + run: cmake -DUNGAR_BUILD_TESTS=ON + -DUNGAR_BUILD_EXAMPLES=ON + -DUNGAR_ENABLE_LOGGING=ON + -DUNGAR_ENABLE_AUTODIFF=ON + -DUNGAR_ENABLE_OPTIMIZATION=ON + -B ${{github.workspace}}/build + -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} + -DCMAKE_C_COMPILER=${{ steps.install_compiler.outputs.cc }} + -DCMAKE_CXX_COMPILER=${{ steps.install_compiler.outputs.cxx }} + + - name: Build + run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} + + - name: Test + working-directory: ${{github.workspace}}/build + run: ctest -C ${{env.BUILD_TYPE}} diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..8dc14ae --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +/build/ +/.cache/ diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..6cec042 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,174 @@ +cmake_minimum_required(VERSION 3.20) + +project(Ungar VERSION 1.0.1) + +add_library(ungar INTERFACE) +add_library(ungar::ungar ALIAS ungar) +target_compile_features(ungar INTERFACE cxx_std_20) + +# ############################################################################## +# Set up CMake options. +# ############################################################################## +include(CMakeDependentOption) + +option( + UNGAR_USE_SYSTEM_LIBRARIES + "Prevent automatic download of missing external dependencies. To selectively use system-wide packages, provide their installation prefixes as CMake variables of the form '_ROOT', where is the (case-preserved) name given to the find_package() call and _ROOT is literal." + OFF) +option(UNGAR_BUILD_TESTS "Add targets for unit testing." OFF) +option(UNGAR_BUILD_EXAMPLES "Add targets showing Ungar's features." OFF) +option(UNGAR_INSTALL "Install CMake targets during install step." OFF) +option(UNGAR_ENABLE_OPTIMIZATION + "Enable Ungar's nonlinear programming interface." OFF) +cmake_dependent_option( + UNGAR_ENABLE_LOGGING + "Enable Ungar's logging functionalities. Automatically enabled if any of the flags 'UNGAR_ENABLE_AUTODIFF', 'UNGAR_ENABLE_OPTIMIZATION', and 'UNGAR_BUILD_EXAMPLES' is set to 'ON'." + OFF + "NOT UNGAR_ENABLE_AUTODIFF;NOT UNGAR_ENABLE_OPTIMIZATION;NOT UNGAR_BUILD_EXAMPLES" + ON) +cmake_dependent_option( + UNGAR_ENABLE_AUTODIFF + "Enable Ungar's autodiff interface. Automatically enabled if 'UNGAR_ENABLE_OPTIMIZATION' is set to 'ON'." + OFF + "NOT UNGAR_ENABLE_OPTIMIZATION" + ON) + +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +message(STATUS "--------------------------------------------------") +message(STATUS "Configuring Ungar with the following options:") +message(STATUS " Enable logging : ${UNGAR_ENABLE_LOGGING}") +message(STATUS " Enable autodiff : ${UNGAR_ENABLE_AUTODIFF}") +message(STATUS " Enable optimization : ${UNGAR_ENABLE_OPTIMIZATION}") +message(STATUS " Build examples : ${UNGAR_BUILD_EXAMPLES}") +message(STATUS " Build tests : ${UNGAR_BUILD_TESTS}") +message(STATUS " Install : ${UNGAR_INSTALL}") +message(STATUS " Use system libraries : ${UNGAR_USE_SYSTEM_LIBRARIES}") + +# Add external dependencies. +set_property(GLOBAL PROPERTY CTEST_TARGETS_ADDED 1) +add_subdirectory(external) +message(STATUS "--------------------------------------------------") + +# ############################################################################## +# Add compile options. +# ############################################################################## +if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang") # Clang. + target_compile_options(ungar INTERFACE "-pedantic" "$<$:-O2>") +elseif(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") # GNU Compiler Collection (GCC). + target_compile_options( + ungar + INTERFACE "-pedantic" + "-ftemplate-backtrace-limit=1" + "-fconstexpr-depth=2147483647" + "-fconstexpr-loop-limit=2147483647" + "-fconstexpr-cache-depth=2147483647" + "-fconstexpr-ops-limit=2147483647" + "$<$:-O3>") +elseif(CMAKE_CXX_COMPILER_ID STREQUAL "Intel") # Intel C++ Compiler. + message(WARNING "Intel C++ Compiler is not supported.") +elseif(CMAKE_CXX_COMPILER_ID STREQUAL "MSVC") # MSVC. + target_compile_options(ungar INTERFACE "/W4" "$<$:/O2>") +else() + message(WARNING "${CMAKE_CXX_COMPILER_ID} is not supported.") +endif() + +# ############################################################################## +# Set Ungar's include directories and link libraries. +# ############################################################################## +# Core modules. +list(APPEND UNGAR_INCLUDE_DIRECTORIES + $ + $) +list(APPEND UNGAR_LINK_LIBRARIES Eigen3::Eigen hana) + +# Optional modules. +if(UNGAR_ENABLE_LOGGING) + list(APPEND UNGAR_LINK_LIBRARIES spdlog::spdlog) +endif() +if(UNGAR_ENABLE_AUTODIFF) + list(APPEND UNGAR_LINK_LIBRARIES CppAD::CppAD CppADCodeGen::CppADCodeGen + ${CMAKE_DL_LIBS} finitediff::finitediff) +endif() +if(UNGAR_ENABLE_OPTIMIZATION) + list(APPEND UNGAR_LINK_LIBRARIES osqp-cpp::osqp-cpp) +endif() + +target_include_directories(ungar INTERFACE ${UNGAR_INCLUDE_DIRECTORIES}) +target_link_libraries(ungar INTERFACE ${UNGAR_LINK_LIBRARIES}) + +# ############################################################################## +# Set Ungar's compile definitions. +# ############################################################################## +target_compile_definitions( + ungar + INTERFACE $<$:_GLIBCXX_ASSERTIONS> + $<$:_GLIBCXX_ASSERTIONS> + $<$:UNGAR_CONFIG_ENABLE_RELEASE_MODE> + $<$:UNGAR_CONFIG_ENABLE_RELEASE_MODE>) + +if(UNGAR_ENABLE_LOGGING) + target_compile_definitions(ungar INTERFACE -DUNGAR_CONFIG_ENABLE_LOGGING) +endif() + +if(UNGAR_ENABLE_AUTODIFF) + target_compile_definitions( + ungar + INTERFACE -DUNGAR_CONFIG_ENABLE_AUTODIFF + INTERFACE "UNGAR_CODEGEN_FOLDER=\"${CMAKE_BINARY_DIR}/ungar_codegen\"") +endif() + +if(UNGAR_ENABLE_OPTIMIZATION) + target_compile_definitions(ungar INTERFACE -DUNGAR_CONFIG_ENABLE_OPTIMIZATION) +endif() + +# ############################################################################## +# Add optional targets. +# ############################################################################## +if(UNGAR_BUILD_TESTS) + enable_testing() + add_subdirectory(test) +endif() + +if(UNGAR_BUILD_EXAMPLES) + add_subdirectory(example) +endif() + +if(UNGAR_INSTALL) + include(GNUInstallDirs) + + # Install Ungar headers. + install(DIRECTORY include/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) + + # Install Ungar binaries. + install( + TARGETS ungar + EXPORT ${PROJECT_NAME}Targets + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + INCLUDES + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) + + # Install Ungar configuration files. + include(CMakePackageConfigHelpers) + + configure_package_config_file( + UngarConfig.cmake.in ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake + INSTALL_DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/ungar) + + write_basic_package_version_file( + ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake + VERSION ${PACKAGE_VERSION} + COMPATIBILITY SameMajorVersion) + install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake + ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake + DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/ungar) + + # Perform installation. + install( + EXPORT ${PROJECT_NAME}Targets + FILE ${PROJECT_NAME}Targets.cmake + NAMESPACE ungar:: + DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/ungar) +endif(UNGAR_INSTALL) diff --git a/LICENSE.md b/LICENSE.md new file mode 100644 index 0000000..261eeb9 --- /dev/null +++ b/LICENSE.md @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/README.md b/README.md new file mode 100644 index 0000000..4e4690d --- /dev/null +++ b/README.md @@ -0,0 +1,152 @@ +# Ungar +[![Language](https://img.shields.io/badge/c%2B%2B-20-blue.svg)](https://en.wikipedia.org/wiki/C%2B%2B#Standardization) +[![Language](https://img.shields.io/badge/c%2B%2B-17-orange.svg)](https://en.wikipedia.org/wiki/C%2B%2B#Standardization) +[![License](https://img.shields.io/badge/License-Apache_2.0-blue.svg)](https://opensource.org/licenses/Apache-2.0) +[![CI](https://github.com/fdevinc/ungar/actions/workflows/ungar-ci.yml/badge.svg)](https://github.com/fdevinc/ungar/actions/workflows/ungar-ci.yml) + +> Simplifying optimal control with metaprogramming +## Overview +```cpp +#include "ungar/variable_map.hpp" +using namespace Ungar; + +int main() { + /*************** QUADROTOR MODEL ***************/ + // Define numeric invariants as integral constants. + constexpr auto N = 30_c; // Discrete time horizon. + constexpr auto NUM_ROTORS = 4_c; + + // Define variables. + UNGAR_VARIABLE(position, 3); // := p + UNGAR_VARIABLE(orientation, Q); // := q + UNGAR_VARIABLE(linear_velocity, 3); // := pDot + UNGAR_VARIABLE(angular_velocity, 3); // := omega + UNGAR_VARIABLE(x) <<= + (position, orientation, linear_velocity, angular_velocity); // x := [p q pDot omega] + UNGAR_VARIABLE(X) <<= (N + 1_c) * x; // X := [x0 x1 ... xN] + + UNGAR_VARIABLE(rotor_speed, 1); // := r + UNGAR_VARIABLE(u) <<= NUM_ROTORS * rotor_speed; // u := [r0 r1 r2 r3] + UNGAR_VARIABLE(U) <<= N * u; // U := [u0 u1 ... uN-1] + + UNGAR_VARIABLE(variables) <<= (X, U); + + // Access information about variables at compile time. + static_assert(x.Size() == 13); + static_assert(variables(x, 0).Index() == 0); // [{x0} x1 x2 ... xN u0 ... ] + static_assert(variables(x, 1).Index() == 13); // [ x0 {x1} x2 ... xN u0 ... ] + static_assert(variables(x, 2).Index() == 26); // [ x0 x1 {x2} ... xN u0 ... ] + static_assert(variables(u, 0).Index() == X.Size()); // [ x0 x1 x2 ... xN {u0} ... ] + + // Create maps over contiguous arrays of data. + auto vars = MakeVariableMap(variables); // [ X u0 u1 ... uN-1 ] + vars.Get(u, 0).setZero(); // [ X 0000 u1 ... uN-1 ] + vars.Get(u, 1).setOnes(); // [ X 0000 1111 ... uN-1 ] + vars.Get(u, N - 1).setLinSpaced(2.0, 8.0); // [ X 0000 1111 ... 2468 ] + + static_assert(std::same_as); + static_assert(std::same_as&>); + static_assert(std::same_as&>); + static_assert(std::same_as>&>); +} + +``` + +### Getting Started +To get started with Ungar, explore the `example/mpc` folder, which contains thoroughly documented code for three nonlinear model predictive control (MPC) implementations: one for a [quadrotor](example/mpc/quadrotor.example.cpp), another for a [radio-controlled miniature car](example/mpc/rc_car.example.cpp), and the third for a [quadruped robot](example/mpc/quadruped.example.cpp). These examples will help you quickly grasp how to utilize the library for your own control systems. + +## Using Ungar +Ungar is designed to be easily integrated in C++ projects using [CMake][]. Since it is a C++20 library, it requires compilers with C++20 support, such as [GCC 11][], [Clang 16][], [MSVC 19][], etc. For users who need C++17 support, a dedicated version of Ungar is available on the `cxx17` branch, which offers all the major features of the main version. + +> :warning: **[GCC 12](https://gcc.gnu.org/gcc-12/) is not supported!** When attempting to create Ungar's variables, you may encounter [internal compiler errors (ICEs)](https://stackoverflow.com/a/12405680). Fortunately, this issue is absent in both [GCC 11](https://gcc.gnu.org/gcc-11/) and [GCC 13](https://gcc.gnu.org/gcc-13/), making them suitable alternatives. + +### FetchContent +The recommended way to use Ungar is through [CMake][]'s [FetchContent][] module. To use Ungar in your CMake-based project, add the following lines to your project's _CMakeLists.txt_ file: +```CMake +include(FetchContent) + +message(STATUS "Fetching ungar...") +FetchContent_Declare( + ungar + GIT_REPOSITORY https://github.com/fdevinc/ungar.git + GIT_TAG main # For C++17: cxx17 + GIT_SHALLOW TRUE + GIT_PROGRESS TRUE) + +# Configure Ungar build options (all options are OFF by default). +set(UNGAR_BUILD_TESTS OFF) # Turn ON to build Ungar's tests. +set(UNGAR_BUILD_EXAMPLES ON) # Turn OFF to not build Ungar's examples. +set(UNGAR_ENABLE_LOGGING ON) # Turn OFF to disable logging in Ungar. +set(UNGAR_ENABLE_AUTODIFF ON) # Not supported on Windows. +set(UNGAR_ENABLE_OPTIMIZATION ON) # Not supported on Windows. +FetchContent_MakeAvailable(ungar) +``` +The above lines automatically handle the download of all the required dependencies if the corresponding targets are not defined. This simplifies the integration process and ensures that your project has everything it needs to use Ungar seamlessly. + +Once you have added Ungar to your [CMake][] project using [FetchContent][], you can link your project to Ungar by adding the following line to your _CMakeLists.txt_: +```CMake +target_link_libraries(PROJECT ungar::ungar) +``` + +### Embedded +Alternatively, you can directly embed the library into an existing [CMake][] project. To this end, copy and paste the entire source tree in a subdirectory and call `add_subdirectory` in your _CMakeLists.txt_ file: +```CMake +add_subdirectory(ungar) +... +target_link_libraries(PROJECT ungar::ungar) +``` +Once again, make sure to configure the various Ungar's build options before adding its directory. + +## Project Organization +The project is organized in the following subdirectories: +- The [`example`](example) directory contains various examples showing the basic functionalities of the library. +- The [`external`](external) directory contains all the third-party libraries. [Eigen][] and + [Boost.Hana][] are the only required dependencies: if their targets are not found, Ungar will use a bundled version + of each. All remaining dependencies are optional and bring logging, automatic differentiation, and nonlinear optimization functionalities to the + library. If enabled, they are downloaded using [CMake][]. +- The [`include`](include) directory contains the library itself. The optional modules are located in the [`io`](include/ungar/io), + [`autodiff`](include/ungar/autodiff), and [`optimization`](include/ungar/optimization) subfolders. +- The [`test`](test) directory contains the source code of all the unit tests. + +## Development Status +Ungar is a project meant for long-term maintenance. While the core API of the library is stable, the optional modules will likely see breaking changes +over time. The current implementations will be enhanced to achieve faster compile times. Finally, new algorithms and types will be introduced to +make the creation of optimal control problems as straightforward as possible. + +## Citing Ungar +If you are using Ungar for your work, we encourage you to cite the [related paper](https://arxiv.org/abs/2309.06783): +``` +@INPROCEEDINGS{DeVincenti-IROS-23, + author={De Vincenti, Flavio and Coros, Stelian}, + booktitle={2023 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, + title={Ungar - A C++ Framework for Real-Time Optimal Control Using Template Metaprogramming}, + year={2023}, + volume={}, + number={}, + pages={6297-6303}, + doi={10.1109/IROS55552.2023.10341365}} +``` +Ungar was used to implement the centralized MPC controller presented in the [following publication](https://www.roboticsproceedings.org/rss19/p050.html): +``` +@INPROCEEDINGS{DeVincenti-RSS-23, + AUTHOR = {Flavio De Vincenti AND Stelian Coros}, + TITLE = {{Centralized Model Predictive Control for Collaborative Loco-Manipulation}}, + BOOKTITLE = {Proceedings of Robotics: Science and Systems}, + YEAR = {2023}, + ADDRESS = {Daegu, Republic of Korea}, + MONTH = {July}, + DOI = {10.15607/RSS.2023.XIX.050} +} +``` + +## License +Please see [LICENSE.md](LICENSE.md). + + +[Eigen]: https://eigen.tuxfamily.org/index.php?title=Main_Page +[Boost.Hana]: https://github.com/boostorg/hana +[CMake]: http://www.cmake.org +[FetchContent]: https://cmake.org/cmake/help/latest/module/FetchContent.html +[GCC 11]: https://gcc.gnu.org/ +[Clang 16]: https://clang.llvm.org/ +[MSVC 19]: https://visualstudio.microsoft.com/vs/features/cplusplus/ diff --git a/UngarConfig.cmake.in b/UngarConfig.cmake.in new file mode 100644 index 0000000..ed31ec8 --- /dev/null +++ b/UngarConfig.cmake.in @@ -0,0 +1,57 @@ +include(CMakeFindDependencyMacro) + +@PACKAGE_INIT@ + +# ############################################################################## +# Core modules. +# ############################################################################## +# Eigen. +find_dependency(Eigen3 3.4.0 REQUIRED NO_MODULE) +# Hana. +find_dependency(Hana REQUIRED) + +# ############################################################################## +# Optional modules. +# ############################################################################## +if(@UNGAR_ENABLE_LOGGING@) + # spdlog. + find_dependency(spdlog REQUIRED) +endif() + +if(@UNGAR_ENABLE_AUTODIFF@) + # FiniteDiff. + find_dependency(FiniteDiff REQUIRED) + + # CppAD. + find_library(CppAD_LIBRARY cppad_lib REQUIRED) + find_path(CppAD_INCLUDE_DIR cppad/cppad.hpp REQUIRED) + + add_library(CppAD SHARED IMPORTED GLOBAL) + add_library(CppAD::CppAD ALIAS CppAD) + target_include_directories(CppAD INTERFACE ${CppAD_INCLUDE_DIR}) + + get_filename_component(CppAD_LIBRARY_NAME ${CppAD_LIBRARY} NAME) + get_filename_component(CppAD_LIBRARY_REAL_PATH ${CppAD_LIBRARY} REALPATH) + + set_target_properties(CppAD PROPERTIES IMPORTED_SONAME ${CppAD_LIBRARY_NAME}) + set_target_properties(CppAD PROPERTIES IMPORTED_LOCATION + ${CppAD_LIBRARY_REAL_PATH}) + + # CppADCodeGen. + find_path(CppADCodeGen_INCLUDE_DIR cppad/cg.hpp REQUIRED) + + add_library(CppADCodeGen INTERFACE IMPORTED GLOBAL) + add_library(CppADCodeGen::CppADCodeGen ALIAS CppADCodeGen) + target_include_directories(CppADCodeGen INTERFACE ${CppADCodeGen_INCLUDE_DIR}) +endif() + +if(@UNGAR_ENABLE_OPTIMIZATION@) + # Abseil. + find_dependency(absl REQUIRED) + # OSQP. + find_dependency(osqp REQUIRED) + # osqp-cpp. + find_dependency(osqp-cpp REQUIRED) +endif() + +include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@Targets.cmake") diff --git a/example/CMakeLists.txt b/example/CMakeLists.txt new file mode 100644 index 0000000..7402960 --- /dev/null +++ b/example/CMakeLists.txt @@ -0,0 +1,37 @@ +add_executable(ungar.variable.example variable.example.cpp) +target_link_libraries(ungar.variable.example ungar::ungar) + +add_executable(ungar.variable_map.example variable_map.example.cpp) +target_link_libraries(ungar.variable_map.example ungar::ungar) + +if(UNGAR_ENABLE_AUTODIFF) + add_executable(ungar.function.example autodiff/function.example.cpp) + target_link_libraries(ungar.function.example ungar::ungar) +endif() + +if(UNGAR_ENABLE_OPTIMIZATION) + add_executable(ungar.quadrotor.example mpc/quadrotor.example.cpp) + target_link_libraries(ungar.quadrotor.example ungar::ungar) + + add_executable(ungar.quadruped.example mpc/quadruped.example.cpp) + target_link_libraries(ungar.quadruped.example ungar::ungar) + + add_executable(ungar.rc_car.example mpc/rc_car.example.cpp) + target_link_libraries(ungar.rc_car.example ungar::ungar) +endif() + +# Add examples to the test targets if testing is enabled. +if(UNGAR_BUILD_TESTS) + add_test(NAME ungar.variable.example COMMAND ungar.variable.example) + add_test(NAME ungar.variable_map.example COMMAND ungar.variable_map.example) + + if(UNGAR_ENABLE_AUTODIFF) + add_test(NAME ungar.function.example COMMAND ungar.function.example) + endif() + + if(UNGAR_ENABLE_OPTIMIZATION) + add_test(NAME ungar.quadrotor.example COMMAND ungar.quadrotor.example) + add_test(NAME ungar.quadruped.example COMMAND ungar.quadruped.example) + add_test(NAME ungar.rc_car.example COMMAND ungar.rc_car.example) + endif() +endif() diff --git a/example/autodiff/function.example.cpp b/example/autodiff/function.example.cpp new file mode 100644 index 0000000..0b422a6 --- /dev/null +++ b/example/autodiff/function.example.cpp @@ -0,0 +1,96 @@ +/****************************************************************************** + * + * @file ungar/example/autodiff/function.example.cpp + * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) + * + * @section LICENSE + * ----------------------------------------------------------------------- + * + * Copyright 2023 Flavio De Vincenti + * + * ----------------------------------------------------------------------- + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an "AS + * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either + * express or implied. See the License for the specific language + * governing permissions and limitations under the License. + * + ******************************************************************************/ + +#include "ungar/autodiff/function.hpp" +#include "ungar/variable_map.hpp" + +int main() { + using namespace Ungar; + + /*************** POINT MASS ***************/ + /*************** Define numeric invariants as integral constants. ***************/ + constexpr auto N = 30_c; // Discrete time horizon. + + /*************** Define "leaf" variables. ***************/ + // Positions are 3-dimensional vectors, masses are scalars, etc. + UNGAR_VARIABLE(position, 3); // := p + UNGAR_VARIABLE(linear_velocity, 3); // := pDot + UNGAR_VARIABLE(force, 3); // := f + UNGAR_VARIABLE(mass, 1); // := m + UNGAR_VARIABLE(state_cost_weight, 1); // := wx + UNGAR_VARIABLE(input_cost_weight, 1); // := wu + + /*************** Define "branch" variables. ***************/ + // States are stacked poses and velocities, while inputs are forces applied + // to the point mass. + UNGAR_VARIABLE(x) <<= (position, linear_velocity); // x := [p pDot] + UNGAR_VARIABLE(u) <<= force; // u := f + UNGAR_VARIABLE(X) <<= (N + 1_c) * x; // X := [x0 x1 ... xN] + UNGAR_VARIABLE(U) <<= N * u; // U := [u0 u1 ... uN-1] + UNGAR_VARIABLE(XRef) <<= (N + 1_c) * x; // XRef := [xRef0 xRef1 ... xRefN] + + UNGAR_VARIABLE(decision_variables) <<= (X, U); + UNGAR_VARIABLE(parameters) <<= (mass, XRef, state_cost_weight, input_cost_weight); + UNGAR_VARIABLE(variables) <<= (decision_variables, parameters); + + /*************** Generate derivatives for functions of variable maps. ***************/ + const Autodiff::Function::Blueprint functionBlueprint{ + [&](const VectorXad& xp, VectorXad& y) { + // Create variables maps on existing arrays of data. + const auto vars = MakeVariableLazyMap(xp, variables); + + // You can further split the stacked independent variables and parameters. + // const auto decVars = + // MakeVariableLazyMap(varsMap.Get(decision_variables), decision_variables); + // const auto params = MakeVariableLazyMap(varsMap.Get(parameters), parameters); + + y = VectorXr::Zero(1); + for (auto k : enumerate(N)) { + y[0] += vars.Get(state_cost_weight) * + (vars.Get(X, x, k) - vars.Get(XRef, x, k)).squaredNorm() + + vars.Get(input_cost_weight) * vars.Get(U, u, k).squaredNorm(); + } + y[0] += vars.Get(state_cost_weight) * + (vars.Get(X, x, N) - vars.Get(XRef, x, N)).squaredNorm(); + }, + decision_variables.Size(), + parameters.Size(), + "function_example"sv, + EnabledDerivatives::JACOBIAN | EnabledDerivatives::HESSIAN}; + const auto function = Autodiff::MakeFunction(functionBlueprint, true); + + auto vars = MakeVariableMap(variables); + vars.Get().setRandom(); + const VectorXr value = function(vars.Get()); + const SparseMatrix jacobian = function.Jacobian(vars.Get()); + const SparseMatrix hessian = function.Hessian(vars.Get()); + + UNGAR_ASSERT(value.size() == function.DependentVariableSize() && value.size() == 1); + UNGAR_ASSERT(function.TestJacobian(vars.Get())); + UNGAR_ASSERT(function.TestHessian(vars.Get())); + + return 0; +} diff --git a/example/mpc/quadrotor.example.cpp b/example/mpc/quadrotor.example.cpp new file mode 100644 index 0000000..aa55217 --- /dev/null +++ b/example/mpc/quadrotor.example.cpp @@ -0,0 +1,431 @@ +/****************************************************************************** + * + * @file ungar/example/mpc/quadrotor.example.cpp + * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) + * + * @section LICENSE + * ----------------------------------------------------------------------- + * + * Copyright 2023 Flavio De Vincenti + * + * ----------------------------------------------------------------------- + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an "AS + * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either + * express or implied. See the License for the specific language + * governing permissions and limitations under the License. + * + * ----------------------------------------------------------------------- + * + * @section DESCRIPTION + * + * This file implements a nonlinear model predictive controller for + * a quadrotor living in SE(3). The dynamical model of the quadrotor + * is adapted from the lecture notes of the course "Robot Dynamics" + * [1] held at ETH Zurich. + * + * @see [1] Marco Hutter, Roland Siegwart, Class Lecture, Topic: + * "Dynamic Modeling of Rotorcraft & Control." 151-0851-00L. + * Department of Mechanical and Process Engineering, + * ETH Zurich, Zurich, 2022. + * + ******************************************************************************/ + +#include "ungar/autodiff/vector_composer.hpp" +#include "ungar/optimization/soft_sqp.hpp" +#include "ungar/variable_map.hpp" + +int main() { + using namespace Ungar; + + /*======================================================================================*/ + /*~~~~~~~~~~~~~| PART I: QUADROTOR MODEL |~~~~~~~~~~~~~*/ + /*======================================================================================*/ + /*************** Define numeric invariants as integral constants. ***************/ + constexpr auto N = 30_c; // Discrete time horizon. + constexpr auto NUM_ROTORS = 4_c; + + /*************** Define decision variables. ***************/ + // Positions are 3-dimensional vectors, orientations are unit quaternions, + // etc. The states are stacked poses and velocities. + UNGAR_VARIABLE(position, 3); // := p + UNGAR_VARIABLE(orientation, Q); // := q + UNGAR_VARIABLE(linear_velocity, 3); // := pDot + UNGAR_VARIABLE(b_angular_velocity, 3); // := bOmega + UNGAR_VARIABLE(x) <<= + (position, orientation, linear_velocity, b_angular_velocity); // x := [p q pDot bOmega] + UNGAR_VARIABLE(X) <<= (N + 1_c) * x; // X := [x0 x1 ... xN] + + // The control inputs are the stacked rotor speeds, one for each rotor. + UNGAR_VARIABLE(rotor_speed, 1); // := r + UNGAR_VARIABLE(u) <<= NUM_ROTORS * rotor_speed; // u := [r0 r1 r2 r3] + UNGAR_VARIABLE(U) <<= N * u; // U := [u0 u1 ... uN-1] + + /*************** Define parameters. ***************/ + // Step size. + UNGAR_VARIABLE(step_size, 1); + + // Inertial and geometric parameters. + UNGAR_VARIABLE(mass, 1); + UNGAR_VARIABLE(b_moi_diagonal, 3); + UNGAR_VARIABLE(b_propeller_position, 3); + + // Physical constants and general parameters. + UNGAR_VARIABLE(standard_gravity, 1); + UNGAR_VARIABLE(thrust_constant, 1); + UNGAR_VARIABLE(drag_constant, 1); + UNGAR_VARIABLE(max_rotor_speed, 1); + + // Reference trajectories. + UNGAR_VARIABLE(reference_position, 3); + UNGAR_VARIABLE(reference_orientation, Q); + UNGAR_VARIABLE(reference_linear_velocity, 3); + UNGAR_VARIABLE(b_reference_angular_velocity, 3); + + // Measurements. + UNGAR_VARIABLE(measured_position, 3); + UNGAR_VARIABLE(measured_orientation, Q); + UNGAR_VARIABLE(measured_linear_velocity, 3); + UNGAR_VARIABLE(b_measured_angular_velocity, 3); + UNGAR_VARIABLE(measured_state) <<= (measured_position, + measured_orientation, + measured_linear_velocity, + b_measured_angular_velocity); + + /*************** Define variables. ***************/ + UNGAR_VARIABLE(decision_variables) <<= (X, U); + UNGAR_VARIABLE(parameters) <<= (step_size, + mass, + b_moi_diagonal, + NUM_ROTORS * b_propeller_position, + standard_gravity, + thrust_constant, + drag_constant, + max_rotor_speed, + (N + 1_c) * reference_position, + (N + 1_c) * reference_orientation, + (N + 1_c) * reference_linear_velocity, + (N + 1_c) * b_reference_angular_velocity, + measured_state); + UNGAR_VARIABLE(variables) <<= (decision_variables, parameters); + + /*======================================================================================*/ + /*~~~~~~~~~~~~~| PART II: QUADROTOR DYNAMICS |~~~~~~~~~~~~~*/ + /*======================================================================================*/ + /// @brief Given vectors of autodiff scalars corresponding to the system's state, + /// input and parameters at a given time step, compute the state at the + /// next time step using a Lie group semi-implicit Euler method. + /*************** Define discrete-time quadrotor dynamics equation. ***************/ + const auto quadrotorDynamics = [&](const VectorXad& xUnderlying, + const VectorXad& uUnderlying, + const VectorXad& parametersUnderlying) -> VectorXad { + // Create variable lazy maps for the system's state, input and parameters. + /// @note As a convention, we name the underlying data representation of a + /// variable \c v as \c vUnderlying, and we name \c v_ the associated + /// map object. + const auto x_ = MakeVariableLazyMap(xUnderlying, x); + const auto u_ = MakeVariableLazyMap(uUnderlying, u); + const auto parameters_ = MakeVariableLazyMap(parametersUnderlying, parameters); + + // Retrieve all variables. + const auto [dt, g0, b, d] = + parameters_.GetTuple(step_size, standard_gravity, thrust_constant, drag_constant); + const auto [m, bMOIDiagonal] = parameters_.GetTuple(mass, b_moi_diagonal); + + const auto [p, q, pDot, bOmega] = + x_.GetTuple(position, orientation, linear_velocity, b_angular_velocity); + + // Calculate thrust forces and drag moments for each rotor. + // Thrust force: bTi = b * ri^2 * ez + // Thrust moment: bMi = pPi x bTi + // Drag moment: bD = d * ri^2 * ez * (-1)^i + std::vector bThrustForces{}, bThrustMoments{}, bDragMoments{}; + for (const auto i : enumerate(NUM_ROTORS)) { + const auto& r = u_.Get(rotor_speed, i); + const auto pP = parameters_.Get(b_propeller_position, i); + + bThrustForces.emplace_back(b * Utils::Pow(r, 2) * Vector3ad::UnitZ()); + bThrustMoments.emplace_back(pP.cross(bThrustForces.back())); + bDragMoments.emplace_back(d * Utils::Pow(r, 2) * Vector3ad::UnitZ() * + Utils::Pow(-1.0, i)); + } + + // Calculate linear acceleration and angular acceleration. + const auto summation = [](const std::vector& seq) { + return std::accumulate(seq.begin(), seq.end(), Vector3ad::Zero().eval()); + }; + const Vector3ad pDotDot = (q * summation(bThrustForces) - m * g0 * Vector3ad::UnitZ()) / m; + const Vector3ad bOmegaDot = bMOIDiagonal.cwiseInverse().cwiseProduct( + summation(bThrustMoments) + summation(bDragMoments) - + bOmega.cross(bMOIDiagonal.cwiseProduct(bOmega))); + + // Create variable map with autodiff scalar type for the next state. + auto xNext_ = MakeVariableMap(x); + + auto [pNext, qNext, pDotNext, bOmegaNext] = + xNext_.GetTuple(position, orientation, linear_velocity, b_angular_velocity); + + // Update next state using Lie group semi-implicit Euler method and + // return underlying data. + /// @note For a description of the integration method, refer to [2]. + /// This approach embeds the quaternion unit norm constraints + /// directly into the discretized dynamics equations. + /// + /// @see [2] Flavio De Vincenti and Stelian Coros. "Centralized Model + /// Predictive Control for Collaborative Loco-Manipulation." + /// Robotics: Science and Systems (2023). + pDotNext = pDot + dt * pDotDot; + bOmegaNext = bOmega + dt * bOmegaDot; + pNext = p + dt * pDotNext; + qNext = q * Utils::ApproximateExponentialMap(dt * bOmegaNext); + + return xNext_.Get(); + }; + + /*======================================================================================*/ + /*~~~~~~~~~~~~~| PART III: OPTIMAL CONTROL PROBLEM |~~~~~~~~~~~~~*/ + /*======================================================================================*/ + /*************** Define objective function. ***************/ + const auto objectiveFunction = [&](const VectorXad& variablesUnderlying, + VectorXad& objectiveFunctionUnderlying) { + // Create variable lazy maps for the system's variables, which include both + // decision variables and parameters. + const auto variables_ = MakeVariableLazyMap(variablesUnderlying, variables); + + ad_scalar_t value{0.0}; + for (const auto k : enumerate(N + 1_step)) { + const auto p = variables_.Get(position, k); + const auto q = variables_.Get(orientation, k); + const auto pDot = variables_.Get(linear_velocity, k); + const auto bOmega = variables_.Get(b_angular_velocity, k); + + const auto pRef = variables_.Get(reference_position, k); + const auto qRef = variables_.Get(reference_orientation, k); + const auto pDotRef = variables_.Get(reference_linear_velocity, k); + const auto bOmegaRef = variables_.Get(b_reference_angular_velocity, k); + + // Reference state tracking. + value += ((p - pRef).squaredNorm() + + Utils::Min((q.coeffs() - qRef.coeffs()).squaredNorm(), + (q.coeffs() + qRef.coeffs()).squaredNorm()) + + (pDot - pDotRef).squaredNorm() + (bOmega - bOmegaRef).squaredNorm()); + if (k && k != N) { + // Regularization of input variations. + const auto ukm1 = variables_.Get(u, k - 1_step); + const auto uk = variables_.Get(u, k); + + value += 1e-6 * (uk - ukm1).squaredNorm(); + } + if (k != N) { + // Input regularization. + const auto uk = variables_.Get(u, k); + value += 1e-6 * uk.squaredNorm(); + } + } + + /// @note Autodiff functions must return Eigen vectors, therefore the + /// objective function returns a vector of size 1 containing the + /// objective value. + objectiveFunctionUnderlying.resize(1_idx); + objectiveFunctionUnderlying << value; + }; + + /*************** Define equality constraints. ***************/ + /// @brief Equality constraints are a function \c g of \c variables such that + /// \c g(variables) = 0. + const auto equalityConstraints = [&](const VectorXad& variablesUnderlying, + VectorXad& equalityConstraintsUnderlying) { + const auto variables_ = MakeVariableLazyMap(variablesUnderlying, variables); + + // Define helper for composing equality constraints into a single Eigen vector. + Autodiff::VectorComposer composer; + + // Add equality constraints for the initial state. + const auto x0 = variables_.Get(x, 0_step); + const auto xm = variables_.Get(measured_state); + composer << x0 - xm; + + // Add system dynamics constraint for each time step. + for (const auto k : enumerate(N)) { + const auto xk = variables_.Get(x, k); + const auto xkp1 = variables_.Get(x, k + 1_step); + const auto uk = variables_.Get(u, k); + const auto params = variables_.Get(parameters); + + composer << xkp1 - quadrotorDynamics(xk, uk, params); + } + + equalityConstraintsUnderlying = composer.Compose(); + }; + + /*************** Define inequality constraints. ***************/ + /// @brief Inequality constraints are a function \c h of \c variables such that + /// \c h(variables) <= 0. + const auto inequalityConstraints = [&](const VectorXad& variablesUnderlying, + VectorXad& inequalityConstraintsUnderlying) { + const auto variables_ = MakeVariableLazyMap(variablesUnderlying, variables); + + // Define helper for composing inequality constraints into a single Eigen vector. + Autodiff::VectorComposer composer; + + const auto& rMax = variables_.Get(max_rotor_speed); + + for (const auto k : enumerate(N)) { + for (const auto i : enumerate(NUM_ROTORS)) { + const auto r = variables_.Get(rotor_speed, k, i); + + // Input bound constraints. + composer << r - rMax; + composer << -r; + } + } + + inequalityConstraintsUnderlying = composer.Compose(); + }; + + /*************** Define optimal control problem (OCP). ***************/ + // Based on the autodiff functions defined above, generate code for the + // corresponding derivatives and compile it just-in-time. + Autodiff::Function::Blueprint objectiveFunctionBlueprint{objectiveFunction, + decision_variables.Size(), + parameters.Size(), + "quadrotor_mpc_obj"sv, + EnabledDerivatives::ALL}; + Autodiff::Function::Blueprint equalityConstraintsBlueprint{equalityConstraints, + decision_variables.Size(), + parameters.Size(), + "quadrotor_mpc_eqs"sv, + EnabledDerivatives::JACOBIAN}; + Autodiff::Function::Blueprint inequalityConstraintsBlueprint{inequalityConstraints, + decision_variables.Size(), + parameters.Size(), + "quadrotor_mpc_ineqs"sv, + EnabledDerivatives::JACOBIAN}; + + const bool recompileLibraries = true; + auto ocp = + MakeNLPProblem(Autodiff::MakeFunction(objectiveFunctionBlueprint, recompileLibraries), + Autodiff::MakeFunction(equalityConstraintsBlueprint, recompileLibraries), + Autodiff::MakeFunction(inequalityConstraintsBlueprint, recompileLibraries)); + + /*======================================================================================*/ + /*~~~~~~~~~~~~~| PART IV: MODEL PREDICTIVE CONTROL |~~~~~~~~~~~~~*/ + /*======================================================================================*/ + /*************** Initialize OCP variables. ***************/ + // Create variable map storing all decision variables and parameters for the quadrotor. + auto variables_ = MakeVariableMap(variables); + + // Step size. + variables_.Get(step_size) = 1.0 / static_cast(N); + + // Inertial and geometric parameters. + variables_.Get(mass) = 1.5; + variables_.Get(b_moi_diagonal).setConstant(3e-2); + + // Propeller positions with respect to the quadrotor's center of mass. + // These values represent a square-shaped quadrotor configuration. + variables_.Get(b_propeller_position, 0_idx) = Vector3r(0.2, 0.2, 0.0); + variables_.Get(b_propeller_position, 1_idx) = Vector3r(-0.2, 0.2, 0.0); + variables_.Get(b_propeller_position, 2_idx) = Vector3r(-0.2, -0.2, 0.0); + variables_.Get(b_propeller_position, 3_idx) = Vector3r(0.2, -0.2, 0.0); + + // Physical constants. + variables_.Get(standard_gravity) = 9.80665; + variables_.Get(thrust_constant) = 0.015; + variables_.Get(drag_constant) = 0.1; + variables_.Get(max_rotor_speed) = 1e2; + + // Measurements. + const real_t initialHeight = 4.0; + variables_.Get(measured_position) = initialHeight * Vector3r::UnitZ(); + variables_.Get(measured_orientation).setIdentity(); + variables_.Get(measured_linear_velocity).setZero(); + variables_.Get(b_measured_angular_velocity).setZero(); + + // Decision variables. + for (const auto k : enumerate(N + 1_step)) { + variables_.Get(x, k) = variables_.Get(measured_state); + } + variables_.Get(U).setConstant( + Utils::Sqrt(variables_.Get(mass) * variables_.Get(standard_gravity) / + variables_.Get(thrust_constant) / static_cast(NUM_ROTORS))); + + // Reference trajectories. + /// @brief Command the quadrotor to track a sinusoidal trajectory along the z-axis + /// while rotating about the z-axis. + const real_t zPeriodReference = 4.0; + const real_t zAmplitudeReference = 1.0; + const real_t yawRateReference = std::numbers::pi; + const real_t missionStartTime = zPeriodReference; + + /*************** Solve OCP over receding horizon. ***************/ + // Define OCP optimizer. + SoftSQPOptimizer optimizer{false, default_value, 40_idx}; + + const real_t finalTime = 10.0; + for (real_t time = 0.0; time < finalTime; time += variables_.Get(step_size)) { + // Integrate quadrotor dynamics using the optimized input. + variables_.Get(measured_state) = Utils::ToRealFunction(quadrotorDynamics)( + variables_.Get(measured_state), variables_.Get(u, 0_step), variables_.Get(parameters)); + + // Update reference trajectories. + for (const auto k : enumerate(N + 1_step)) { + const real_t t = time + static_cast(k) * variables_.Get(step_size); + const real_t missionStarted = static_cast(t > missionStartTime); + + variables_.Get(reference_position, k) = + (initialHeight + missionStarted * zAmplitudeReference * + sin(2.0 * std::numbers::pi / zPeriodReference * t)) * + Vector3r::UnitZ(); + variables_.Get(reference_orientation, k) = + missionStarted ? Utils::ElementaryZQuaternion(yawRateReference * t) + : Quaternionr::Identity(); + variables_.Get(reference_linear_velocity, k) = + missionStarted * 2.0 * std::numbers::pi / zPeriodReference * zAmplitudeReference * + cos(2.0 * std::numbers::pi / zPeriodReference * t) * Vector3r::UnitZ(); + variables_.Get(b_reference_angular_velocity, k) = + missionStarted * yawRateReference * Vector3r::UnitZ(); + } + + // Shift the previous solution by one time step to warm start the optimization. + for (const auto k : enumerate(N - 1_step)) { + variables_.Get(x, k) = variables_.Get(x, k + 1_step); + variables_.Get(u, k) = variables_.Get(u, k + 1_step); + } + variables_.Get(x, N - 1_step) = variables_.Get(x, N); + + /// @note This trick prevents quaternion sign switches from + /// spoiling the optimization. Find more details in [2]. + const auto& qm = std::as_const(variables_).Get(measured_orientation); + const auto& q0 = std::as_const(variables_).Get(orientation, 0_step); + if ((qm.coeffs() - q0.coeffs()).squaredNorm() > + (-qm.coeffs() - q0.coeffs()).squaredNorm()) { + variables_.Get(measured_orientation).coeffs() *= -1.0; + } + + // Solve OCP and log optimization results. + variables_.Get(decision_variables) = optimizer.Optimize(ocp, variables_.Get()); + UNGAR_LOG( + info, + "t = {:.3f}, obj = {:.3f}, eqs = {:.3f}, ineqs = {:.3f}, z ref = {:.3f}, z = " + "{:.3f}, yaw ref = {:.3f}, yaw = {:.3f}, u = {:.3f}", + time, + Utils::Squeeze(ocp.objective(variables_.Get())), + ocp.equalityConstraints(variables_.Get()).lpNorm(), + ocp.inequalityConstraints(variables_.Get()).maxCoeff(), + variables_.Get(reference_position, 0_step).z(), + variables_.Get(position, 0_step).z(), + Utils::QuaternionToYawPitchRoll(variables_.Get(reference_orientation, 0_step)).z(), + Utils::QuaternionToYawPitchRoll(variables_.Get(orientation, 0_step)).z(), + fmt::join(variables_.Get(u, 0_step), ", ")); + } + + return 0; +} diff --git a/example/mpc/quadruped.example.cpp b/example/mpc/quadruped.example.cpp new file mode 100644 index 0000000..c0e62c9 --- /dev/null +++ b/example/mpc/quadruped.example.cpp @@ -0,0 +1,537 @@ +/****************************************************************************** + * + * @file ungar/example/mpc/quadruped.example.cpp + * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) + * + * @section LICENSE + * ----------------------------------------------------------------------- + * + * Copyright 2023 Flavio De Vincenti + * + * ----------------------------------------------------------------------- + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an "AS + * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either + * express or implied. See the License for the specific language + * governing permissions and limitations under the License. + * + * ----------------------------------------------------------------------- + * + * @section DESCRIPTION + * + * This file implements a nonlinear model predictive controller for + * quadrupedal locomotion using the single rigid body dynamics model. + * This controller is based on [1] but does not linearize the dynamics + * equations and adopts the Lie group time stepping method described + * in [2]. + * + * @see [1] Gerardo Bledt and Sangbae Kim. “Implementing Regularized + * Predictive Control for Simultaneous Real-Time Footstep and + * Ground Reaction Force Optimization.” 2019 IEEE/RSJ + * International Conference on Intelligent Robots and Systems + * (IROS) (2019): 6316-6323. + * [2] Flavio De Vincenti and Stelian Coros. "Centralized Model + * Predictive Control for Collaborative Loco-Manipulation." + * Robotics: Science and Systems (2023). + * + ******************************************************************************/ + +#include "ungar/autodiff/vector_composer.hpp" +#include "ungar/optimization/soft_sqp.hpp" +#include "ungar/variable_map.hpp" + +int main() { + using namespace Ungar; + + /*======================================================================================*/ + /*~~~~~~~~~~~~~| PART I: QUADRUPED MODEL |~~~~~~~~~~~~~*/ + /*======================================================================================*/ + /*************** Define numeric invariants as integral constants. ***************/ + constexpr auto N = 30_c; // Discrete time horizon. + constexpr auto NUM_LEGS = 4_c; + + /*************** Define decision variables. ***************/ + // Positions are 3-dimensional vectors, orientations are unit quaternions, + // etc. The states are stacked poses and velocities. + UNGAR_VARIABLE(position, 3); // := p + UNGAR_VARIABLE(orientation, Q); // := q + UNGAR_VARIABLE(linear_velocity, 3); // := pDot + UNGAR_VARIABLE(b_angular_velocity, 3); // := bOmega + UNGAR_VARIABLE(x) <<= + (position, orientation, linear_velocity, b_angular_velocity); // x := [p q pDot bOmega] + UNGAR_VARIABLE(X) <<= (N + 1_c) * x; // X := [x0 x1 ... xN] + + // The control inputs are the stacked ground reaction forces (GRFs) and + // foot positions, the latter expressed in the base frame. + UNGAR_VARIABLE(ground_reaction_force, 3); // := f + UNGAR_VARIABLE(b_foot_position, 3); // := r + UNGAR_VARIABLE(leg_input) <<= (ground_reaction_force, b_foot_position); // uL = [f r] + UNGAR_VARIABLE(u) <<= NUM_LEGS * leg_input; // u := [uL0 uL1 uL2 uL3] + UNGAR_VARIABLE(U) <<= N * u; // U := [u0 u1 ... uN-1] + + /*************** Define time-varying parameters. ***************/ + // Reference trajectories. + UNGAR_VARIABLE(reference_position, 3); + UNGAR_VARIABLE(reference_orientation, Q); + UNGAR_VARIABLE(reference_linear_velocity, 3); + UNGAR_VARIABLE(b_reference_angular_velocity, 3); + UNGAR_VARIABLE(reference_state) <<= (reference_position, + reference_orientation, + reference_linear_velocity, + b_reference_angular_velocity); + + UNGAR_VARIABLE(reference_contact_state, 1); // := s + UNGAR_VARIABLE(b_reference_foot_position, 3); + UNGAR_VARIABLE(reference_leg_state) <<= (reference_contact_state, b_reference_foot_position); + + UNGAR_VARIABLE(p) <<= (reference_state, NUM_LEGS * reference_leg_state); + UNGAR_VARIABLE(P) <<= (N + 1_c) * p; + + /*************** Define stationary parameters. ***************/ + // Step size. + UNGAR_VARIABLE(step_size, 1); + + // Inertial and geometric parameters. + UNGAR_VARIABLE(mass, 1); + UNGAR_VARIABLE(b_moi_diagonal, 3); + UNGAR_VARIABLE(inertial_properties) <<= (mass, b_moi_diagonal); + + UNGAR_VARIABLE(b_hip_position, 3); + UNGAR_VARIABLE(leg_length, 1); + UNGAR_VARIABLE(geometric_data) <<= (NUM_LEGS * b_hip_position, leg_length); + + // Physical constants. + UNGAR_VARIABLE(standard_gravity, 1); + UNGAR_VARIABLE(friction_coefficient, 1); + UNGAR_VARIABLE(physical_constants) <<= (standard_gravity, friction_coefficient); + + // Measurements. + UNGAR_VARIABLE(measured_position, 3); + UNGAR_VARIABLE(measured_orientation, Q); + UNGAR_VARIABLE(measured_linear_velocity, 3); + UNGAR_VARIABLE(b_measured_angular_velocity, 3); + UNGAR_VARIABLE(measured_state) <<= (measured_position, + measured_orientation, + measured_linear_velocity, + b_measured_angular_velocity); + + UNGAR_VARIABLE(measured_contact_state, 1); + UNGAR_VARIABLE(measured_foot_position, 3); + UNGAR_VARIABLE(measured_leg_state) <<= (measured_contact_state, measured_foot_position); + + UNGAR_VARIABLE(Rho) <<= (step_size, + inertial_properties, + geometric_data, + physical_constants, + measured_state, + NUM_LEGS * measured_leg_state); + + /*************** Define variables. ***************/ + UNGAR_VARIABLE(decision_variables) <<= (X, U); + UNGAR_VARIABLE(parameters) <<= (P, Rho); + UNGAR_VARIABLE(variables) <<= (decision_variables, parameters); + + /*======================================================================================*/ + /*~~~~~~~~~~~~~| PART II: QUADRUPED DYNAMICS |~~~~~~~~~~~~~*/ + /*======================================================================================*/ + /// @brief Given vectors of autodiff scalars corresponding to the system's state, + /// input and parameters at a given time step, compute the state at the + /// next time step using a Lie group semi-implicit Euler method. + /*************** Define discrete-time quadruped dynamics equation. ***************/ + const auto quadrupedDynamics = [&](const VectorXad& xUnderlying, + const VectorXad& uUnderlying, + const VectorXad& pUnderlying, + const VectorXad& RhoUnderlying) -> VectorXad { + // Create variable lazy maps for the system's state, input and parameters. + /// @note As a convention, we name the underlying data representation of a + /// variable \c v as \c vUnderlying, and we name \c v_ the associated + /// map object. + const auto x_ = MakeVariableLazyMap(xUnderlying, x); + const auto u_ = MakeVariableLazyMap(uUnderlying, u); + const auto p_ = MakeVariableLazyMap(pUnderlying, p); + const auto Rho_ = MakeVariableLazyMap(RhoUnderlying, Rho); + + // Retrieve all variables. + const auto [dt, g0, m, bMOIDiagonal] = + Rho_.GetTuple(step_size, standard_gravity, mass, b_moi_diagonal); + + const auto [p, q, pDot, bOmega] = + x_.GetTuple(position, orientation, linear_velocity, b_angular_velocity); + + // Calculate linear acceleration and angular acceleration. + Vector3ad pDotDot = -g0 * Vector3r::UnitZ(); + Vector3ad bOmegaDot = -bOmega.cross(bMOIDiagonal.asDiagonal() * bOmega); + for (const auto i : enumerate(NUM_LEGS)) { + const auto f = u_.Get(ground_reaction_force, i); + const auto r = u_.Get(b_foot_position, i); + const auto& s = p_.Get(reference_contact_state, i); + + pDotDot += s * f / m; + bOmegaDot += s * r.cross(q * f); + } + bOmegaDot = bOmegaDot.array() / bMOIDiagonal.array(); + + // Create variable map with autodiff scalar type for the next state. + VectorXad xNextUnderlying{x.Size()}; + auto xNext_ = MakeVariableLazyMap(xNextUnderlying, x); + // Alternatively, the following line creates both the underlying data + // representation and the variable map: it achieves the best real-time + // performance at the cost of longer compile time. + // auto xNext_ = MakeVariableMap(x); + + auto [pNext, qNext, pDotNext, bOmegaNext] = + xNext_.GetTuple(position, orientation, linear_velocity, b_angular_velocity); + + // Update next state using Lie group semi-implicit Euler method and + // return underlying data. + /// @note For a description of the integration method, refer to [2]. + /// This approach embeds the quaternion unit norm constraints + /// directly into the discretized dynamics equations. + pDotNext = pDot + dt * pDotDot; + bOmegaNext = bOmega + dt * bOmegaDot; + pNext = p + dt * pDotNext; + qNext = q * Utils::ApproximateExponentialMap(dt * bOmegaNext); + + return xNext_.Get(); + }; + + /*======================================================================================*/ + /*~~~~~~~~~~~~~| PART III: OPTIMAL CONTROL PROBLEM |~~~~~~~~~~~~~*/ + /*======================================================================================*/ + /*************** Define objective function. ***************/ + const auto objectiveFunction = [&](const VectorXad& variablesUnderlying, + VectorXad& objectiveFunctionUnderlying) { + // Create variable lazy maps for the system's variables, which include both + // decision variables and parameters. + const auto variables_ = MakeVariableLazyMap(variablesUnderlying, variables); + + ad_scalar_t value{0.0}; + for (const auto k : enumerate(N + 1_step)) { + const auto p = variables_.Get(position, k); + const auto q = variables_.Get(orientation, k); + const auto pDot = variables_.Get(linear_velocity, k); + const auto bOmega = variables_.Get(b_angular_velocity, k); + + const auto pRef = variables_.Get(reference_position, k); + const auto qRef = variables_.Get(reference_orientation, k); + const auto pDotRef = variables_.Get(reference_linear_velocity, k); + const auto bOmegaRef = variables_.Get(b_reference_angular_velocity, k); + + // Reference state tracking. + value += (Vector3r{0.1, 0.1, 10.0}.cwiseProduct(p - pRef).squaredNorm() + + Utils::Min((q.coeffs() - qRef.coeffs()).squaredNorm(), + (q.coeffs() + qRef.coeffs()).squaredNorm()) + + (pDot - pDotRef).squaredNorm() + (bOmega - bOmegaRef).squaredNorm()); + if (k != N) { + // Input regularization. + for (const auto i : enumerate(NUM_LEGS)) { + const auto f = variables_.Get(ground_reaction_force, k, i); + const auto r = variables_.Get(b_foot_position, k, i); + + const auto rRef = variables_.Get(b_reference_foot_position, k, i); + + value += (r - rRef).squaredNorm(); + value += 1e-8 * f.squaredNorm(); + } + } + } + + /// @note Autodiff functions must return Eigen vectors, therefore the + /// objective function returns a vector of size 1 containing the + /// objective value. + objectiveFunctionUnderlying.resize(1_idx); + objectiveFunctionUnderlying << value; + }; + + /*************** Define equality constraints. ***************/ + /// @brief Equality constraints are a function \c g of \c variables such that + /// \c g(variables) = 0. + const auto equalityConstraints = [&](const VectorXad& variablesUnderlying, + VectorXad& equalityConstraintsUnderlying) { + const auto variables_ = MakeVariableLazyMap(variablesUnderlying, variables); + + // Define helper for composing equality constraints into a single Eigen vector. + Autodiff::VectorComposer composer; + + // Add equality constraints for the initial state. + const auto x0 = variables_.Get(x, 0_step); + const auto xm = variables_.Get(measured_state); + composer << x0 - xm; + + // Add system dynamics constraint for each time step. + for (const auto k : enumerate(N)) { + const auto xk = variables_.Get(x, k); + const auto xkp1 = variables_.Get(x, k + 1_step); + const auto uk = variables_.Get(u, k); + const auto pk = variables_.Get(p, k); + const auto rho = variables_.Get(Rho); + + composer << xkp1 - quadrupedDynamics(xk, uk, pk, rho); + } + + for (const auto k : enumerate(N)) { + for (const auto i : enumerate(NUM_LEGS)) { + const auto& s = variables_.Get(reference_contact_state, k, i); + const auto& sPrev = k ? variables_.Get(reference_contact_state, k - 1_step, i) + : variables_.Get(measured_contact_state, i); + + const auto p = variables_.Get(position, k); + const auto q = variables_.Get(orientation, k); + const auto r = variables_.Get(b_foot_position, k, i); + const Vector3ad pFoot = p + q * r; + + Vector3ad pFootPrev; + if (k) { + const auto pPrev = variables_.Get(position, k - 1_step); + const auto qPrev = variables_.Get(orientation, k - 1_step); + const auto rPrev = variables_.Get(b_foot_position, k - 1_step, i); + + pFootPrev = pPrev + qPrev * rPrev; + } else { + pFootPrev = variables_.Get(measured_foot_position, i); + } + + composer << (1.0 - sPrev) * s * pFoot.z(); + composer << sPrev * s * (pFoot - pFootPrev); + } + } + + equalityConstraintsUnderlying = composer.Compose(); + }; + + /*************** Define inequality constraints. ***************/ + /// @brief Inequality constraints are a function \c h of \c variables such that + /// \c h(variables) <= 0. + const auto inequalityConstraints = [&](const VectorXad& variablesUnderlying, + VectorXad& inequalityConstraintsUnderlying) { + const auto variables_ = MakeVariableLazyMap(variablesUnderlying, variables); + + // Define helper for composing inequality constraints into a single Eigen vector. + Autodiff::VectorComposer composer; + + const auto& mu = variables_.Get(friction_coefficient); + + for (const auto k : enumerate(N)) { + const auto p = variables_.Get(position, k); + const auto q = variables_.Get(orientation, k); + + for (const auto i : enumerate(NUM_LEGS)) { + const auto& s = variables_.Get(reference_contact_state, k, i); + const auto f = variables_.Get(ground_reaction_force, k, i); + const auto r = variables_.Get(b_foot_position, k, i); + + composer << -s * f.z(); + composer << s * Utils::ApproximateNorm(f.head<2>()) - mu * f.z(); + composer << s * Utils::ApproximateNorm(r - variables_.Get(b_hip_position, i)) - + variables_.Get(leg_length); + } + } + + inequalityConstraintsUnderlying = composer.Compose(); + }; + + /*************** Define optimal control problem (OCP). ***************/ + // Based on the autodiff functions defined above, generate code for the + // corresponding derivatives and compile it just-in-time. + Autodiff::Function::Blueprint objectiveFunctionBlueprint{objectiveFunction, + decision_variables.Size(), + parameters.Size(), + "quadruped_mpc_obj"sv, + EnabledDerivatives::ALL}; + Autodiff::Function::Blueprint equalityConstraintsBlueprint{equalityConstraints, + decision_variables.Size(), + parameters.Size(), + "quadruped_mpc_eqs"sv, + EnabledDerivatives::JACOBIAN}; + Autodiff::Function::Blueprint inequalityConstraintsBlueprint{inequalityConstraints, + decision_variables.Size(), + parameters.Size(), + "quadruped_mpc_ineqs"sv, + EnabledDerivatives::JACOBIAN}; + + const bool recompileLibraries = false; + auto ocp = + MakeNLPProblem(Autodiff::MakeFunction(objectiveFunctionBlueprint, recompileLibraries), + Autodiff::MakeFunction(equalityConstraintsBlueprint, recompileLibraries), + Autodiff::MakeFunction(inequalityConstraintsBlueprint, recompileLibraries)); + + /*======================================================================================*/ + /*~~~~~~~~~~~~~| PART IV: MODEL PREDICTIVE CONTROL |~~~~~~~~~~~~~*/ + /*======================================================================================*/ + /*************** Initialize OCP variables. ***************/ + // Create variable map storing all decision variables and parameters for the quadruped. + VectorXr variablesUnderlying{variables.Size()}; + auto variables_ = MakeVariableLazyMap(variablesUnderlying, variables); + // Alternatively, the following line creates both the underlying data + // representation and the variable map: it achieves the best real-time + // performance at the cost of longer compile time. + // auto variables_ = MakeVariableMap(variables); + + // Step size. + variables_.Get(step_size) = 1.0 / static_cast(N); + + // Inertial and geometric parameters. + variables_.Get(mass) = 25.0; + variables_.Get(b_moi_diagonal) = Vector3r{0.048125, 0.093125, 0.055625}; + + variables_.Get(b_hip_position, 0_idx) = Vector3r{0.2, 0.15, -0.1}; // RF + variables_.Get(b_hip_position, 1_idx) = Vector3r{0.2, -0.15, -0.1}; // LF + variables_.Get(b_hip_position, 2_idx) = Vector3r{-0.2, 0.15, -0.1}; // RH + variables_.Get(b_hip_position, 3_idx) = Vector3r{-0.2, -0.15, -0.1}; // LH + variables_.Get(leg_length) = 0.42; + + // Physical constants. + variables_.Get(standard_gravity) = 9.80665; + variables_.Get(friction_coefficient) = 0.7; + + // Measurements. + const real_t initialHeight = 0.38; + variables_.Get(measured_position) = initialHeight * Vector3r::UnitZ(); + variables_.Get(measured_orientation).setIdentity(); + variables_.Get(measured_linear_velocity).setZero(); + variables_.Get(b_measured_angular_velocity).setZero(); + + variables_.Get(measured_contact_state, 0_idx) = 1.0; + variables_.Get(measured_foot_position, 0_idx) = Vector3r(0.2, 0.1, 0.0); + variables_.Get(measured_contact_state, 1_idx) = 1.0; + variables_.Get(measured_foot_position, 1_idx) = Vector3r(0.2, -0.1, 0.0); + variables_.Get(measured_contact_state, 2_idx) = 1.0; + variables_.Get(measured_foot_position, 2_idx) = Vector3r(-0.2, 0.1, 0.0); + variables_.Get(measured_contact_state, 3_idx) = 1.0; + variables_.Get(measured_foot_position, 3_idx) = Vector3r(-0.2, -0.1, 0.0); + + // Decision variables. + for (const auto k : enumerate(N + 1_step)) { + variables_.Get(x, k) = variables_.Get(measured_state); + } + for (const auto k : enumerate(N)) { + for (const auto i : enumerate(NUM_LEGS)) { + variables_.Get(ground_reaction_force, k, i) = + variables_.Get(mass) * variables_.Get(standard_gravity) / 4.0 * Vector3r::UnitZ(); + variables_.Get(b_foot_position, k, i) = variables_.Get(measured_orientation).inverse() * + variables_.Get(measured_foot_position, i); + } + } + + // Reference contact states. + /// @brief Initialize all feet of the robot on the ground for the whole time + /// horizon. + for (const auto k : enumerate(N + 1_step)) { + for (const auto i : enumerate(NUM_LEGS)) { + variables_.Get(reference_contact_state, k, i) = 1.0; + } + } + + // Reference trajectories. + /// @brief Command the quadruped to track a sinusoidal trajectory along the z-axis + /// while rotating about the z-axis using a pace gait. + const real_t zPeriodReference = 8.0; + const real_t zAmplitudeReference = 0.03; + const real_t yawRateReference = std::numbers::pi / 6.0; + + const real_t missionStartTime = 2.0; + const real_t paceGaitPeriod = 0.4; + + /*************** Solve OCP over receding horizon. ***************/ + // Define OCP optimizer. + SoftSQPOptimizer optimizer{false, variables_.Get(step_size), 4_idx, 1.0, 1.0}; + + const real_t finalTime = 10.0; + for (real_t time = 0.0; time < finalTime; time += variables_.Get(step_size)) { + // Integrate quadruped dynamics using the optimized input. + variables_.Get(measured_state) = + Utils::ToRealFunction(quadrupedDynamics)(variables_.Get(measured_state), + variables_.Get(u, 0_step), + variables_.Get(p, 0_step), + variables_.Get(Rho)); + for (const auto i : enumerate(NUM_LEGS)) { + variables_.Get(measured_foot_position, i) = + variables_.Get(measured_position) + + variables_.Get(measured_orientation) * variables_.Get(b_foot_position, 1_step, i); + variables_.Get(measured_contact_state, i) = + variables_.Get(reference_contact_state, 1_step, i); + } + + // Update reference trajectories. + for (const auto k : enumerate(N + 1_step)) { + const real_t t = time + static_cast(k) * variables_.Get(step_size); + const real_t missionStarted = static_cast(t > missionStartTime); + + variables_.Get(reference_position, k) = + (initialHeight + + missionStarted * zAmplitudeReference * + sin(2.0 * std::numbers::pi / zPeriodReference * (t - missionStartTime))) * + Vector3r::UnitZ(); + variables_.Get(reference_orientation, k) = + missionStarted + ? Utils::ElementaryZQuaternion(yawRateReference * (t - missionStartTime)) + : Quaternionr::Identity(); + variables_.Get(reference_linear_velocity, k) = + missionStarted * 2.0 * std::numbers::pi / zPeriodReference * zAmplitudeReference * + cos(2.0 * std::numbers::pi / zPeriodReference * (t - missionStartTime)) * + Vector3r::UnitZ(); + variables_.Get(b_reference_angular_velocity, k) = + missionStarted * yawRateReference * Vector3r::UnitZ(); + + for (const bool paceGaitPhase = sin(2.0 * std::numbers::pi / paceGaitPeriod * t) > 0.0; + const auto i : enumerate(NUM_LEGS)) { + const auto odd = [](const index_t n) { return static_cast(n & 0b1); }; + variables_.Get(reference_contact_state, k, i) = + missionStarted ? static_cast(odd(i) ? paceGaitPhase : !paceGaitPhase) + : 1.0; + variables_.Get(b_reference_foot_position, k, i) = + variables_.Get(b_hip_position, i) - + 0.8 * variables_.Get(leg_length) * Vector3r::UnitZ(); + } + } + + // Shift the previous solution by one time step to warm start the optimization. + for (const auto k : enumerate(N - 1_step)) { + variables_.Get(x, k) = variables_.Get(x, k + 1_step); + variables_.Get(u, k) = variables_.Get(u, k + 1_step); + } + variables_.Get(x, N - 1_step) = variables_.Get(x, N); + + /// @note This trick prevents quaternion sign switches from + /// spoiling the optimization. Find more details in [2]. + const auto& qm = std::as_const(variables_).Get(measured_orientation); + const auto& q0 = std::as_const(variables_).Get(orientation, 0_step); + if ((qm.coeffs() - q0.coeffs()).squaredNorm() > + (-qm.coeffs() - q0.coeffs()).squaredNorm()) { + variables_.Get(measured_orientation).coeffs() *= -1.0; + } + + // Solve OCP and log optimization results. + variables_.Get(decision_variables) = optimizer.Optimize(ocp, variables_.Get()); + index_t maxCoeffIndex; + const real_t maxCoeff = + ocp.inequalityConstraints(variables_.Get()).maxCoeff(&maxCoeffIndex); + UNGAR_LOG( + info, + "t = {:.3f}, obj = {:.3f}, eqs = {:.3f}, ineqs = {:.3f} ({}), z ref = {:.3f}, z = " + "{:.3f}, yaw ref = {:.3f}, yaw = {:.3f}, grf z = {:.3f}", + time, + Utils::Squeeze(ocp.objective(variables_.Get())), + ocp.equalityConstraints(variables_.Get()).lpNorm(), + maxCoeff, + maxCoeffIndex, + variables_.Get(reference_position, 0_step).z(), + variables_.Get(position, 0_step).z(), + Utils::QuaternionToYawPitchRoll(variables_.Get(reference_orientation, 0_step)).z(), + Utils::QuaternionToYawPitchRoll(variables_.Get(orientation, 0_step)).z(), + fmt::join(enumerate(NUM_LEGS) | std::views::transform([&](const auto i) { + return variables_.Get(reference_contact_state, 0_step, i) * + variables_.Get(ground_reaction_force, 0_step, i).z(); + }), + ", ")); + } + + return 0; +} diff --git a/example/mpc/rc_car.example.cpp b/example/mpc/rc_car.example.cpp new file mode 100644 index 0000000..3ccfdc8 --- /dev/null +++ b/example/mpc/rc_car.example.cpp @@ -0,0 +1,402 @@ +/****************************************************************************** + * + * @file ungar/example/mpc/rc_car.example.cpp + * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) + * + * @section LICENSE + * ----------------------------------------------------------------------- + * + * Copyright 2023 Flavio De Vincenti + * + * ----------------------------------------------------------------------- + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an "AS + * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either + * express or implied. See the License for the specific language + * governing permissions and limitations under the License. + * + * ----------------------------------------------------------------------- + * + * @section DESCRIPTION + * + * This file implements a nonlinear model predictive controller for + * the autonomous racing of 1:43 scale RC cars. The dynamical models + * and physical parameters are based on [1]. + * + * @see [1] Alexander Liniger, Alexander Domahidi and Manfred Morari. + * "Optimization‐based autonomous racing of 1:43 scale RC cars." + * Optimal Control Applications and Methods 36 (2015): 628 - 647. + * + ******************************************************************************/ + +#include "ungar/autodiff/vector_composer.hpp" +#include "ungar/optimization/soft_sqp.hpp" +#include "ungar/variable_map.hpp" + +int main() { + using namespace Ungar; + + /*======================================================================================*/ + /*~~~~~~~~~~~~~| PART I: RC CAR VARIABLES |~~~~~~~~~~~~~*/ + /*======================================================================================*/ + /*************** Define numeric invariants as integral constants. ***************/ + constexpr auto N = 30_c; // Discrete time horizon. + + /*************** Define decision variables. ***************/ + // RC cars navigate on flat terrains, so positions are 2-dimensional vectors, + // orientations are scalars, etc. + UNGAR_VARIABLE(position, 2); // := p + UNGAR_VARIABLE(yaw, 1); // := phi + UNGAR_VARIABLE(b_linear_velocity, 2); // := v + UNGAR_VARIABLE(yaw_rate, 1); // := omega + UNGAR_VARIABLE(x) <<= (position, yaw, b_linear_velocity, yaw_rate); // x := [p phi v omega] + UNGAR_VARIABLE(X) <<= (N + 1_c) * x; // X := [x0 x1 ... xN] + + // The control inputs are the PWM duty cycle of the electric drive train motor and + // the steering angle. + UNGAR_VARIABLE(pwm_duty_cycle, 1); // := d + UNGAR_VARIABLE(steering_angle, 1); // := delta + UNGAR_VARIABLE(u) <<= (pwm_duty_cycle, steering_angle); // u := [d delta] + UNGAR_VARIABLE(U) <<= N * u; // U := [u0 u1 ... uN-1] + + /*************** Define parameters. ***************/ + // Step size. + UNGAR_VARIABLE(step_size, 1); + + // Inertial and geometric parameters. + UNGAR_VARIABLE(mass, 1); + UNGAR_VARIABLE(b_moi, 1); + UNGAR_VARIABLE(front_wheel_distance, 1); + UNGAR_VARIABLE(rear_wheel_distance, 1); + + // Simplified Pacejka tire model parameters. + UNGAR_VARIABLE(ptm_front_b, 1); + UNGAR_VARIABLE(ptm_front_c, 1); + UNGAR_VARIABLE(ptm_front_d, 1); + UNGAR_VARIABLE(ptm_rear_b, 1); + UNGAR_VARIABLE(ptm_rear_c, 1); + UNGAR_VARIABLE(ptm_rear_d, 1); + UNGAR_VARIABLE(ptm_cm1, 1); + UNGAR_VARIABLE(ptm_cm2, 1); + UNGAR_VARIABLE(ptm_cr0, 1); + UNGAR_VARIABLE(ptm_cr2, 1); + + // Reference trajectories. + UNGAR_VARIABLE(reference_position, 2); + UNGAR_VARIABLE(reference_trajectory) <<= (N + 1_c) * reference_position; + + // Measurements. + UNGAR_VARIABLE(measured_position, 2); + UNGAR_VARIABLE(measured_yaw, 1); + UNGAR_VARIABLE(b_measured_linear_velocity, 2); + UNGAR_VARIABLE(measured_yaw_rate, 1); + UNGAR_VARIABLE(measured_state) <<= + (measured_position, measured_yaw, b_measured_linear_velocity, measured_yaw_rate); + + /*************** Define variables. ***************/ + UNGAR_VARIABLE(decision_variables) <<= (X, U); + UNGAR_VARIABLE(parameters) <<= (step_size, + mass, + b_moi, + front_wheel_distance, + rear_wheel_distance, + ptm_front_b, + ptm_front_c, + ptm_front_d, + ptm_rear_b, + ptm_rear_c, + ptm_rear_d, + ptm_cm1, + ptm_cm2, + ptm_cr0, + ptm_cr2, + reference_trajectory, + measured_state); + UNGAR_VARIABLE(variables) <<= (decision_variables, parameters); + + /*======================================================================================*/ + /*~~~~~~~~~~~~~| PART II: RC CAR DYNAMICS |~~~~~~~~~~~~~*/ + /*======================================================================================*/ + /// @brief Given vectors of autodiff scalars corresponding to the system's state, + /// input and parameters at a given time step, compute the state at the + /// next time step using a semi-implicit Euler method. + /*************** Define discrete-time RC car dynamics equation. ***************/ + const auto rcCarDynamics = [&](const VectorXad& xUnderlying, + const VectorXad& uUnderlying, + const VectorXad& parametersUnderlying) -> VectorXad { + // Create variable lazy maps for the system's state, input and parameters. + /// @note As a convention, we name the underlying data representation of a + /// variable \c v as \c vUnderlying, and we name \c v_ the associated + /// map object. + const auto x_ = MakeVariableLazyMap(xUnderlying, x); + const auto u_ = MakeVariableLazyMap(uUnderlying, u); + const auto parameters_ = MakeVariableLazyMap(parametersUnderlying, parameters); + + // Retrieve all variables. + const auto& dt = parameters_.Get(step_size); + + const auto [Bf, Cf, Df] = parameters_.GetTuple(ptm_front_b, ptm_front_c, ptm_front_d); + const auto [Br, Cr, Dr] = parameters_.GetTuple(ptm_rear_b, ptm_rear_c, ptm_rear_d); + const auto [Cm1, Cm2] = parameters_.GetTuple(ptm_cm1, ptm_cm2); + const auto [Cr0, Cr2] = parameters_.GetTuple(ptm_cr0, ptm_cr2); + + const auto [m, bMOI, lf, lr] = + parameters_.GetTuple(mass, b_moi, front_wheel_distance, rear_wheel_distance); + + const auto [p, phi, v, omega] = x_.GetTuple(position, yaw, b_linear_velocity, yaw_rate); + const auto [d, delta] = u_.GetTuple(pwm_duty_cycle, steering_angle); + + // Compute forces acting on the RC cars. + /// @note To avoid numerical issues, add a small constant to the numerators. + const ad_scalar_t alphaf = + -atan((omega * lf + v.y()) / (v.x() + Eigen::NumTraits::epsilon())) + delta; + const ad_scalar_t alphar = + atan((omega * lr - v.y()) / (v.x() + Eigen::NumTraits::epsilon())); + const ad_scalar_t Ffy = Df * sin(Cf * atan(Bf * alphaf)); + const ad_scalar_t Fry = Dr * sin(Cr * atan(Br * alphar)); + const ad_scalar_t Frx = (Cm1 - Cm2 * v.x()) * d - Cr0 - Cr2 * Utils::Pow(v.x(), 2); + + // Calculate linear acceleration and angular acceleration of the RC car. + const Vector2ad vDot{(Frx - Ffy * sin(delta) + m * v.y() * omega) / m, + (Fry + Ffy * cos(delta) - m * v.x() * omega) / m}; + const ad_scalar_t omegaDot = (Ffy * lf * cos(delta) - Fry * lr) / bMOI; + + // Create variable map with autodiff scalar type for the next state. + auto xNext_ = MakeVariableMap(x); + + auto [pNext, phiNext, vNext, omegaNext] = + xNext_.GetTuple(position, yaw, b_linear_velocity, yaw_rate); + + // Update next state using semi-implicit Euler method and return underlying data. + vNext = v + dt * vDot; + omegaNext = omega + dt * omegaDot; + pNext = p + dt * Vector2ad{vNext.x() * cos(phi) - vNext.y() * sin(phi), + vNext.x() * sin(phi) + vNext.y() * cos(phi)}; + phiNext = phi + dt * omegaNext; + + return xNext_.Get(); + }; + + /*======================================================================================*/ + /*~~~~~~~~~~~~~| PART III: OPTIMAL CONTROL PROBLEM |~~~~~~~~~~~~~*/ + /*======================================================================================*/ + /// @brief Given vectors of autodiff scalars corresponding to the system's variables, + /// implement an optimal control problem (OCP) for the RC car. The objective + /// function only comprises position tracking and input regularization goals. + /// The equality constraints enforce the system dynamics, while the inequality + /// constraints bound the inputs and impose a minimum forward velocity of + /// 0.3 m/s for the RC car. + /*************** Define objective function. ***************/ + const auto objectiveFunction = [&](const VectorXad& variablesUnderlying, + VectorXad& objectiveFunctionUnderlying) { + // Create variable lazy maps for the system's variables, which include both + // decision variables and parameters. + const auto variables_ = MakeVariableLazyMap(variablesUnderlying, variables); + + ad_scalar_t value{0.0}; + for (const auto k : enumerate(N)) { + const auto p = variables_.Get(position, k); + const auto pRef = variables_.Get(reference_position, k); + + const auto uk = variables_.Get(u, k); + + // Reference position tracking. + value += (p - pRef).squaredNorm(); + + // Input regularization. + value += 1e-6 * uk.squaredNorm(); + if (k) { + // Regularization of input variations. + const auto ukm1 = variables_.Get(u, k - 1_step); + value += 1e-6 * (uk - ukm1).squaredNorm(); + } + } + // Final reference position tracking. + const auto p = variables_.Get(position, N); + const auto pRef = variables_.Get(reference_position, N); + value += (p - pRef).squaredNorm(); + + /// @note Autodiff functions must return Eigen vectors, therefore the + /// objective function returns a vector of size 1 containing the + /// objective value. + objectiveFunctionUnderlying.resize(1_idx); + objectiveFunctionUnderlying << value; + }; + + /*************** Define equality constraints. ***************/ + /// @brief Equality constraints are a function \c g of \c variables such that + /// \c g(variables) = 0. + const auto equalityConstraints = [&](const VectorXad& variablesUnderlying, + VectorXad& equalityConstraintsUnderlying) { + const auto variables_ = MakeVariableLazyMap(variablesUnderlying, variables); + + // Define helper for composing equality constraints into a single Eigen vector. + Autodiff::VectorComposer composer; + + // Add equality constraint for the initial state. + const auto x0 = variables_.Get(x, 0_step); + const auto xm = variables_.Get(measured_state); + composer << x0 - xm; + + // Add system dynamics constraint for each time step. + for (const auto k : enumerate(N)) { + const auto xk = variables_.Get(x, k); + const auto xkp1 = variables_.Get(x, k + 1_step); + const auto uk = variables_.Get(u, k); + const auto params = variables_.Get(parameters); + + composer << xkp1 - rcCarDynamics(xk, uk, params); + } + + equalityConstraintsUnderlying = composer.Compose(); + }; + + /*************** Define inequality constraints. ***************/ + /// @brief Inequality constraints are a function \c h of \c variables such that + /// \c h(variables) <= 0. + const auto inequalityConstraints = [&](const VectorXad& variablesUnderlying, + VectorXad& inequalityConstraintsUnderlying) { + const auto variables_ = MakeVariableLazyMap(variablesUnderlying, variables); + + // Define helper for composing inequality constraints into a single Eigen vector. + Autodiff::VectorComposer composer; + + for (const auto k : enumerate(N)) { + const auto v = variables_.Get(b_linear_velocity, k); + const auto& d = variables_.Get(pwm_duty_cycle, k); + const auto& delta = variables_.Get(steering_angle, k); + + // Input bound constraints. + composer << Utils::Abs(d) - 15.0; + composer << Utils::Abs(delta) - 15.0; + + // Minimum forward velocity constraint. + composer << 0.3 - v.x(); + } + + inequalityConstraintsUnderlying = composer.Compose(); + }; + + /*************** Define optimal control problem (OCP). ***************/ + // Based on the autodiff functions defined above, generate code for the + // corresponding derivatives and compile it just-in-time. + Autodiff::Function::Blueprint objectiveFunctionBlueprint{objectiveFunction, + decision_variables.Size(), + parameters.Size(), + "rc_car_mpc_obj"sv, + EnabledDerivatives::ALL}; + Autodiff::Function::Blueprint equalityConstraintsBlueprint{equalityConstraints, + decision_variables.Size(), + parameters.Size(), + "rc_car_mpc_eqs"sv, + EnabledDerivatives::JACOBIAN}; + Autodiff::Function::Blueprint inequalityConstraintsBlueprint{inequalityConstraints, + decision_variables.Size(), + parameters.Size(), + "rc_car_mpc_ineqs"sv, + EnabledDerivatives::JACOBIAN}; + + const bool recompileLibraries = true; + auto ocp = + MakeNLPProblem(Autodiff::MakeFunction(objectiveFunctionBlueprint, recompileLibraries), + Autodiff::MakeFunction(equalityConstraintsBlueprint, recompileLibraries), + Autodiff::MakeFunction(inequalityConstraintsBlueprint, recompileLibraries)); + + /*======================================================================================*/ + /*~~~~~~~~~~~~~| PART IV: MODEL PREDICTIVE CONTROL |~~~~~~~~~~~~~*/ + /*======================================================================================*/ + /*************** Initialize OCP variables. ***************/ + // Create variable map storing all decision variables and parameters for the RC car. + auto variables_ = MakeVariableMap(variables); + + // Inertial and geometric parameters. + variables_.Get(mass) = 0.041; + variables_.Get(b_moi) = 27.8e-6; + variables_.Get(front_wheel_distance) = 0.029; + variables_.Get(rear_wheel_distance) = 0.033; + + variables_.Get(step_size) = 1.0 / static_cast(N); + + // Simplified Pacejka tire model parameters. + variables_.Get(ptm_front_b) = 2.579; + variables_.Get(ptm_front_c) = 1.2; + variables_.Get(ptm_front_d) = 0.192; + variables_.Get(ptm_rear_b) = 3.3852; + variables_.Get(ptm_rear_c) = 1.2691; + variables_.Get(ptm_rear_d) = 0.1737; + variables_.Get(ptm_cm1) = 0.287; + variables_.Get(ptm_cm2) = 0.0545; + variables_.Get(ptm_cr0) = 0.0518; + variables_.Get(ptm_cr2) = 0.00035; + + // Measurements. + variables_.Get(measured_position).setZero(); + variables_.Get(measured_yaw) = 0.0; + variables_.Get(b_measured_linear_velocity).setUnit(0_idx); + variables_.Get(measured_yaw_rate) = 0.0; + + // Decision variables. + for (const auto k : enumerate(N + 1_step)) { + variables_.Get(x, k) = variables_.Get(measured_state); + + const real_t t = static_cast(k) * variables_.Get(step_size); + variables_.Get(position, k).x() = variables_.Get(b_measured_linear_velocity).x() * t; + } + variables_.Get(U).setZero(); + + // Reference trajectories. + /// @brief Command the RC car to have a constant velocity along the x-axis while + /// tracking a sinusoidal trajectory along the y-axis. + const real_t xDotReference = 1.0; + const real_t yPeriodReference = 8.0; + const real_t yAmplitudeReference = 0.2; + + /*************** Solve OCP over receding horizon. ***************/ + // Define OCP optimizer. + SoftSQPOptimizer optimizer{false, variables_.Get(step_size), 40_idx, 100.0, 1e-2}; + + const real_t finalTime = 10.0; + for (real_t time = 0.0; time < finalTime; time += variables_.Get(step_size)) { + // Integrate RC car dynamics using the optimized input. + variables_.Get(measured_state) = Utils::ToRealFunction(rcCarDynamics)( + variables_.Get(measured_state), variables_.Get(u, 0_step), variables_.Get(parameters)); + + // Update reference trajectories. + for (const auto k : enumerate(N + 1_step)) { + const real_t t = time + static_cast(k) * variables_.Get(step_size); + + variables_.Get(reference_position, k) << xDotReference * t, + yAmplitudeReference * sin(2.0 * std::numbers::pi / yPeriodReference * t); + } + + // Shift the previous solution by one time step to warm start the optimization. + for (const auto k : enumerate(N - 1_step)) { + variables_.Get(x, k) = variables_.Get(x, k + 1_step); + variables_.Get(u, k) = variables_.Get(u, k + 1_step); + } + variables_.Get(x, N - 1_step) = variables_.Get(x, N); + + // Solve OCP and log optimization results. + variables_.Get(decision_variables) = optimizer.Optimize(ocp, variables_.Get()); + UNGAR_LOG(info, + "t = {:.3f}, obj = {:.3f}, eqs = {:.3f}, ineqs = {:.3f}, p ref = [{:.3f}], p = " + "[{:.3f}], d = {:.3f}, delta = {:.3f}", + time, + Utils::Squeeze(ocp.objective(variables_.Get())), + ocp.equalityConstraints(variables_.Get()).lpNorm(), + ocp.inequalityConstraints(variables_.Get()).maxCoeff(), + fmt::join(variables_.Get(reference_position, 0_step), ", "), + fmt::join(variables_.Get(position, 0_step), ", "), + variables_.Get(pwm_duty_cycle, 0_step), + variables_.Get(steering_angle, 0_step)); + } + + return 0; +} diff --git a/example/variable.example.cpp b/example/variable.example.cpp new file mode 100644 index 0000000..9f27814 --- /dev/null +++ b/example/variable.example.cpp @@ -0,0 +1,130 @@ +/****************************************************************************** + * + * @file ungar/example/variable.example.cpp + * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) + * + * @section LICENSE + * ----------------------------------------------------------------------- + * + * Copyright 2023 Flavio De Vincenti + * + * ----------------------------------------------------------------------- + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an "AS + * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either + * express or implied. See the License for the specific language + * governing permissions and limitations under the License. + * + ******************************************************************************/ + +#include "ungar/variable.hpp" + +int main() { + using namespace Ungar; + + /*************** SINGLE RIGID BODY MODEL ***************/ + /*************** Define numeric invariants as integral constants. ***************/ + constexpr auto N = 30_c; // Discrete time horizon. + constexpr auto NUM_LEGS = 4_c; + + /*************** Define "leaf" variables. ***************/ + // Positions are 3-dimensional vectors, orientations are unit quaternions, etc. + UNGAR_VARIABLE(position, 3); // := p + UNGAR_VARIABLE(orientation, Q); // := q + UNGAR_VARIABLE(linear_velocity, 3); // := pDot + UNGAR_VARIABLE(angular_velocity, 3); // := omega + + UNGAR_VARIABLE(relative_position, 3); // := r + UNGAR_VARIABLE(force, 3); // := f + + /*************** Define "branch" variables. ***************/ + // States are stacked poses and velocities, while inputs are ground reaction + // forces and relative positions for each foot. + UNGAR_VARIABLE(x) <<= + (position, orientation, linear_velocity, angular_velocity); // x := [p q pDot omega] + UNGAR_VARIABLE(leg_input) <<= (force, relative_position); // lu := [f r] + UNGAR_VARIABLE(u) <<= NUM_LEGS * leg_input; // u := [lu0 lu1 lu2 lu3] + UNGAR_VARIABLE(X) <<= (N + 1_c) * x; // X := [x0 x1 ... xN] + UNGAR_VARIABLE(U) <<= N * u; // U := [u0 u1 ... uN-1] + + UNGAR_VARIABLE(variables) <<= (X, U); + + /*************** Retrieve information about variables at compile time. ***************/ + static_assert(position.Size() == 3); + static_assert(orientation.Size() == 4); + static_assert(x.Size() == position.Size() + orientation.Size() + linear_velocity.Size() + + angular_velocity.Size()); + static_assert(X.Size() == x.Size() * (N + 1)); + static_assert(variables.Size() == X.Size() + U.Size()); + + // Access subvariables using an intuitive syntax. + static_assert(variables(x, 0).Index() == 0); // [{x0} x1 x2 ... xN u0 ... ] + static_assert(variables(x, 1).Index() == 13); // [ x0 {x1} x2 ... xN u0 ... ] + static_assert(variables(x, 2).Index() == 26); // [ x0 x1 {x2} ... xN u0 ... ] + static_assert(variables(u, 0).Index() == X.Size()); // [ x0 x1 x2 ... xN {u0} ... ] + + static_assert(variables(u, 0, leg_input, 0).Index() == + X.Size()); // [ X {lu00} lu01 lu02 lu03 lu10 lu11 lu12 lu13 ... ] + static_assert(variables(u, 0, leg_input, 1).Index() == + X.Size() + 6); // [ X lu00 {lu01} lu02 lu03 lu10 lu11 lu12 lu13 ... ] + static_assert(variables(u, 0, leg_input, 2).Index() == + X.Size() + 12); // [ X lu00 lu01 {lu02} lu03 lu10 lu11 lu12 lu13 ... ] + static_assert(variables(u, 1, leg_input, 3).Index() == + X.Size() + u.Size() + + 18); // [ X lu00 lu01 lu02 lu03 lu10 lu11 lu12 {lu13} ... ] + + // Use concise notations to select subvariables when there is no ambiguity. + static_assert(variables(u, 0, leg_input, 0).Index() == variables(leg_input, 0, 0).Index()); + static_assert(variables(u, 0, leg_input, 1).Index() == variables(leg_input, 0, 1).Index()); + static_assert(variables(u, 0, leg_input, 2).Index() == variables(leg_input, 0, 2).Index()); + static_assert(variables(u, 1, leg_input, 3).Index() == variables(leg_input, 1, 3).Index()); + + static_assert(variables(u, 4, leg_input, 0, force).Index() == variables(force, 4, 0).Index()); + static_assert(variables(u, 4, leg_input, 1, force).Index() == variables(force, 4, 1).Index()); + static_assert(variables(u, 4, leg_input, 2, force).Index() == variables(force, 4, 2).Index()); + + // Use verbose notations if you prefer. + static_assert(X.At<"x">(0).Index() == X(x, 0).Index()); + static_assert(X.At<"x">(1).Index() == X(x, 1).Index()); + static_assert(X.At<"x">(2).Index() == X(x, 2).Index()); + static_assert(U.At<"u">(8).At<"leg_input">(2).At<"relative_position">().Index() == + U(relative_position, 8, 2).Index()); + + /*************** Operate on each subvariable in a given hierarchy. ***************/ + const auto getKind = [](Concepts::Variable auto v) { + if (v.IsScalar()) { + return "'scalar'"s; + } else if (v.IsVector()) { + return "'vector'"s; + } else if (v.IsQuaternion()) { + return "'quaternion'"s; + } else { + return "'unknown'"s; + } + }; + + // Define a Boost.Hana struct to benefit from the printing capabilities of UNGAR_LOG. + struct VariableInfo { + BOOST_HANA_DEFINE_STRUCT(VariableInfo, + (std::string, name), + (index_t, index), + (index_t, size), + (std::string, kind)); + }; + + UNGAR_LOG(info, + "Printing all subvariables of 'x' with the format {{ name, index, size, kind }}..."); + x.ForEach([&](Concepts::Variable auto var) { + UNGAR_LOG( + info, "{:c}", VariableInfo{var.Name().c_str(), var.Index(), var.Size(), getKind(var)}); + }); + + return 0; +} diff --git a/example/variable_map.example.cpp b/example/variable_map.example.cpp new file mode 100644 index 0000000..f25789a --- /dev/null +++ b/example/variable_map.example.cpp @@ -0,0 +1,80 @@ +/****************************************************************************** + * + * @file ungar/example/variable_map.example.cpp + * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) + * + * @section LICENSE + * ----------------------------------------------------------------------- + * + * Copyright 2023 Flavio De Vincenti + * + * ----------------------------------------------------------------------- + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an "AS + * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either + * express or implied. See the License for the specific language + * governing permissions and limitations under the License. + * + ******************************************************************************/ + +#include "ungar/variable_map.hpp" + +int main() { + using namespace Ungar; + + /*************** QUADROTOR MODEL ***************/ + /*************** Define numeric invariants as integral constants. ***************/ + constexpr auto N = 30_c; // Discrete time horizon. + constexpr auto NUM_ROTORS = 4_c; + + /*************** Define "leaf" variables. ***************/ + // Positions are 3-dimensional vectors, orientations are unit quaternions, + // rotor speeds are scalars, etc. + constexpr auto position = var_c<"position", 3>; // := p + constexpr auto orientation = var_c<"orientation", Q>; // := q + constexpr auto linear_velocity = var_c<"linear_velocity", 3>; // := pDot + constexpr auto angular_velocity = var_c<"angular_velocity", 3>; // := omega + constexpr auto rotor_speed = var_c<"rotor_speed", 1>; // := r + + /*************** Define "branch" variables. ***************/ + // States are stacked poses and velocities, while inputs are stacked rotor + // speeds: one for each rotor. + constexpr auto x = var_c<"x"> <<= + (position, orientation, linear_velocity, angular_velocity); // x := [p q pDot omega] + constexpr auto u = var_c<"u"> <<= NUM_ROTORS * rotor_speed; // u := [r0 r1 r2 r3] + constexpr auto X = var_c<"X"> <<= (N + 1_c) * x; // X := [x0 x1 ... xN] + constexpr auto U = var_c<"U"> <<= N * u; // U := [u0 u1 ... uN-1] + + constexpr auto variables = var_c<"variables"> <<= (X, U); + + /*************** Instantiate and use variable maps. ***************/ + // Access information about variables at compile time. + static_assert(x.Size() == 13); + static_assert(variables(x, 0).Index() == 0); // [{x0} x1 x2 ... xN u0 ... ] + static_assert(variables(x, 1).Index() == 13); // [ x0 {x1} x2 ... xN u0 ... ] + static_assert(variables(x, 2).Index() == 26); // [ x0 x1 {x2} ... xN u0 ... ] + static_assert(variables(u, 0).Index() == X.Size()); // [ x0 x1 x2 ... xN {u0} ... ] + + // Create maps over contiguous arrays of data. + auto vars = MakeVariableMap(variables); // [ X u0 u1 ... uN-1 ] + vars.Get(u, 0).setZero(); // [ X 0000 u1 ... uN-1 ] + vars.Get(u, 1).setOnes(); // [ X 0000 1111 ... uN-1 ] + vars.Get(u, N - 1).setLinSpaced(2.0, 8.0); // [ X 0000 1111 ... 2468 ] + UNGAR_LOG(info, "u0 = {}", vars.Get(u, 0)); + UNGAR_LOG(info, "u1 = {}", vars.Get(u, 1)); + UNGAR_LOG(info, "uN-1 = {}", vars.Get(u, N - 1)); + + static_assert(std::same_as); + static_assert(std::same_as&>); + static_assert(std::same_as&>); + static_assert(std::same_as>&>); + + return 0; +} diff --git a/external/CMakeLists.txt b/external/CMakeLists.txt new file mode 100644 index 0000000..f725161 --- /dev/null +++ b/external/CMakeLists.txt @@ -0,0 +1,390 @@ +# ############################################################################## +# Define utility function. +# ############################################################################## +# Download and install an external project according to a provided configuration +# file. +function(download_external_project external_project_config_file + external_project_name) + # Create external project 'CMakeLists.txt' file inside the build folder. + configure_file( + ${external_project_config_file} + "${CMAKE_CURRENT_BINARY_DIR}/${external_project_name}/CMakeLists.txt" @ONLY) + + # Create external project build directory 'ep-build'. + file(MAKE_DIRECTORY + ${CMAKE_CURRENT_BINARY_DIR}/${external_project_name}/ep-build) + + # Build external project. + execute_process( + COMMAND ${CMAKE_COMMAND} -G "${CMAKE_GENERATOR}" .. + WORKING_DIRECTORY + ${CMAKE_CURRENT_BINARY_DIR}/${external_project_name}/ep-build) + execute_process( + COMMAND ${CMAKE_COMMAND} --build . + WORKING_DIRECTORY + ${CMAKE_CURRENT_BINARY_DIR}/${external_project_name}/ep-build) +endfunction() + +# ############################################################################## +# Define dependency directories. +# ############################################################################## +# Eigen. +if(DEFINED Eigen3_ROOT) + set(EIGEN3_ROOT_DIRECTORY ${Eigen3_ROOT}) +elseif(NOT UNGAR_USE_SYSTEM_LIBRARIES) + set(USE_BUNDLED_EIGEN ON) + set(BUNDLED_EIGEN_FILENAME + ${CMAKE_CURRENT_LIST_DIR}/config/eigen/eigen-3.4.0.zip) + set(EIGEN3_ROOT_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/eigen/install) +endif() + +# Hana. +if(DEFINED Hana_ROOT) + set(HANA_ROOT_DIRECTORY ${Hana_ROOT}) +elseif(NOT UNGAR_USE_SYSTEM_LIBRARIES) + set(USE_BUNDLED_HANA ON) + set(BUNDLED_HANA_FILENAME + ${CMAKE_CURRENT_LIST_DIR}/config/hana/hana-boost-1.84.0.zip) + set(HANA_ROOT_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/hana/install) +endif() + +# GoogleTest. +if(DEFINED GTest_ROOT) + set(GTEST_ROOT_DIRECTORY ${GTest_ROOT}) +elseif(NOT UNGAR_USE_SYSTEM_LIBRARIES) + set(USE_DOWNLOADED_GOOGLETEST ON) + set(GTEST_ROOT_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/googletest/install) +endif() + +# spdlog. +if(DEFINED spdlog_ROOT) + set(SPDLOG_ROOT_DIRECTORY ${spdlog_ROOT}) +elseif(NOT UNGAR_USE_SYSTEM_LIBRARIES) + set(USE_DOWNLOADED_SPDLOG ON) + set(SPDLOG_ROOT_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/spdlog/install) +endif() + +# FiniteDiff. +if(DEFINED FiniteDiff_ROOT) + set(FINITEDIFF_ROOT_DIRECTORY ${FiniteDiff_ROOT}) +elseif(NOT UNGAR_USE_SYSTEM_LIBRARIES) + set(USE_DOWNLOADED_FINITEDIFF ON) + set(FINITEDIFF_ROOT_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/finite-diff/install) +endif() + +# Abseil. +if(DEFINED absl_ROOT) + set(ABSL_ROOT_DIRECTORY ${absl_ROOT}) +elseif(NOT UNGAR_USE_SYSTEM_LIBRARIES) + set(USE_DOWNLOADED_ABSEIL ON) + set(ABSL_ROOT_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/abseil-cpp/install) +endif() + +# OSQP. +if(DEFINED osqp_ROOT) + set(OSQP_ROOT_DIRECTORY ${osqp_ROOT}) +elseif(NOT UNGAR_USE_SYSTEM_LIBRARIES) + set(USE_DOWNLOADED_OSQP ON) + set(OSQP_ROOT_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/osqp/install) +endif() + +# osqp-cpp. +if(DEFINED osqp-cpp_ROOT) + set(OSQP_CPP_ROOT_DIRECTORY ${osqp-cpp_ROOT}) +elseif(NOT UNGAR_USE_SYSTEM_LIBRARIES) + set(USE_DOWNLOADED_OSQPCPP ON) + set(OSQP_CPP_ROOT_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/osqp-cpp/install) +endif() + +# CppAD. +if(DEFINED CppAD_ROOT) + set(CPPAD_ROOT_DIRECTORY ${CppAD_ROOT}) +elseif(NOT UNGAR_USE_SYSTEM_LIBRARIES) + set(USE_DOWNLOADED_CPPAD ON) + set(CPPAD_ROOT_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/CppAD/install) +endif() + +# CppADCodeGen. +if(DEFINED CppADCodeGen_ROOT) + set(CPPADCODEGEN_ROOT_DIRECTORY ${CppADCodeGen_ROOT}) +elseif(NOT UNGAR_USE_SYSTEM_LIBRARIES) + set(USE_DOWNLOADED_CPPADCODEGEN ON) + set(CPPADCODEGEN_ROOT_DIRECTORY + ${CMAKE_CURRENT_BINARY_DIR}/CppADCodeGen/install) +endif() + +# =========================================================================== +# 1) CORE DEPENDENCIES +# ############################################################################## +# Add Eigen. +# ############################################################################## +message(STATUS "--------------------------------------------------") +if(USE_BUNDLED_EIGEN) + message(STATUS "Using bundled Eigen...") + download_external_project(config/eigen/CMakeLists.txt.in eigen) + find_package(Eigen3 REQUIRED PATHS + ${EIGEN3_ROOT_DIRECTORY}/share/eigen3/cmake NO_DEFAULT_PATH) +else() + message(STATUS "Using system-wide Eigen...") + find_package(Eigen3 3.4.0 REQUIRED NO_MODULE) +endif() +set_target_properties(Eigen3::Eigen PROPERTIES IMPORTED_GLOBAL TRUE) + +# If the Eigen package is found, it defines the variable 'EIGEN3_ROOT_DIR'. +# However, this feature is deprecated and 'EIGEN3_ROOT_DIR' may not be defined. +if(DEFINED EIGEN3_ROOT_DIR) + message(STATUS "Eigen found at '${EIGEN3_ROOT_DIR}'.") +elseif(DEFINED EIGEN3_ROOT_DIRECTORY) + message(STATUS "Eigen found at '${EIGEN3_ROOT_DIRECTORY}'.") +else() + message(STATUS "Eigen found.") +endif() + +# ############################################################################## +# Add Hana. +# ############################################################################## +message(STATUS "--------------------------------------------------") +if(USE_BUNDLED_HANA) + message(STATUS "Using bundled Hana...") + download_external_project(config/hana/CMakeLists.txt.in hana) + find_package(Hana REQUIRED PATHS ${HANA_ROOT_DIRECTORY}/lib/cmake/hana + NO_DEFAULT_PATH) +else() + message(STATUS "Using system-wide Hana...") + find_package(Hana REQUIRED) +endif() +set_target_properties(hana PROPERTIES IMPORTED_GLOBAL TRUE) + +if(DEFINED HANA_ROOT_DIRECTORY) + message(STATUS "Hana found at '${HANA_ROOT_DIRECTORY}'.") +else() + message(STATUS "Hana found.") +endif() + +# =========================================================================== +# 2) TESTING +if(UNGAR_BUILD_TESTS) + # ############################################################################ + # Add GoogleTest. + # ############################################################################ + message(STATUS "--------------------------------------------------") + include(GoogleTest) + + if(USE_DOWNLOADED_GOOGLETEST) + message(STATUS "Using locally downloaded GoogleTest...") + download_external_project(config/googletest/CMakeLists.txt.in googletest) + find_package(GTest CONFIG REQUIRED PATHS + ${GTEST_ROOT_DIRECTORY}/lib/cmake/GTest NO_DEFAULT_PATH) + else() + message(STATUS "Using system-wide GoogleTest...") + find_package(GTest CONFIG REQUIRED) + endif() + set_target_properties(GTest::gtest PROPERTIES IMPORTED_GLOBAL TRUE) + + if(DEFINED GTEST_ROOT_DIRECTORY) + message(STATUS "GoogleTest found at '${GTEST_ROOT_DIRECTORY}'.") + else() + message(STATUS "GoogleTest found.") + endif() + +endif(UNGAR_BUILD_TESTS) + +# =========================================================================== +# 3) LOGGING +if(UNGAR_ENABLE_LOGGING) + # ############################################################################ + # Add spdlog. + # ############################################################################ + message(STATUS "--------------------------------------------------") + if(USE_DOWNLOADED_SPDLOG) + message(STATUS "Using locally downloaded spdlog...") + download_external_project(config/spdlog/CMakeLists.txt.in spdlog) + find_package(spdlog REQUIRED PATHS + ${SPDLOG_ROOT_DIRECTORY}/lib/cmake/spdlog NO_DEFAULT_PATH) + else() + message(STATUS "Using system-wide spdlog...") + find_package(spdlog REQUIRED) + endif() + set_target_properties(spdlog::spdlog PROPERTIES IMPORTED_GLOBAL TRUE) + set_target_properties(spdlog::spdlog_header_only PROPERTIES IMPORTED_GLOBAL + TRUE) + + if(DEFINED SPDLOG_ROOT_DIRECTORY) + message(STATUS "spdlog found at '${SPDLOG_ROOT_DIRECTORY}'.") + else() + message(STATUS "spdlog found.") + endif() + +endif(UNGAR_ENABLE_LOGGING) + +# =========================================================================== +# 4) AUTOMATIC DIFFERENTIATION +if(UNGAR_ENABLE_AUTODIFF) + # ############################################################################ + # Add FiniteDiff. + # ############################################################################ + message(STATUS "--------------------------------------------------") + if(USE_DOWNLOADED_FINITEDIFF) + message(STATUS "Using locally downloaded FiniteDiff...") + download_external_project(config/finite-diff/CMakeLists.txt.in finite-diff) + find_package( + FiniteDiff REQUIRED PATHS + ${FINITEDIFF_ROOT_DIRECTORY}/lib/cmake/finitediff NO_DEFAULT_PATH) + else() + message(STATUS "Using system-wide FiniteDiff...") + find_package(FiniteDiff REQUIRED) + endif() + set_target_properties(finitediff::finitediff PROPERTIES IMPORTED_GLOBAL TRUE) + + if(DEFINED FINITEDIFF_ROOT_DIRECTORY) + message(STATUS "FiniteDiff found at '${FINITEDIFF_ROOT_DIRECTORY}'.") + else() + message(STATUS "FiniteDiff found.") + endif() + + # ############################################################################ + # Add CppAD. + # ############################################################################ + message(STATUS "--------------------------------------------------") + if(USE_DOWNLOADED_CPPAD) + message(STATUS "Using locally downloaded CppAD...") + download_external_project(config/CppAD/CMakeLists.txt.in CppAD) + else() + message(STATUS "Using system-wide CppAD...") + endif() + + if(DEFINED CPPAD_ROOT_DIRECTORY) + set(CPPAD_LIB_DIRECTORY ${CPPAD_ROOT_DIRECTORY}/lib) + set(CPPAD_INCLUDE_DIRECTORY ${CPPAD_ROOT_DIRECTORY}/include) + + find_library( + CppAD_LIBRARY cppad_lib + PATHS ${CPPAD_LIB_DIRECTORY} REQUIRED + NO_DEFAULT_PATH) + find_path( + CppAD_INCLUDE_DIR cppad/cppad.hpp + PATHS ${CPPAD_INCLUDE_DIRECTORY} REQUIRED + NO_DEFAULT_PATH) + else() + find_library(CppAD_LIBRARY cppad_lib REQUIRED) + find_path(CppAD_INCLUDE_DIR cppad/cppad.hpp REQUIRED) + endif() + + if(DEFINED CPPAD_ROOT_DIRECTORY) + message(STATUS "CppAD found at '${CPPAD_ROOT_DIRECTORY}'.") + else() + message(STATUS "CppAD found.") + endif() + + # Create target for CppAD. + add_library(CppAD SHARED IMPORTED GLOBAL) + add_library(CppAD::CppAD ALIAS CppAD) + target_include_directories(CppAD INTERFACE ${CppAD_INCLUDE_DIR}) + + get_filename_component(CppAD_LIBRARY_NAME ${CppAD_LIBRARY} NAME) + get_filename_component(CppAD_LIBRARY_REAL_PATH ${CppAD_LIBRARY} REALPATH) + + set_target_properties(CppAD PROPERTIES IMPORTED_SONAME ${CppAD_LIBRARY_NAME}) + set_target_properties(CppAD PROPERTIES IMPORTED_LOCATION + ${CppAD_LIBRARY_REAL_PATH}) + + # ############################################################################ + # Add CppADCodeGen. + # ############################################################################ + message(STATUS "--------------------------------------------------") + if(USE_DOWNLOADED_CPPADCODEGEN) + message(STATUS "Using locally downloaded CppADCodeGen...") + download_external_project(config/CppADCodeGen/CMakeLists.txt.in + CppADCodeGen) + else() + message(STATUS "Using system-wide CppADCodeGen...") + endif() + + if(DEFINED CPPADCODEGEN_ROOT_DIRECTORY) + set(CPPADCODEGEN_INCLUDE_DIRECTORY ${CPPADCODEGEN_ROOT_DIRECTORY}/include) + find_path( + CppADCodeGen_INCLUDE_DIR cppad/cg.hpp + PATHS ${CPPADCODEGEN_INCLUDE_DIRECTORY} REQUIRED + NO_DEFAULT_PATH) + else() + find_path(CppADCodeGen_INCLUDE_DIR cppad/cg.hpp REQUIRED) + endif() + + if(DEFINED CPPADCODEGEN_ROOT_DIRECTORY) + message(STATUS "CppADCodeGen found at '${CPPADCODEGEN_ROOT_DIRECTORY}'.") + else() + message(STATUS "CppADCodeGen found.") + endif() + + # Create target for CppADCodeGen. + add_library(CppADCodeGen INTERFACE IMPORTED GLOBAL) + add_library(CppADCodeGen::CppADCodeGen ALIAS CppADCodeGen) + target_include_directories(CppADCodeGen INTERFACE ${CppADCodeGen_INCLUDE_DIR}) + +endif(UNGAR_ENABLE_AUTODIFF) + +# =========================================================================== +# 5) OPTIMIZATION +if(UNGAR_ENABLE_OPTIMIZATION) + # ############################################################################ + # Add Abseil. + # ############################################################################ + message(STATUS "--------------------------------------------------") + if(USE_DOWNLOADED_ABSEIL) + message(STATUS "Using locally downloaded Abseil...") + download_external_project(config/abseil-cpp/CMakeLists.txt.in abseil-cpp) + find_package(absl REQUIRED PATHS ${ABSL_ROOT_DIRECTORY}/lib/cmake/absl + NO_DEFAULT_PATH) + else() + message(STATUS "Using system-wide Abseil...") + find_package(absl REQUIRED) + endif() + + if(DEFINED ABSL_ROOT_DIRECTORY) + message(STATUS "Abseil found at '${ABSL_ROOT_DIRECTORY}'.") + else() + message(STATUS "Abseil found.") + endif() + + # ############################################################################ + # Add OSQP. + # ############################################################################ + message(STATUS "--------------------------------------------------") + if(USE_DOWNLOADED_OSQP) + message(STATUS "Using locally downloaded OSQP...") + download_external_project(config/osqp/CMakeLists.txt.in osqp) + find_package(osqp REQUIRED PATHS ${OSQP_ROOT_DIRECTORY}/lib/cmake/osqp + NO_DEFAULT_PATH) + else() + message(STATUS "Using system-wide OSQP...") + find_package(osqp REQUIRED) + endif() + + if(DEFINED OSQP_ROOT_DIRECTORY) + message(STATUS "OSQP found at '${OSQP_ROOT_DIRECTORY}'.") + else() + message(STATUS "OSQP found.") + endif() + + # ############################################################################ + # Add osqp-cpp. + # ############################################################################ + message(STATUS "--------------------------------------------------") + if(USE_DOWNLOADED_OSQPCPP) + message(STATUS "Using locally downloaded osqp-cpp...") + download_external_project(config/osqp-cpp/CMakeLists.txt.in osqp-cpp) + find_package(osqp-cpp REQUIRED PATHS + ${OSQP_CPP_ROOT_DIRECTORY}/lib/cmake/osqp-cpp NO_DEFAULT_PATH) + else() + message(STATUS "Using system-wide osqp-cpp...") + find_package(osqp-cpp REQUIRED) + endif() + set_target_properties(osqp-cpp::osqp-cpp PROPERTIES IMPORTED_GLOBAL TRUE) + + if(DEFINED OSQP_CPP_ROOT_DIRECTORY) + message(STATUS "osqp-cpp found at '${OSQP_CPP_ROOT_DIRECTORY}'.") + else() + message(STATUS "osqp-cpp found.") + endif() + +endif(UNGAR_ENABLE_OPTIMIZATION) diff --git a/external/config/CppAD/CMakeLists.txt.in b/external/config/CppAD/CMakeLists.txt.in new file mode 100644 index 0000000..e49ee39 --- /dev/null +++ b/external/config/CppAD/CMakeLists.txt.in @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.20) + +project(ExternalCppAD NONE) + +include(ExternalProject) + +if(NOT "@CppAD_CMAKE_FLAGS@" STREQUAL "") + message(STATUS "CppAD additional CMake flags: @CppAD_CMAKE_FLAGS@") +endif() + +ExternalProject_Add( + CppAD-20230000.0 + URL https://github.com/coin-or/CppAD/archive/refs/tags/20230000.0.zip + URL_HASH + SHA256=a5a6b32e026c01f71dba580552176cd9fc69b72ec3156c9438552d42068ac54a + CMAKE_CACHE_ARGS + -Dcppad_prefix:STRING=@CMAKE_CURRENT_BINARY_DIR@/CppAD/install + @CppAD_CMAKE_FLAGS@ + UPDATE_DISCONNECTED 1) diff --git a/external/config/CppADCodeGen/CMakeLists.txt.in b/external/config/CppADCodeGen/CMakeLists.txt.in new file mode 100644 index 0000000..30e2201 --- /dev/null +++ b/external/config/CppADCodeGen/CMakeLists.txt.in @@ -0,0 +1,22 @@ +cmake_minimum_required(VERSION 3.20) + +project(ExternalCppADCodeGen NONE) + +include(ExternalProject) + +if(NOT "@CppADCodeGen_CMAKE_FLAGS@" STREQUAL "") + message( + STATUS "CppADCodeGen additional CMake flags: @CppADCodeGen_CMAKE_FLAGS@") +endif() + +ExternalProject_Add( + CppADCodeGen-v2.4.3 + URL https://github.com/fdevinc/CppADCodeGen/archive/refs/tags/v2.4.3-ungar.zip + URL_HASH + SHA256=63adcd082b6de959763460797619dcdef92353498b8ccdf0a312ae0a91dd1d3a + CMAKE_CACHE_ARGS + -DCPPAD_HOME:STRING=@CPPAD_ROOT_DIRECTORY@/include + -DCMAKE_INSTALL_PREFIX:STRING=@CMAKE_CURRENT_BINARY_DIR@/CppADCodeGen/install + -DCPPADCODEGEN_BUILD_TESTS:BOOL=OFF + @CppADCodeGen_CMAKE_FLAGS@ + UPDATE_DISCONNECTED 1) diff --git a/external/config/abseil-cpp/CMakeLists.txt.in b/external/config/abseil-cpp/CMakeLists.txt.in new file mode 100644 index 0000000..01452e5 --- /dev/null +++ b/external/config/abseil-cpp/CMakeLists.txt.in @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.20) + +project(ExternalAbseil NONE) + +include(ExternalProject) + +if(NOT "@absl_CMAKE_FLAGS@" STREQUAL "") + message(STATUS "Abseil additional CMake flags: @absl_CMAKE_FLAGS@") +endif() + +ExternalProject_Add( + abseil-cpp-20230802.1 + URL https://github.com/abseil/abseil-cpp/archive/refs/tags/20230802.1.zip + URL_HASH + SHA256=497ebdc3a4885d9209b9bd416e8c3f71e7a1fb8af249f6c2a80b7cbeefcd7e21 + CMAKE_CACHE_ARGS + -DCMAKE_INSTALL_PREFIX:STRING=@CMAKE_CURRENT_BINARY_DIR@/abseil-cpp/install + @absl_CMAKE_FLAGS@ + UPDATE_DISCONNECTED 1) diff --git a/external/config/eigen/CMakeLists.txt.in b/external/config/eigen/CMakeLists.txt.in new file mode 100644 index 0000000..62b1894 --- /dev/null +++ b/external/config/eigen/CMakeLists.txt.in @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 3.20) + +project(ExternalEigen NONE) + +include(ExternalProject) + +if(NOT "@Eigen3_CMAKE_FLAGS@" STREQUAL "") + message(STATUS "Eigen additional CMake flags: @Eigen3_CMAKE_FLAGS@") +endif() + +ExternalProject_Add( + eigen-3.4.0 + URL @BUNDLED_EIGEN_FILENAME@ + URL_HASH + SHA256=1ccaabbfe870f60af3d6a519c53e09f3dcf630207321dffa553564a8e75c4fc8 + CMAKE_CACHE_ARGS + -DCMAKE_INSTALL_PREFIX:STRING=@CMAKE_CURRENT_BINARY_DIR@/eigen/install + -DEIGEN_BUILD_DOC:BOOL=OFF + -DBUILD_TESTING:BOOL=OFF + -DEIGEN_LEAVE_TEST_IN_ALL_TARGET:BOOL=OFF + -DEIGEN_BUILD_PKGCONFIG:BOOL=OFF + @Eigen3_CMAKE_FLAGS@ + UPDATE_DISCONNECTED 1) diff --git a/external/config/eigen/eigen-3.4.0.zip b/external/config/eigen/eigen-3.4.0.zip new file mode 100644 index 0000000..f44261f Binary files /dev/null and b/external/config/eigen/eigen-3.4.0.zip differ diff --git a/external/config/finite-diff/CMakeLists.txt.in b/external/config/finite-diff/CMakeLists.txt.in new file mode 100644 index 0000000..2a49f43 --- /dev/null +++ b/external/config/finite-diff/CMakeLists.txt.in @@ -0,0 +1,22 @@ +cmake_minimum_required(VERSION 3.20) + +project(ExternalFiniteDiff NONE) + +include(ExternalProject) + +if(NOT "@FiniteDiff_CMAKE_FLAGS@" STREQUAL "") + message(STATUS "FiniteDiff additional CMake flags: @FiniteDiff_CMAKE_FLAGS@") +endif() + +ExternalProject_Add( + finite-diff-v1.0.2-ungar + URL https://github.com/fdevinc/finite-diff/archive/refs/tags/v1.0.2-ungar.zip + URL_HASH + SHA256=9583bc34ab446d2313ded912fc791b7ad39e853d27f29dc78438bd3223cd2a85 + CMAKE_CACHE_ARGS + -DCMAKE_INSTALL_PREFIX:STRING=@CMAKE_CURRENT_BINARY_DIR@/finite-diff/install + -DFINITE_DIFF_INSTALL:BOOL=ON + -DEigen3_ROOT:STRING=@EIGEN3_ROOT_DIRECTORY@ + -Dspdlog_ROOT:STRING=@SPDLOG_ROOT_DIRECTORY@ + @FiniteDiff_CMAKE_FLAGS@ + UPDATE_DISCONNECTED 1) diff --git a/external/config/googletest/CMakeLists.txt.in b/external/config/googletest/CMakeLists.txt.in new file mode 100644 index 0000000..052a5d0 --- /dev/null +++ b/external/config/googletest/CMakeLists.txt.in @@ -0,0 +1,20 @@ +cmake_minimum_required(VERSION 3.20) + +project(ExternalGoogleTest NONE) + +include(ExternalProject) + +if(NOT "@GTest_CMAKE_FLAGS@" STREQUAL "") + message(STATUS "GoogleTest additional CMake flags: @GTest_CMAKE_FLAGS@") +endif() + +ExternalProject_Add( + googletest-v1.14.0 + URL https://github.com/google/googletest/archive/refs/tags/v1.14.0.zip + URL_HASH + SHA256=1f357c27ca988c3f7c6b4bf68a9395005ac6761f034046e9dde0896e3aba00e4 + CMAKE_CACHE_ARGS + -DBUILD_GMOCK:BOOL=OFF + -DCMAKE_INSTALL_PREFIX:STRING=@CMAKE_CURRENT_BINARY_DIR@/googletest/install + @GTest_CMAKE_FLAGS@ + UPDATE_DISCONNECTED 1) diff --git a/external/config/hana/CMakeLists.txt.in b/external/config/hana/CMakeLists.txt.in new file mode 100644 index 0000000..0d287ad --- /dev/null +++ b/external/config/hana/CMakeLists.txt.in @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 3.20) + +project(ExternalHana NONE) + +include(ExternalProject) + +if(NOT "@Hana_CMAKE_FLAGS@" STREQUAL "") + message(STATUS "Hana additional CMake flags: @Hana_CMAKE_FLAGS@") +endif() + +ExternalProject_Add( + hana-1.84.0 + URL @BUNDLED_HANA_FILENAME@ + URL_HASH + SHA256=ca29f3af12099394cbf52421e9e624254d60fb87412fcfa11576773d53a23ded + CMAKE_CACHE_ARGS + -DCMAKE_INSTALL_PREFIX:STRING=@CMAKE_CURRENT_BINARY_DIR@/hana/install + -DBOOST_HANA_ENABLE_CONCEPT_CHECKS:BOOL=ON + -DBOOST_HANA_ENABLE_DEBUG_MODE:BOOL=OFF + -DBOOST_HANA_ENABLE_STRING_UDL:BOOL=OFF + -DBOOST_HANA_ENABLE_EXCEPTIONS:BOOL=ON + @Hana_CMAKE_FLAGS@ + UPDATE_DISCONNECTED 1) diff --git a/external/config/hana/hana-boost-1.84.0.zip b/external/config/hana/hana-boost-1.84.0.zip new file mode 100644 index 0000000..d125c35 Binary files /dev/null and b/external/config/hana/hana-boost-1.84.0.zip differ diff --git a/external/config/osqp-cpp/CMakeLists.txt.in b/external/config/osqp-cpp/CMakeLists.txt.in new file mode 100644 index 0000000..5bc877b --- /dev/null +++ b/external/config/osqp-cpp/CMakeLists.txt.in @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 3.20) + +project(ExternalOSQPCpp NONE) + +include(ExternalProject) + +if(NOT "@osqp-cpp_CMAKE_FLAGS@" STREQUAL "") + message(STATUS "osqp-cpp additional CMake flags: @osqp-cpp_CMAKE_FLAGS@") +endif() + +ExternalProject_Add( + osqp-cpp-v0.5.1-ungar + URL https://github.com/fdevinc/osqp-cpp/archive/refs/tags/v0.5.1-ungar.zip + URL_HASH + SHA256=7d46a394ebdd1532c0eeaa4e3f798600ef9fbc2de4f4325d513289d92563150a + CMAKE_CACHE_ARGS + -DCMAKE_INSTALL_PREFIX:STRING=@CMAKE_CURRENT_BINARY_DIR@/osqp-cpp/install + -DOSQP_CPP_INSTALL:BOOL=ON + -Dabsl_ROOT:STRING=@ABSL_ROOT_DIRECTORY@ + -DEigen3_ROOT:STRING=@EIGEN3_ROOT_DIRECTORY@ + -Dosqp_ROOT:STRING=@OSQP_ROOT_DIRECTORY@ + @osqp-cpp_CMAKE_FLAGS@ + UPDATE_DISCONNECTED 1) diff --git a/external/config/osqp/CMakeLists.txt.in b/external/config/osqp/CMakeLists.txt.in new file mode 100644 index 0000000..1119e12 --- /dev/null +++ b/external/config/osqp/CMakeLists.txt.in @@ -0,0 +1,22 @@ +cmake_minimum_required(VERSION 3.20) + +project(ExternalOSQP NONE) + +include(ExternalProject) + +if(NOT "@osqp_CMAKE_FLAGS@" STREQUAL "") + message(STATUS "OSQP additional CMake flags: @osqp_CMAKE_FLAGS@") +endif() + +# OSQP integrates QLDL as a Git submodule, so it is convenient to download it +# using Git rather than using the URL method. +ExternalProject_Add( + osqp-v0.6.3 + GIT_REPOSITORY https://github.com/osqp/osqp.git + GIT_TAG v0.6.3 + GIT_SHALLOW TRUE + GIT_PROGRESS TRUE + CMAKE_CACHE_ARGS + -DCMAKE_INSTALL_PREFIX:STRING=@CMAKE_CURRENT_BINARY_DIR@/osqp/install + @osqp_CMAKE_FLAGS@ + UPDATE_DISCONNECTED 1) diff --git a/external/config/spdlog/CMakeLists.txt.in b/external/config/spdlog/CMakeLists.txt.in new file mode 100644 index 0000000..45b4394 --- /dev/null +++ b/external/config/spdlog/CMakeLists.txt.in @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 3.20) + +project(ExternalSpdLog NONE) + +include(ExternalProject) + +if(NOT "@spdlog_CMAKE_FLAGS@" STREQUAL "") + message(STATUS "spdlog additional CMake flags: @spdlog_CMAKE_FLAGS@") +endif() + +ExternalProject_Add( + spdlog-v1.9.2 + URL https://github.com/gabime/spdlog/archive/refs/tags/v1.9.2.zip + URL_HASH + SHA256=130bd593c33e2e2abba095b551db6a05f5e4a5a19c03ab31256c38fa218aa0a6 + CMAKE_CACHE_ARGS + -DCMAKE_INSTALL_PREFIX:STRING=@CMAKE_CURRENT_BINARY_DIR@/spdlog/install + -DSPDLOG_BUILD_SHARED:BOOL=OFF + -DSPDLOG_BUILD_ALL:BOOL=OFF + @spdlog_CMAKE_FLAGS@ + UPDATE_DISCONNECTED 1) diff --git a/include/ungar/assert.hpp b/include/ungar/assert.hpp new file mode 100644 index 0000000..73d6e82 --- /dev/null +++ b/include/ungar/assert.hpp @@ -0,0 +1,110 @@ +/****************************************************************************** + * + * @file ungar/assert.hpp + * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) + * + * @section LICENSE + * ----------------------------------------------------------------------- + * + * Copyright 2023 Flavio De Vincenti + * + * ----------------------------------------------------------------------- + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an "AS + * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either + * express or implied. See the License for the specific language + * governing permissions and limitations under the License. + * + ******************************************************************************/ + +#ifndef _UNGAR__ASSERT_HPP_ +#define _UNGAR__ASSERT_HPP_ + +#include +#include +#include + +#ifdef UNGAR_CONFIG_ENABLE_LOGGING +#include "ungar/io/logging.hpp" +#endif + +namespace Ungar { + +inline constexpr long expect(long exp, long c) { + if (exp == c) [[likely]] { + return c; + } else { + return exp; + } +} + +inline void assertion_failed( + char const* expr, const std::source_location location = std::source_location::current()) { +#ifdef UNGAR_CONFIG_ENABLE_LOGGING + UNGAR_LOG(error, + "{}:{}: {}: Assertion `{}` failed.", + location.file_name(), + location.line(), + location.function_name(), + expr); +#else + std::fprintf(stderr, + "%s:%u: %s: Assertion `%s` failed.", + location.file_name(), + location.line(), + location.function_name(), + expr); +#endif + std::abort(); +} + +inline void assertion_failed_msg( + char const* expr, + char const* msg, + const std::source_location location = std::source_location::current()) { +#ifdef UNGAR_CONFIG_ENABLE_LOGGING + UNGAR_LOG(error, + "{}:{}: {}: Assertion `{} && \"{}\"` failed.", + location.file_name(), + location.line(), + location.function_name(), + expr, + msg); +#else + std::fprintf(stderr, + "%s:%u: %s: Assertion `%s && \"%s\"` failed.", + location.file_name(), + location.line(), + location.function_name(), + expr, + msg); +#endif + std::abort(); +} + +} // namespace Ungar + +#define UNGAR_LIKELY(x) ::Ungar::expect(x, 1) +#define UNGAR_UNLIKELY(x) ::Ungar::expect(x, 0) + +#ifdef UNGAR_CONFIG_ENABLE_RELEASE_MODE +#define UNGAR_ASSERT(expr) ((void)0) +#define UNGAR_ASSERT_MSG(expr, msg) ((void)0) +#define UNGAR_ASSERT_EXPLICIT(expr) ((void)0) +#define UNGAR_ASSERT_EXPLICIT_MSG(expr, msg) ((void)0) +#else +#define UNGAR_ASSERT(expr) (UNGAR_LIKELY(!!(expr)) ? ((void)0) : ::Ungar::assertion_failed(#expr)) +#define UNGAR_ASSERT_MSG(expr, msg) \ + (UNGAR_LIKELY(!!(expr)) ? ((void)0) : ::Ungar::assertion_failed_msg(#expr, msg)) +#define UNGAR_ASSERT_EXPLICIT(expr) UNGAR_ASSERT(static_cast(expr)) +#define UNGAR_ASSERT_EXPLICIT_MSG(expr, msg) UNGAR_ASSERT_MSG(static_cast(expr), msg) +#endif + +#endif /* _UNGAR__ASSERT_HPP_ */ diff --git a/include/ungar/autodiff/data_types.hpp b/include/ungar/autodiff/data_types.hpp new file mode 100644 index 0000000..e356016 --- /dev/null +++ b/include/ungar/autodiff/data_types.hpp @@ -0,0 +1,115 @@ +/****************************************************************************** + * + * @file ungar/autodiff/data_types.hpp + * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) + * + * @section LICENSE + * ----------------------------------------------------------------------- + * + * Copyright 2023 Flavio De Vincenti + * + * ----------------------------------------------------------------------- + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an "AS + * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either + * express or implied. See the License for the specific language + * governing permissions and limitations under the License. + * + ******************************************************************************/ + +#ifndef _UNGAR__AUTODIFF__DATA_TYPES_HPP_ +#define _UNGAR__AUTODIFF__DATA_TYPES_HPP_ + +#include + +#include +#include + +#include "ungar/data_types.hpp" + +namespace Ungar { + +using ADCG = CppAD::cg::CG; +using ADFun = CppAD::ADFun; +using ad_scalar_t = CppAD::AD; + +#define _UNGAR_MAKE_EIGEN_TYPEDEFS_IMPL(Type, TypeSuffix, SIZE, SizeSuffix) \ + using Matrix##SizeSuffix##TypeSuffix = Eigen::Matrix; \ + using Vector##SizeSuffix##TypeSuffix = Eigen::Matrix; \ + using RowVector##SizeSuffix##TypeSuffix = Eigen::Matrix; \ + \ + using RefToMatrix##SizeSuffix##TypeSuffix = Eigen::Ref>; \ + using RefToVector##SizeSuffix##TypeSuffix = Eigen::Ref>; \ + using RefToRowVector##SizeSuffix##TypeSuffix = Eigen::Ref>; \ + using RefToConstMatrix##SizeSuffix##TypeSuffix = \ + Eigen::Ref>; \ + using RefToConstVector##SizeSuffix##TypeSuffix = \ + Eigen::Ref>; \ + using RefToConstRowVector##SizeSuffix##TypeSuffix = \ + Eigen::Ref>; \ + \ + using MapToMatrix##SizeSuffix##TypeSuffix = Eigen::Map>; \ + using MapToVector##SizeSuffix##TypeSuffix = Eigen::Map>; \ + using MapToRowVector##SizeSuffix##TypeSuffix = Eigen::Map>; \ + using MapToConstMatrix##SizeSuffix##TypeSuffix = \ + Eigen::Map>; \ + using MapToConstVector##SizeSuffix##TypeSuffix = \ + Eigen::Map>; \ + using MapToConstRowVector##SizeSuffix##TypeSuffix = \ + Eigen::Map> +#define UNGAR_MAKE_EIGEN_TYPEDEFS(Type, TypeSuffix) \ + _UNGAR_MAKE_EIGEN_TYPEDEFS_IMPL(Type, TypeSuffix, 2, 2); \ + _UNGAR_MAKE_EIGEN_TYPEDEFS_IMPL(Type, TypeSuffix, 3, 3); \ + _UNGAR_MAKE_EIGEN_TYPEDEFS_IMPL(Type, TypeSuffix, 4, 4); \ + _UNGAR_MAKE_EIGEN_TYPEDEFS_IMPL(Type, TypeSuffix, Eigen::Dynamic, X) + +UNGAR_MAKE_EIGEN_TYPEDEFS(ad_scalar_t, ad); +#undef UNGAR_MAKE_EIGEN_TYPEDEFS +#undef _UNGAR_MAKE_EIGEN_TYPEDEFS_IMPL + +using Quaternionad = Eigen::Quaternion; +using MapToQuaternionad = Eigen::Map>; +using MapToConstQuaternionad = Eigen::Map>; +using AngleAxisad = Eigen::AngleAxis; +using Rotation2Dad = Eigen::Rotation2D; + +/** + * @brief An AD function f computes the value of a dependent variable + * vector y as: + * y = f([x^t p^t]^t) , + * where x is a vector of independent variables and p is a + * vector of parameters. In particular, all variables are AD + * scalars. + */ +using ADFunction = std::function; + +using SparsityPattern = std::vector>; + +enum class EnabledDerivatives : unsigned { + NONE = 1U << 0, + JACOBIAN = 1U << 1, + HESSIAN = 1U << 2, + ALL = JACOBIAN | HESSIAN +}; + +constexpr auto operator|(const EnabledDerivatives lhs, const EnabledDerivatives rhs) { + return static_cast( + static_cast>(lhs) | + static_cast>(rhs)); +} + +constexpr auto operator&(const EnabledDerivatives lhs, const EnabledDerivatives rhs) { + return static_cast(static_cast>(lhs) & + static_cast>(rhs)); +} + +} // namespace Ungar + +#endif /* _UNGAR__AUTODIFF__DATA_TYPES_HPP_ */ diff --git a/include/ungar/autodiff/function.hpp b/include/ungar/autodiff/function.hpp new file mode 100644 index 0000000..a469425 --- /dev/null +++ b/include/ungar/autodiff/function.hpp @@ -0,0 +1,618 @@ +/****************************************************************************** + * + * @file ungar/autodiff/function.hpp + * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) + * + * @section LICENSE + * ----------------------------------------------------------------------- + * + * Copyright 2023 Flavio De Vincenti + * + * ----------------------------------------------------------------------- + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an "AS + * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either + * express or implied. See the License for the specific language + * governing permissions and limitations under the License. + * + ******************************************************************************/ + +#ifndef _UNGAR__AUTODIFF__FUNCTION_HPP_ +#define _UNGAR__AUTODIFF__FUNCTION_HPP_ + +#include + +#include "ungar/autodiff/data_types.hpp" +#include "ungar/io/logging.hpp" +#include "ungar/utils/passkey.hpp" +#include "ungar/utils/utils.hpp" + +namespace Ungar { +namespace Autodiff { + +class FunctionFactory; + +class Function { // clang-format on + public: + struct Blueprint { + Blueprint(const ADFunction& functionImpl_, + const index_t independentVariableSize_, + const index_t parameterSize_, + std::string_view name_, + const EnabledDerivatives enabledDerivatives_ = EnabledDerivatives::ALL, + const std::filesystem::path& folder_ = UNGAR_CODEGEN_FOLDER) + : independentVariableSize{independentVariableSize_}, + parameterSize{parameterSize_}, + dependentVariableSize{[&functionImpl_, independentVariableSize_, parameterSize_]() { + VectorXad dependentVariable; + functionImpl_(VectorXad::Ones(independentVariableSize_ + parameterSize_), + dependentVariable); + return dependentVariable.size(); + }()}, + name{name_}, + folder{folder_}, + enabledDerivatives{enabledDerivatives_}, + functionImpl{functionImpl_} { + } + + index_t independentVariableSize; + index_t parameterSize; + index_t dependentVariableSize; + + std::string name; + std::filesystem::path folder; + + EnabledDerivatives enabledDerivatives; + + ADFunction functionImpl; + }; + + Function(Passkey, + std::unique_ptr> dynamicLib, + std::unique_ptr> model, + const index_t independentVariableSize, + const index_t parameterSize, + const index_t dependentVariableSize) + : _dynamicLib{std::move(dynamicLib)}, + _model{std::move(model)}, + _jacobianInnerStarts{}, + _jacobianOuterIndices{}, + _hessianInnerStarts{}, + _hessianOuterIndices{}, + _jacobianData{}, + _jacobianMap{0_idx, 0_idx, 0_idx, nullptr, nullptr, nullptr}, + _hessianData{}, + _hessianMap{0_idx, 0_idx, 0_idx, nullptr, nullptr, nullptr}, + _independentVariableSize{independentVariableSize}, + _parameterSize{parameterSize}, + _dependentVariableSize{dependentVariableSize}, + _nnzJacobian{0UL}, + _nnzHessian{0UL} { + if (_model->isJacobianSparsityAvailable()) { + for (const auto& row : _model->JacobianSparsitySet()) { + _nnzJacobian += row.size(); + } + + std::vector rows{}; + std::vector cols{}; + _model->JacobianSparsity(rows, cols); + + _jacobianOuterIndices.clear(); + _jacobianOuterIndices.reserve(_nnzJacobian); + for (const auto col : cols) { + _jacobianOuterIndices.emplace_back(static_cast(col)); + } + _jacobianInnerStarts.clear(); + _jacobianInnerStarts.reserve(_dependentVariableSize + 1UL); + _jacobianInnerStarts.emplace_back(0); + for (int i = 0; const int j : enumerate(static_cast(_dependentVariableSize))) { + if (i < static_cast(_nnzJacobian)) { + while (i < _nnzJacobian && rows[i] == j) { + ++i; + } + _jacobianInnerStarts.emplace_back(i); + } else { + _jacobianInnerStarts.emplace_back(_jacobianInnerStarts.back()); + } + } + + _jacobianData.resize(_nnzJacobian); + new (&_jacobianMap) Eigen::Map>{ + _dependentVariableSize, + _independentVariableSize, + static_cast(_nnzJacobian), + _jacobianInnerStarts.data(), + _jacobianOuterIndices.data(), + _jacobianData.data()}; + } + if (_model->isHessianSparsityAvailable()) { + UNGAR_ASSERT_MSG(_dependentVariableSize == 1_idx, + "The Hessian is implemented only for scalar functions."); + + for (const auto& row : _model->HessianSparsitySet()) { + _nnzHessian += row.size(); + } + + std::vector rows{}; + std::vector cols{}; + _model->HessianSparsity(0UL, rows, cols); + + _hessianOuterIndices.clear(); + _hessianOuterIndices.reserve(_nnzHessian); + for (const auto col : cols) { + _hessianOuterIndices.emplace_back(static_cast(col)); + } + _hessianInnerStarts.clear(); + _hessianInnerStarts.reserve(_independentVariableSize + 1UL); // n rows + _hessianInnerStarts.emplace_back(0); + for (int i = 0; const int j : enumerate(static_cast(_independentVariableSize))) { + if (i < static_cast(_nnzHessian)) { + while (i < static_cast(_nnzHessian) && rows[i] == j) { + ++i; + } + _hessianInnerStarts.emplace_back(i); + } else { + _hessianInnerStarts.emplace_back(_hessianInnerStarts.back()); + } + } + + _hessianData.resize(_nnzHessian); + new (&_hessianMap) Eigen::Map>{ + _independentVariableSize, + _independentVariableSize, + static_cast(_nnzHessian), + _hessianInnerStarts.data(), + _hessianOuterIndices.data(), + _hessianData.data()}; + } + } + + Function(Function&& rhs) = default; + Function& operator=(Function&& rhs) = default; + + template // clang-format off + requires Concepts::Same + void Evaluate(const Eigen::MatrixBase<_XP>& xp, Eigen::MatrixBase<_Y> const& y) const { // clang-format on + UNGAR_ASSERT(xp.size() == _independentVariableSize + _parameterSize); + UNGAR_ASSERT(y.size() == _dependentVariableSize); + + _model->ForwardZero( + CppAD::cg::ArrayView{xp.derived().data(), static_cast(xp.size())}, + CppAD::cg::ArrayView{y.const_cast_derived().data(), + static_cast(y.size())}); + } + + template _Y> // clang-format off + requires std::same_as + void Evaluate(const Eigen::MatrixBase<_XP>& xp, _Y&& y) const { // clang-format on + UNGAR_ASSERT(xp.size() == _independentVariableSize + _parameterSize); + UNGAR_ASSERT(static_cast(std::ranges::size(std::forward<_Y>(y))) == + _dependentVariableSize); + + _model->ForwardZero( + CppAD::cg::ArrayView{xp.derived().data(), static_cast(xp.size())}, + CppAD::cg::ArrayView{ + std::ranges::data(std::forward<_Y>(y)), + static_cast(std::ranges::size(std::forward<_Y>(y)))}); + } + + template // clang-format off + requires std::same_as + VectorXr operator()(const Eigen::MatrixBase<_XP>& xp) const { // clang-format on + UNGAR_ASSERT(xp.size() == _independentVariableSize + _parameterSize); + + VectorXr y{_dependentVariableSize}; + Evaluate(xp, y); + return y; + } + + template + const Eigen::Map>& Jacobian( + const Eigen::MatrixBase<_XP>& xp) const { + UNGAR_ASSERT(ImplementsJacobian()); + UNGAR_ASSERT(xp.size() == _independentVariableSize + _parameterSize); + + const size_t* rows; + const size_t* cols; + _model->SparseJacobian( + CppAD::cg::ArrayView{xp.derived().data(), static_cast(xp.size())}, + CppAD::cg::ArrayView{_jacobianData}, + &rows, + &cols); + return _jacobianMap; + } + + /** + * @brief Returns an upper triangular view of the Hessian matrix for a given + * dependent variable. + */ + template + const Eigen::Map>& Hessian( + const index_t dependentVariableIndex, const Eigen::MatrixBase<_XP>& xp) const { + UNGAR_ASSERT(ImplementsHessian()); + UNGAR_ASSERT_MSG(_dependentVariableSize == 1_idx, + "Although this method is meant for general multivariable functions, it is " + "currently implemented only for scalar functions."); + UNGAR_ASSERT(dependentVariableIndex >= 0_idx && + dependentVariableIndex < _dependentVariableSize); + UNGAR_ASSERT(xp.size() == _independentVariableSize + _parameterSize); + + const size_t* rows; + const size_t* cols; + const VectorXr mask = VectorXr::Unit(_dependentVariableSize, dependentVariableIndex); + CppAD::cg::ArrayView maskArrayView{mask.data(), + static_cast(mask.size())}; + _model->SparseHessian( + CppAD::cg::ArrayView{xp.derived().data(), static_cast(xp.size())}, + maskArrayView, + CppAD::cg::ArrayView{_hessianData}, + &rows, + &cols); + return _hessianMap; + } + + /** + * @brief Returns an upper triangular view of the Hessian matrix. + * + * @note This method can only be used for scalar functions. + */ + template + const Eigen::Map>& Hessian( + const Eigen::MatrixBase<_XP>& xp) const { + UNGAR_ASSERT(ImplementsHessian()); + UNGAR_ASSERT(_dependentVariableSize == 1_idx); + UNGAR_ASSERT(xp.size() == _independentVariableSize + _parameterSize); + + return Hessian(0_idx, xp); + } + + template + [[nodiscard]] bool TestFunction( + const Eigen::MatrixBase<_XP>& xp, + const std::function& groundTruthFunction) const { + UNGAR_ASSERT(ImplementsFunction()); + return Utils::CompareMatrices( + (*this)(xp), "Autodiff function"sv, groundTruthFunction(xp), "Ground truth"sv); + } + + template + [[nodiscard]] bool TestJacobian( + const Eigen::MatrixBase<_XP>& xp, + const fd::AccuracyOrder accuracyOrder = fd::AccuracyOrder::SECOND, + const real_t epsilon = 1e-8) const { + UNGAR_ASSERT(ImplementsJacobian()); + MatrixXr fdJacobian; + const VectorXr p{xp.tail(_parameterSize)}; + fd::finite_jacobian( + xp.head(_independentVariableSize), + [&](const VectorXr& xFD) { return (*this)(Utils::Compose(xFD, p).ToDynamic()); }, + fdJacobian, + accuracyOrder, + epsilon); + return Utils::CompareMatrices( + Jacobian(xp).toDense(), "Autodiff Jacobian"sv, fdJacobian, "FD Jacobian"sv); + } + + template + [[nodiscard]] bool TestHessian( + const index_t dependentVariableIndex, + const Eigen::MatrixBase<_XP>& xp, + const fd::AccuracyOrder accuracyOrder = fd::AccuracyOrder::SECOND, + const real_t epsilon = 1e-5) const { + UNGAR_ASSERT(dependentVariableIndex >= 0_idx && + dependentVariableIndex < _dependentVariableSize); + MatrixXr fdHessian; + const VectorXr p{xp.tail(_parameterSize)}; + fd::finite_hessian( + xp.head(_independentVariableSize), + [&](const VectorXr& xFD) { + return (*this)(Utils::Compose(xFD, p).ToDynamic())[dependentVariableIndex]; + }, + fdHessian, + accuracyOrder, + epsilon); + return Utils::CompareMatrices(Hessian(dependentVariableIndex, xp).toDense(), + "Autodiff Hessian"sv, + fdHessian, + "FD Hessian"sv); + } + + /** + * @note This method can only be used for scalar functions. + */ + template + [[nodiscard]] bool TestHessian( + const Eigen::MatrixBase<_XP>& xp, + const fd::AccuracyOrder accuracyOrder = fd::AccuracyOrder::SECOND, + const real_t epsilon = 1e-5) const { + UNGAR_ASSERT(_dependentVariableSize == 1_idx); + return TestHessian(0_idx, xp, accuracyOrder, epsilon); + } + + bool ImplementsFunction() const { + return _model->isForwardZeroAvailable(); + } + + bool ImplementsJacobian() const { + return _model->isSparseJacobianAvailable(); + } + + bool ImplementsHessian() const { + return _model->isSparseHessianAvailable(); + } + + index_t IndependentVariableSize() const { + return _independentVariableSize; + } + + index_t ParameterSize() const { + return _parameterSize; + } + + index_t DependentVariableSize() const { + return _dependentVariableSize; + } + + private: + std::unique_ptr> _dynamicLib; + std::unique_ptr> _model; + + /// @note For both the Jacobian and the Hessian, we store maps to arrays of data + /// that are manipulated by the \c Autodiff::Function generated code. + /// Unfortunately, \a CppADCodeGen generates sparse matrices in row major + /// representation with outer indices sorted in decreasing order, while + /// \a Eigen sorts them in increasing order. This means that the maps + /// we create cannot be used reliably for sparse operations. However, + /// assigning them to column major sparse matrices results in well-defined + /// conversions. + std::vector _jacobianInnerStarts; + std::vector _jacobianOuterIndices; + std::vector _hessianInnerStarts; + std::vector _hessianOuterIndices; + + mutable std::vector _jacobianData; + Eigen::Map> _jacobianMap; + mutable std::vector _hessianData; + Eigen::Map> _hessianMap; + + index_t _independentVariableSize; + index_t _parameterSize; + index_t _dependentVariableSize; + size_t _nnzJacobian; + size_t _nnzHessian; +}; + +class FunctionFactory { + private: + class Worker { + public: + Worker(const Function::Blueprint& blueprint, std::vector compilerFlags) + : _blueprint{blueprint}, _compilerFlags{std::move(compilerFlags)} { + SetUpFolders(); + } + + auto CreateModel() { + CreateModelDirectories(); + + UNGAR_LOG(info, "Creating internal model..."); + CreateModelsImpl(_blueprint.name + "_internal"s, _internalLibrary, false); + UNGAR_LOG(info, "Creating model..."); + CreateModelsImpl(_blueprint.name, _library, true); + + UNGAR_LOG(info, + "Removing internal model folder {}...", + _internalLibrary.parent_path().parent_path()); + std::filesystem::remove_all(_internalLibrary.parent_path().parent_path()); + + UNGAR_LOG(info, "Success."); + + return std::pair{std::unique_ptr>{_dynamicLib.release()}, + std::unique_ptr>{_model.release()}}; + } + + auto TryCreateModel() { + if (IsLibraryAvailable()) { + LoadModel(); + return std::pair{ + std::unique_ptr>{_dynamicLib.release()}, + std::unique_ptr>{_model.release()}}; + } else { + return CreateModel(); + } + } + + private: + void SetUpFolders() { + _library = _blueprint.folder / _blueprint.name / "cppad_cg"s / + (_blueprint.name + "_lib"s + + CppAD::cg::system::SystemInfo<>::DYNAMIC_LIB_EXTENSION); + _internalLibrary = _blueprint.folder / (_blueprint.name + "_internal/cppad_cg"s) / + (_blueprint.name + "_internal_lib"s + + CppAD::cg::system::SystemInfo<>::DYNAMIC_LIB_EXTENSION); + + _tmpFolder = Utils::TemporaryDirectoryPath("cppadcg_tmp"s); + } + + void CreateModelDirectories() const { + std::filesystem::create_directories(_library.parent_path()); + std::filesystem::create_directories(_internalLibrary.parent_path()); + std::filesystem::create_directories(_tmpFolder); + } + + bool IsLibraryAvailable() const { + return std::filesystem::exists(_library); + } + + void CreateModelsImpl(const std::string& modelName, + const std::filesystem::path& library, + const bool trimInternalModelSparsity) { + VectorXad xp = + VectorXad::Ones(_blueprint.independentVariableSize + _blueprint.parameterSize); + CppAD::Independent(xp); + + VectorXad y; + + _blueprint.functionImpl(xp, y); + UNGAR_ASSERT(y.size() && y.size() == _blueprint.dependentVariableSize); + + ADFun adFun(xp, y); + adFun.optimize(); + + CppAD::cg::ModelCSourceGen sourceGen(adFun, modelName); + if (_blueprint.enabledDerivatives & EnabledDerivatives::JACOBIAN) { + sourceGen.setCreateSparseJacobian(true); + + if (trimInternalModelSparsity) { + TrimParameterEntriesFromInternalModelJacobianSparsity(sourceGen); + } + } + if (_blueprint.enabledDerivatives & EnabledDerivatives::HESSIAN) { + sourceGen.setCreateSparseHessian(true); + sourceGen.setCreateHessianSparsityByEquation(true); + + if (trimInternalModelSparsity) { + TrimParameterEntriesFromInternalModelHessianSparsity(sourceGen); + } + } + + const std::filesystem::path tmpLibrary{ + library.parent_path() / (library.stem().string() + _tmpFolder.stem().string() + + library.extension().string())}; + + CppAD::cg::ModelLibraryCSourceGen libraryCSourceGen{sourceGen}; + CppAD::cg::GccCompiler gccCompiler; + CppAD::cg::DynamicModelLibraryProcessor libraryProcessor{ + libraryCSourceGen, (tmpLibrary.parent_path() / tmpLibrary.stem()).string()}; + SetCompilerOptions(library.parent_path(), gccCompiler); + + UNGAR_LOG(info, "Compiling shared library {}...", tmpLibrary); + _dynamicLib = libraryProcessor.createDynamicLibrary(gccCompiler); + _model = _dynamicLib->model(modelName); + UNGAR_ASSERT(_dynamicLib); + UNGAR_ASSERT(_model); + + UNGAR_LOG(info, "Renaming {} to {}...", tmpLibrary, library); + std::filesystem::rename(tmpLibrary, library); + } + + void LoadModel() { + UNGAR_LOG(info, "Loading shared library {}...", _library); + + _dynamicLib.reset(new CppAD::cg::LinuxDynamicLib(_library)); + _model = _dynamicLib->model(_blueprint.name); + UNGAR_ASSERT(_dynamicLib); + UNGAR_ASSERT(_model); + + UNGAR_LOG(info, "Success."); + } + + void SetCompilerOptions(const std::filesystem::path& libraryFolder, + CppAD::cg::GccCompiler& compiler) const { + if (!_compilerFlags.empty()) { + compiler.setCompileLibFlags(_compilerFlags); + compiler.addCompileLibFlag("-shared"); + compiler.addCompileLibFlag("-rdynamic"); + } + + compiler.setTemporaryFolder(_tmpFolder); + compiler.setSourcesFolder(libraryFolder); + compiler.setSaveToDiskFirst(true); + } + + void TrimParameterEntriesFromInternalModelJacobianSparsity( + CppAD::cg::ModelCSourceGen& sourceGen) const { + if (!_model || _model->getName() != _blueprint.name + "_internal"s) { + throw std::runtime_error( + "Cannot trim Jacobian sparsity for nonexistent internal model."); + } + + const auto fullJacobianSparsity = _model->JacobianSparsitySet(); + + SparsityPattern jacobianSparsity{static_cast(_blueprint.dependentVariableSize)}; + for (const auto row : enumerate(_blueprint.dependentVariableSize)) { + for (const auto col : + enumerate(_blueprint.independentVariableSize) | + std::views::filter([&fullJacobianSparsity, row](const auto col) { + return fullJacobianSparsity[static_cast(row)].contains( + static_cast(col)); + })) { + jacobianSparsity[static_cast(row)].insert(static_cast(col)); + } + } + sourceGen.setCustomSparseJacobianElements(jacobianSparsity); + } + + void TrimParameterEntriesFromInternalModelHessianSparsity( + CppAD::cg::ModelCSourceGen& sourceGen) const { + if (!_model || _model->getName() != _blueprint.name + "_internal"s) { + throw std::runtime_error( + "Cannot trim Hessian sparsity for nonexistent internal model."); + } + + const auto fullHessianSparsity = _model->HessianSparsitySet(); + + SparsityPattern hessianSparsity{ + static_cast(_blueprint.independentVariableSize + _blueprint.parameterSize)}; + for (const auto row : enumerate(_blueprint.independentVariableSize)) { + for (const auto col : + std::views::iota(row, _blueprint.independentVariableSize) | + std::views::filter([&fullHessianSparsity, row](const auto col) { + return fullHessianSparsity[static_cast(row)].contains( + static_cast(col)); + })) { + hessianSparsity[static_cast(row)].insert(static_cast(col)); + } + } + sourceGen.setCustomSparseHessianElements(hessianSparsity); + } + + Function::Blueprint _blueprint; + + std::unique_ptr> _dynamicLib; + std::unique_ptr> _model; + std::vector _compilerFlags; + + std::filesystem::path _library; + std::filesystem::path _internalLibrary; + + std::filesystem::path _tmpFolder; + }; + + public: + static Function Make(const Function::Blueprint& blueprint, + const bool recompileLibraries, + std::vector compilerFlags) { + Worker worker{blueprint, std::move(compilerFlags)}; + + UNGAR_LOG(info, "{}", Utils::DASH_LINE_SEPARATOR); + auto [dynamicLib, model] = + recompileLibraries ? worker.CreateModel() : worker.TryCreateModel(); + + return {UNGAR_PASSKEY, + std::move(dynamicLib), + std::move(model), + blueprint.independentVariableSize, + blueprint.parameterSize, + blueprint.dependentVariableSize}; + } +}; + +inline Function MakeFunction( + const Function::Blueprint& blueprint, + const bool recompileLibraries = false, + std::vector compilerFlags = { + "-O3"s, "-g"s, "-march=native"s, "-mtune=native"s, "-ffast-math"s}) { + return FunctionFactory::Make(blueprint, recompileLibraries, std::move(compilerFlags)); +} + +} // namespace Autodiff +} // namespace Ungar + +#endif /* _UNGAR__AUTODIFF__FUNCTION_HPP_ */ diff --git a/include/ungar/autodiff/support/quaternion.hpp b/include/ungar/autodiff/support/quaternion.hpp new file mode 100644 index 0000000..29c7e47 --- /dev/null +++ b/include/ungar/autodiff/support/quaternion.hpp @@ -0,0 +1,226 @@ +/****************************************************************************** + * + * @file ungar/autodiff/support/quaternion.hpp + * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) + * + * @section LICENSE + * ----------------------------------------------------------------------- + * + * Copyright 2023 Flavio De Vincenti + * + * ----------------------------------------------------------------------- + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an "AS + * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either + * express or implied. See the License for the specific language + * governing permissions and limitations under the License. + * + ******************************************************************************/ + +#ifndef _UNGAR__AUTODIFF__SUPPORT__QUATERNION_HPP_ +#define _UNGAR__AUTODIFF__SUPPORT__QUATERNION_HPP_ + +#include "ungar/autodiff/data_types.hpp" + +namespace Eigen { + +template <> +inline Quaternion QuaternionBase::inverse() const { + Scalar n2 = this->squaredNorm(); + return Quaternion{conjugate().coeffs() / + CppAD::CondExpGt(n2, RealScalar{0}, n2, RealScalar{1.0})}; +} +template <> +inline Quaternion QuaternionBase>::inverse() + const { + Scalar n2 = this->squaredNorm(); + return Quaternion{conjugate().coeffs() / + CppAD::CondExpGt(n2, RealScalar{0}, n2, RealScalar{1.0})}; +} +template <> +inline Quaternion +QuaternionBase>::inverse() const { + Scalar n2 = this->squaredNorm(); + return Quaternion{conjugate().coeffs() / + CppAD::CondExpGt(n2, RealScalar{0}, n2, RealScalar{1.0})}; +} + +template <> +inline void MatrixBase::normalize() { + RealScalar z = squaredNorm(); + derived() /= CppAD::CondExpGt(z, RealScalar{0}, numext::sqrt(z), RealScalar{1.0}); +} +template <> +inline void MatrixBase>::normalize() { + RealScalar z = squaredNorm(); + derived() /= CppAD::CondExpGt(z, RealScalar{0}, numext::sqrt(z), RealScalar{1.0}); +} + +template <> +inline const typename MatrixBase::PlainObject +MatrixBase::normalized() const { + using _Nested = const Eigen::Matrix>, 4, 1, 0>&; + _Nested n(derived()); + RealScalar z = n.squaredNorm(); + return n / CppAD::CondExpGt(z, RealScalar{0}, numext::sqrt(z), RealScalar{1.0}); +} +template <> +inline const typename MatrixBase>::PlainObject +MatrixBase>::normalized() const { + using _Nested = const Eigen::Map>, 4, 1, 0>, 0>; + _Nested n(derived()); + RealScalar z = n.squaredNorm(); + return n / CppAD::CondExpGt(z, RealScalar{0}, numext::sqrt(z), RealScalar{1.0}); +} +template <> +inline const typename MatrixBase>::PlainObject +MatrixBase>::normalized() const { + using _Nested = + const Eigen::Map>, 4, 1, 0>, 0>; + _Nested n(derived()); + RealScalar z = n.squaredNorm(); + return n / CppAD::CondExpGt(z, RealScalar{0}, numext::sqrt(z), RealScalar{1.0}); +} + +template <> +inline const typename MatrixBase::PlainObject +MatrixBase::normalized() const { + using _Nested = const Eigen::Matrix>, 3, 1, 0>&; + _Nested n(derived()); + RealScalar z = n.squaredNorm(); + return n / CppAD::CondExpGt(z, RealScalar{0}, numext::sqrt(z), RealScalar{1.0}); +} +template <> +inline const typename MatrixBase>::PlainObject +MatrixBase>::normalized() const { + using _Nested = const Eigen::Map>, 3, 1, 0>, 0>; + _Nested n(derived()); + RealScalar z = n.squaredNorm(); + return n / CppAD::CondExpGt(z, RealScalar{0}, numext::sqrt(z), RealScalar{1.0}); +} +template <> +inline const typename MatrixBase>::PlainObject +MatrixBase>::normalized() const { + using _Nested = + const Eigen::Map>, 3, 1, 0>, 0>; + _Nested n(derived()); + RealScalar z = n.squaredNorm(); + return n / CppAD::CondExpGt(z, RealScalar{0}, numext::sqrt(z), RealScalar{1.0}); +} + +namespace internal { + +template <> +struct quaternionbase_assign_impl { + typedef typename Ungar::Matrix3ad::Scalar Scalar; + template + static inline void run(QuaternionBase& q, const Ungar::Matrix3ad& a_mat) { + static_assert(Ungar::dependent_false, + "The construction of unit quaternions from rotation matrices with scalar " + "type 'ad_scalar_t' is not implemented."); + } +}; + +} // namespace internal + +template <> +template +Quaternion QuaternionBase::slerp( + const Scalar& t, const QuaternionBase& other) const { + using std::acos; + using std::sin; + const Scalar one = Scalar{1.0} - NumTraits::epsilon(); + const Scalar d = this->dot(other); + const Scalar absD = numext::abs(d); + + Scalar scale0; + Scalar scale1; + + scale0 = CppAD::CondExpGe( + absD, one, Scalar{1.0} - t, sin((Scalar{1.0} - t) * acos(absD)) / sin(acos(absD))); + scale1 = CppAD::CondExpGe(absD, one, t, sin((t * acos(absD))) / sin(acos(absD))); + scale1 = CppAD::CondExpLt(d, Scalar{0.0}, -scale1, scale1); + + return Quaternion{scale0 * coeffs() + scale1 * other.coeffs()}; +} +template <> +template +Quaternion QuaternionBase>::slerp( + const Scalar& t, const QuaternionBase& other) const { + using std::acos; + using std::sin; + const Scalar one = Scalar{1.0} - NumTraits::epsilon(); + const Scalar d = this->dot(other); + const Scalar absD = numext::abs(d); + + Scalar scale0; + Scalar scale1; + + scale0 = CppAD::CondExpGe( + absD, one, Scalar{1.0} - t, sin((Scalar{1.0} - t) * acos(absD)) / sin(acos(absD))); + scale1 = CppAD::CondExpGe(absD, one, t, sin((t * acos(absD))) / sin(acos(absD))); + scale1 = CppAD::CondExpLt(d, Scalar{0.0}, -scale1, scale1); + + return Quaternion{scale0 * coeffs() + scale1 * other.coeffs()}; +} +template <> +template +Quaternion QuaternionBase>::slerp( + const Scalar& t, const QuaternionBase& other) const { + using std::acos; + using std::sin; + const Scalar one = Scalar{1.0} - NumTraits::epsilon(); + const Scalar d = this->dot(other); + const Scalar absD = numext::abs(d); + + Scalar scale0; + Scalar scale1; + + scale0 = CppAD::CondExpGe( + absD, one, Scalar{1.0} - t, sin((Scalar{1.0} - t) * acos(absD)) / sin(acos(absD))); + scale1 = CppAD::CondExpGe(absD, one, t, sin((t * acos(absD))) / sin(acos(absD))); + scale1 = CppAD::CondExpLt(d, Scalar{0.0}, -scale1, scale1); + + return Quaternion{scale0 * coeffs() + scale1 * other.coeffs()}; +} + +template <> +template +inline Ungar::Quaternionad& QuaternionBase::setFromTwoVectors( + const MatrixBase& a, const MatrixBase& b) { + static_assert(Ungar::dependent_false, + "The construction of unit quaternions with scalar type 'ad_scalar_t' from two " + "vectors is not implemented."); + return derived(); +} +template <> +template +inline Eigen::Map& +QuaternionBase>::setFromTwoVectors(const MatrixBase& a, + const MatrixBase& b) { + static_assert(Ungar::dependent_false, + "The construction of unit quaternions with scalar type 'ad_scalar_t' from two " + "vectors is not implemented."); + return derived(); +} +template <> +template +inline Eigen::Map& +QuaternionBase>::setFromTwoVectors( + const MatrixBase& a, const MatrixBase& b) { + static_assert(Ungar::dependent_false, + "The construction of unit quaternions with scalar type 'ad_scalar_t' from two " + "vectors is not implemented."); + return derived(); +} + +} // namespace Eigen + +#endif /* _UNGAR__AUTODIFF__SUPPORT__QUATERNION_HPP_ */ diff --git a/include/ungar/autodiff/vector_composer.hpp b/include/ungar/autodiff/vector_composer.hpp new file mode 100644 index 0000000..365cf50 --- /dev/null +++ b/include/ungar/autodiff/vector_composer.hpp @@ -0,0 +1,81 @@ +/****************************************************************************** + * + * @file ungar/autodiff/vector_composer.hpp + * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) + * + * @section LICENSE + * ----------------------------------------------------------------------- + * + * Copyright 2023 Flavio De Vincenti + * + * ----------------------------------------------------------------------- + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an "AS + * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either + * express or implied. See the License for the specific language + * governing permissions and limitations under the License. + * + ******************************************************************************/ + +#ifndef _UNGAR__AUTODIFF__VECTOR_COMPOSER_HPP_ +#define _UNGAR__AUTODIFF__VECTOR_COMPOSER_HPP_ + +#include "ungar/autodiff/data_types.hpp" + +namespace Ungar { +namespace Autodiff { + +class VectorComposer { + public: + VectorComposer() : _impl{} { + } + + template // clang-format off + requires std::same_as + VectorComposer& operator<<( + const Eigen::MatrixBase<_Vector>& vector) { // clang-format on + _impl.emplace_back(vector); + return *this; + } + + VectorComposer& operator<<(const ad_scalar_t& scalar) { + _impl.emplace_back((VectorXad{1_idx} << scalar).finished()); + return *this; + } + + void Clear() { + _impl.clear(); + } + + index_t Size() const { + index_t size = 0_idx; + for (const VectorXad& el : _impl) { + size += el.size(); + } + return size; + } + + VectorXad Compose() const { + VectorXad composedOutput{Size()}; + for (index_t i = 0; const VectorXad& el : _impl) { + composedOutput.segment(i, el.size()) = el; + i += el.size(); + } + return composedOutput; + } + + private: + std::vector _impl; +}; + +} // namespace Autodiff +} // namespace Ungar + +#endif /* _UNGAR__AUTODIFF__VECTOR_COMPOSER_HPP_ */ diff --git a/include/ungar/data_types.hpp b/include/ungar/data_types.hpp new file mode 100644 index 0000000..06758ce --- /dev/null +++ b/include/ungar/data_types.hpp @@ -0,0 +1,472 @@ +/****************************************************************************** + * + * @file ungar/data_types.hpp + * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) + * + * @section LICENSE + * ----------------------------------------------------------------------- + * + * Copyright 2023 Flavio De Vincenti + * + * ----------------------------------------------------------------------- + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an "AS + * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either + * express or implied. See the License for the specific language + * governing permissions and limitations under the License. + * + ******************************************************************************/ + +#ifndef _UNGAR__DATA_TYPES_HPP_ +#define _UNGAR__DATA_TYPES_HPP_ + +#include + +#include "Eigen/Geometry" +#include "Eigen/Sparse" + +#include "boost/hana.hpp" +#include "boost/hana/ext/std/array.hpp" +#include "boost/hana/ext/std/tuple.hpp" + +#ifndef UNGAR_CODEGEN_FOLDER +#define UNGAR_CODEGEN_FOLDER (std::filesystem::temp_directory_path() / "ungar_codegen").c_str() +#endif + +namespace std { + +template +struct tuple_element> { + using type = typename decltype(+boost::hana::tuple_t<_Ts...>[boost::hana::size_c])::type; +}; + +template +struct tuple_size> + : public integral_constant {}; + +} // namespace std + +namespace boost { +namespace hana { + +template +constexpr decltype(auto) get(hana::tuple<_Ts...>& tuple) { + return tuple[hana::size_c]; +} + +template +constexpr decltype(auto) get(const hana::tuple<_Ts...>& tuple) { + return tuple[hana::size_c]; +} + +template +constexpr decltype(auto) get(hana::tuple<_Ts...>&& tuple) { + return static_cast&&>(tuple)[hana::size_c]; +} + +template +constexpr decltype(auto) get(const hana::tuple<_Ts...>&& tuple) { + return static_cast&&>(tuple)[hana::size_c]; +} + +} // namespace hana +} // namespace boost + +namespace Ungar { + +namespace hana = boost::hana; + +using namespace std::literals; +using namespace hana::literals; + +using real_t = double; + +using index_t = Eigen::Index; +using time_step_t = index_t; + +template typename _TemplateType> +struct is_specialization_of : std::false_type {}; +template