diff --git a/.clang-tidy b/.clang-tidy index c5a9f946..cfb2b12f 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -35,7 +35,6 @@ Checks: bugprone-unhandled-self-assignment, bugprone-unused-raii, bugprone-virtual-near-miss, - cert-dcl58-cpp, cert-err52-cpp, cert-err60-cpp, cert-mem57-cpp, diff --git a/.github/workflows/build-rust.yml b/.github/workflows/build-rust.yml new file mode 100644 index 00000000..b817d014 --- /dev/null +++ b/.github/workflows/build-rust.yml @@ -0,0 +1,80 @@ +name: Build Rust + +on: [pull_request, push] + +concurrency: + group: ${{ github.workflow }}-${{ github.head_ref || github.ref }} + cancel-in-progress: true + +jobs: + build-rust: + strategy: + fail-fast: false + matrix: + include: + - artifact-name: Rust Native - Windows x86_64 (CasADi) + optimizer_backend: casadi + os: windows-2022 + rust-target: x86_64-pc-windows-gnu + - artifact-name: Rust Native - macOS x86_64 (CasADi) + optimizer_backend: casadi + os: macOS-13 + rust-target: x86_64-apple-darwin + - artifact-name: Rust Native - macOS arm64 (CasADi) + optimizer_backend: casadi + os: macOS-14 + rust-target: aarch64-apple-darwin + - artifact-name: Rust Native - Linux x86_64 (CasADi) + optimizer_backend: casadi + os: ubuntu-latest + rust-target: x86_64-unknown-linux-gnu + - artifact-name: Rust Native - Windows x86_64 (Sleipnir) + optimizer_backend: sleipnir + os: windows-2022 + rust-target: x86_64-pc-windows-gnu + - artifact-name: Rust Native - macOS x86_64 (Sleipnir) + optimizer_backend: sleipnir + os: macOS-13 + rust-target: x86_64-apple-darwin + - artifact-name: Rust Native - macOS arm64 (Sleipnir) + optimizer_backend: sleipnir + os: macOS-14 + rust-target: aarch64-apple-darwin + - artifact-name: Rust Native - Linux x86_64 (Sleipnir) + optimizer_backend: sleipnir + os: ubuntu-latest + rust-target: x86_64-unknown-linux-gnu + + name: "${{ matrix.artifact-name }}" + runs-on: ${{ matrix.os }} + + steps: + - uses: actions/checkout@v4 + + - name: Set up MinGW + if: matrix.os == 'windows-2022' && matrix.optimizer_backend == 'casadi' + uses: egor-tensin/setup-mingw@v2 + with: + platform: x64 + version: 12.2.0 + + - name: Set up Rust + uses: hecrj/setup-rust-action@v1 + with: + targets: ${{matrix.rust-target}} + + - name: Set up Clang + if: ${{ !startsWith(matrix.os, 'macOS') }} + uses: egor-tensin/setup-clang@v1 + with: + version: latest + platform: x64 + + - name: Build Rust + working-directory: rust + run: cargo build --target ${{ matrix.rust-target }} --features ${{ matrix.optimizer_backend }} + + - name: Run Rust + if: matrix.rust-target != 'aarch64-apple-darwin' && matrix.rust-target != 'x86_64-pc-windows-gnu' + working-directory: rust + run: cargo run --example swerve --target ${{ matrix.rust-target }} --features ${{ matrix.optimizer_backend }} diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 3a96813f..1095b96e 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -8,42 +8,42 @@ concurrency: jobs: build-cpp: + timeout-minutes: 30 strategy: fail-fast: false matrix: include: - artifact-name: Native - Windows (CasADi) - cmake-config-env-vars: - cmake-flags: "-G \"MinGW Makefiles\" -DCMAKE_CXX_COMPILER=x86_64-w64-mingw32-g++" + # FIXME: TrajoptLibTest.exe can't find an unspecified DLL + cmake-flags: "-G \"MinGW Makefiles\" -DCMAKE_CXX_COMPILER=x86_64-w64-mingw32-g++ -DBUILD_TESTING=OFF" optimizer: casadi os: windows-2022 - artifact-name: Native - macOS x86_64 (CasADi) - cmake-config-env-vars: CFLAGS="$CFLAGS -arch x86_64" CXXFLAGS="$CXXFLAGS -arch x86_64" cmake-flags: "-DCMAKE_APPLE_SILICON_PROCESSOR=x86_64" optimizer: casadi - os: macOS-12 + os: macOS-13 - artifact-name: Native - macOS arm64 (CasADi) - cmake-config-env-vars: CFLAGS="$CFLAGS -arch arm64" CXXFLAGS="$CXXFLAGS -arch arm64" cmake-flags: "-DCMAKE_APPLE_SILICON_PROCESSOR=arm64" optimizer: casadi - os: macOS-12 + os: macOS-14 - artifact-name: Native - Linux (CasADi) - cmake-config-env-vars: cmake-flags: optimizer: casadi os: ubuntu-latest - artifact-name: Native - Windows (Sleipnir) - cmake-config-env-vars: - cmake-flags: + # FIXME: TrajoptLibTest.exe can't find an unspecified DLL + cmake-flags: "-DBUILD_TESTING=OFF" optimizer: sleipnir os: windows-2022 - - artifact-name: Native - macOS universal (Sleipnir) - cmake-config-env-vars: CFLAGS="$CFLAGS -arch x86_64 -arch arm64" CXXFLAGS="$CXXFLAGS -arch x86_64 -arch arm64" + - artifact-name: Native - macOS x86_64 (Sleipnir) + cmake-flags: + optimizer: sleipnir + os: macOS-13 + - artifact-name: Native - macOS arm64 (Sleipnir) cmake-flags: optimizer: sleipnir - os: macOS-12 + os: macOS-14 - artifact-name: Native - Linux (Sleipnir) - cmake-config-env-vars: cmake-flags: optimizer: sleipnir os: ubuntu-latest @@ -52,29 +52,34 @@ jobs: runs-on: ${{ matrix.os }} steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: Set up MinGW if: matrix.os == 'windows-2022' && matrix.optimizer == 'casadi' uses: egor-tensin/setup-mingw@v2 with: platform: x64 + version: 12.2.0 - name: configure - run: ${{ matrix.cmake-config-env-vars }} cmake -B build -S . ${{ matrix.cmake-flags }} -DOPTIMIZER_BACKEND=${{ matrix.optimizer }} -DWITH_JAVA=ON -DJAVA_INSTALL_JAR=OFF + run: cmake -B build -S . ${{ matrix.cmake-flags }} -DOPTIMIZER_BACKEND=${{ matrix.optimizer }} -DWITH_JAVA=ON -DJAVA_INSTALL_JAR=OFF - name: build run: cmake --build build --config RelWithDebInfo --parallel $(nproc) + - name: test + working-directory: build + run: ctest -C RelWithDebInfo --output-on-failure + - name: install run: cmake --install build --config RelWithDebInfo --prefix pkg - - uses: actions/upload-artifact@v3.1.1 + - uses: actions/upload-artifact@v4 with: name: ${{ matrix.artifact-name }} path: pkg - - uses: actions/upload-artifact@v3.1.1 + - uses: actions/upload-artifact@v4 with: name: ${{ matrix.artifact-name }}-java-libraries path: java/src/main/resources @@ -83,7 +88,7 @@ jobs: name: "Wasm" runs-on: ubuntu-22.04 steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: Setup emscripten uses: numworks/setup-emscripten@latest @@ -97,7 +102,7 @@ jobs: - name: install run: cmake --install build --config Release --prefix pkg - - uses: actions/upload-artifact@v3.1.1 + - uses: actions/upload-artifact@v4 with: name: Wasm path: pkg @@ -115,31 +120,31 @@ jobs: packages: write steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - - uses: actions/download-artifact@v3 + - uses: actions/download-artifact@v4 with: name: Native - Windows (CasADi)-java-libraries path: java/src/main/resources - - uses: actions/download-artifact@v3 + - uses: actions/download-artifact@v4 with: name: Native - macOS x86_64 (CasADi)-java-libraries path: java/src/main/resources - - uses: actions/download-artifact@v3 + - uses: actions/download-artifact@v4 with: name: Native - macOS arm64 (CasADi)-java-libraries path: java/src/main/resources - - uses: actions/download-artifact@v3 + - uses: actions/download-artifact@v4 with: name: Native - Linux (CasADi)-java-libraries path: java/src/main/resources - run: cd java/src/main/resources/osx;ls - - uses: actions/upload-artifact@v3.1.1 + - uses: actions/upload-artifact@v4 with: name: all-java-libraries path: java/src/main/resources diff --git a/.github/workflows/documentation.yml b/.github/workflows/documentation.yml index e4aa024b..6340c5c6 100644 --- a/.github/workflows/documentation.yml +++ b/.github/workflows/documentation.yml @@ -11,17 +11,17 @@ jobs: name: "Doxygen" runs-on: ubuntu-22.04 steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: Create build directory run: mkdir build - name: Run Doxygen - uses: mattnotmitt/doxygen-action@1.9.5 + uses: mattnotmitt/doxygen-action@edge with: doxyfile-path: 'docs/Doxyfile' - - uses: actions/upload-artifact@v3.1.1 + - uses: actions/upload-artifact@v4 with: name: documentation path: build/docs diff --git a/.github/workflows/lint-format.yml b/.github/workflows/lint-format.yml index c6182244..5a063b57 100644 --- a/.github/workflows/lint-format.yml +++ b/.github/workflows/lint-format.yml @@ -11,11 +11,11 @@ jobs: name: "wpiformat" runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 + with: + fetch-depth: 0 - name: Fetch all history and metadata run: | - git config --global --add safe.directory /__w/TrajoptLib/TrajoptLib - git fetch --prune --unshallow git checkout -b pr git branch -f main origin/main - name: Set up Python 3.8 @@ -31,7 +31,7 @@ jobs: - name: Generate diff run: git diff HEAD > wpiformat-fixes.patch if: ${{ failure() }} - - uses: actions/upload-artifact@v3 + - uses: actions/upload-artifact@v4 with: name: wpiformat fixes path: wpiformat-fixes.patch @@ -47,13 +47,14 @@ jobs: tidy: name: "clang-tidy" runs-on: ubuntu-latest - container: wpilib/roborio-cross-ubuntu:2023-22.04 + container: wpilib/roborio-cross-ubuntu:2024-22.04 steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 + with: + fetch-depth: 0 - name: Fetch all history and metadata run: | git config --global --add safe.directory /__w/TrajoptLib/TrajoptLib - git fetch --prune --unshallow git checkout -b pr git branch -f main origin/main - name: Set up Python 3.8 @@ -74,3 +75,12 @@ jobs: run: wpiformat -list-changed-files - name: Run clang-tidy run: wpiformat -no-format -tidy-all -compile-commands=build -tidy-extra-args std=c++20 -vv + + format-rust: + name: "Format Rust" + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + - name: Run rustfmt + working-directory: rust + run: cargo fmt --check diff --git a/.github/workflows/publish-java.yml b/.github/workflows/publish-java.yml index b3571f8c..50ab32a1 100644 --- a/.github/workflows/publish-java.yml +++ b/.github/workflows/publish-java.yml @@ -18,7 +18,7 @@ jobs: packages: write steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - uses: actions/setup-java@v3 with: @@ -40,7 +40,7 @@ jobs: working-directory: java run: ./gradlew jar - - uses: actions/upload-artifact@v3.1.1 + - uses: actions/upload-artifact@v4 with: name: TrajoptLib-jar path: java/build/libs/*.jar diff --git a/.github/workflows/website.yml b/.github/workflows/website.yml index af1f3702..7a34d0a2 100644 --- a/.github/workflows/website.yml +++ b/.github/workflows/website.yml @@ -2,7 +2,8 @@ name: Website on: push: - branches: ["main"] + branches: + - main # Allows you to run this workflow manually from the Actions tab workflow_dispatch: @@ -21,6 +22,8 @@ concurrency: jobs: # Single deploy job since we're just deploying deploy: + if: ${{ github.repository_owner == 'SleipnirGroup' }} + environment: name: github-pages url: ${{ steps.deployment.outputs.page_url }} @@ -28,11 +31,11 @@ jobs: runs-on: ubuntu-22.04 steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Create build directory run: mkdir build - name: Run Doxygen - uses: mattnotmitt/doxygen-action@1.9.5 + uses: mattnotmitt/doxygen-action@edge with: doxyfile-path: 'docs/Doxyfile' - name: Setup Pages diff --git a/.gitignore b/.gitignore index 3dbf9c15..7177ac84 100644 --- a/.gitignore +++ b/.gitignore @@ -2,7 +2,7 @@ .gradle # Ignore Gradle build output directory -build +build*/ altbuild # Ignore resources dir which will eventually hold binaries @@ -32,3 +32,6 @@ compile_commands.json # Ignore node node_modules nodebuild + +# Ignore rust build +rust/target/ diff --git a/.styleguide b/.styleguide index af39ff55..8c3e3765 100644 --- a/.styleguide +++ b/.styleguide @@ -24,8 +24,6 @@ licenseUpdateExclude { includeOtherLibs { ^Eigen/ - ^fmt/ ^casadi/ ^sleipnir/ - ^gtest/ } diff --git a/CMakeLists.txt b/CMakeLists.txt index f52b3f3f..b1aa1b63 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,13 +1,19 @@ # Disable in-source builds to prevent source tree corruption -if ("${CMAKE_CURRENT_SOURCE_DIR}" STREQUAL "${CMAKE_CURRENT_BINARY_DIR}") - message(FATAL_ERROR " +if("${CMAKE_CURRENT_SOURCE_DIR}" STREQUAL "${CMAKE_CURRENT_BINARY_DIR}") + message( + FATAL_ERROR + " FATAL: In-source builds are not allowed. You should create a separate directory for build files. -") +" + ) endif() cmake_minimum_required(VERSION 3.24) -set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}/cmake/modules") +set(CMAKE_MODULE_PATH + ${CMAKE_MODULE_PATH} + "${CMAKE_CURRENT_SOURCE_DIR}/cmake/modules" +) project(TrajoptLib LANGUAGES CXX) @@ -18,27 +24,30 @@ set(CMAKE_SKIP_BUILD_RPATH FALSE) # installing) set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE) -set(CMAKE_INSTALL_RPATH ${CMAKE_INSTALL_PREFIX}/lib) - -# add the automatically determined parts of the RPATH -# which point to directories outside the build tree to the install RPATH +# Add the automatically determined parts of the RPATH which point to directories +# outside the build tree to the install RPATH set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) -# the RPATH to be used when installing, but only if it's not a system directory -list(FIND CMAKE_PLATFORM_IMPLICIT_LINK_DIRECTORIES "${CMAKE_INSTALL_PREFIX}/lib" isSystemDir) -if ("${isSystemDir}" STREQUAL "-1") - set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib") -endif("${isSystemDir}" STREQUAL "-1") +# The RPATH to be used when installing, but only if it's not a system directory +list( + FIND + CMAKE_PLATFORM_IMPLICIT_LINK_DIRECTORIES + "${CMAKE_INSTALL_PREFIX}/lib" + isSystemDir +) +if("${isSystemDir}" STREQUAL "-1") + list(APPEND CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib") +endif() # Set default build type to release with debug info (i.e. release mode # optimizations are performed, but debug info still exists). -if (NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE "RelWithDebInfo" CACHE STRING "" FORCE) +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE "RelWithDebInfo" CACHE STRING "" FORCE) endif() # Generate compile_commands.json by default -if (NOT CMAKE_EXPORT_COMPILE_COMMANDS) - set(CMAKE_EXPORT_COMPILE_COMMANDS "YES" CACHE STRING "" FORCE) +if(NOT CMAKE_EXPORT_COMPILE_COMMANDS) + set(CMAKE_EXPORT_COMPILE_COMMANDS "YES" CACHE STRING "" FORCE) endif() # Control where the static and shared libraries are built so that on Windows, @@ -54,11 +63,14 @@ set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS FALSE) include(TrajoptLibCompilerFlags) file(GLOB_RECURSE src src/*.cpp) +if(${OPTIMIZER_BACKEND} STREQUAL "casadi") + list(FILTER src EXCLUDE REGEX "SleipnirOpti.cpp") +elseif(${OPTIMIZER_BACKEND} STREQUAL "sleipnir") + list(FILTER src EXCLUDE REGEX "CasADiOpti.cpp") +endif() add_library(TrajoptLib ${src}) trajoptlib_compiler_flags(TrajoptLib) -target_include_directories(TrajoptLib - PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/src) +target_include_directories(TrajoptLib PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/src) set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS TRUE) @@ -67,200 +79,249 @@ set_target_properties(TrajoptLib PROPERTIES DEBUG_POSTFIX "d") set_property(TARGET TrajoptLib PROPERTY FOLDER "libraries") target_compile_definitions(TrajoptLib PRIVATE TRAJOPT_EXPORTS) +include(CMakeDependentOption) +include(CTest) include(FetchContent) -# fmt dependency -set(BUILD_SHARED_LIBS_SAVE ${BUILD_SHARED_LIBS}) -set(BUILD_SHARED_LIBS OFF) -set(CMAKE_POSITION_INDEPENDENT_CODE ${BUILD_SHARED_LIBS_SAVE}) -set(FMT_INSTALL ON) -FetchContent_Declare( - fmt - GIT_REPOSITORY https://github.com/fmtlib/fmt.git - GIT_TAG 9.1.0 -) -FetchContent_MakeAvailable(fmt) -set(BUILD_SHARED_LIBS ${BUILD_SHARED_LIBS_SAVE}) - -if (${OPTIMIZER_BACKEND} STREQUAL "casadi" AND - ${CMAKE_SYSTEM_NAME} STREQUAL "Linux" AND - ${CMAKE_CXX_COMPILER_ID} STREQUAL "GNU") - target_compile_definitions(fmt PRIVATE _GLIBCXX_USE_CXX11_ABI=0) +if(BUILD_TESTING) + # Catch2 dependency + fetchcontent_declare( + Catch2 + GIT_REPOSITORY https://github.com/catchorg/Catch2.git + GIT_TAG v3.5.2 + CMAKE_ARGS + ) + fetchcontent_makeavailable(Catch2) endif() -target_link_libraries(TrajoptLib PUBLIC fmt) - set(OPTIMIZER_BACKEND "casadi" CACHE STRING "Optimizer backend") set_property(CACHE OPTIMIZER_BACKEND PROPERTY STRINGS casadi sleipnir) -if (${OPTIMIZER_BACKEND} STREQUAL "casadi") - message(STATUS "Using CasADi optimizer") - include(FetchCasADi) - fetch_casadi() - target_compile_definitions(TrajoptLib PRIVATE OPTIMIZER_BACKEND_CASADI) - target_include_directories(TrajoptLib SYSTEM PRIVATE ${CASADI_INCLUDEDIR}) - target_link_directories(TrajoptLib PRIVATE ${CASADI_LIBDIR}) - target_link_libraries(TrajoptLib PRIVATE casadi) - - # Install CasADi libraries since FetchContent setting that up properly - install(FILES ${CASADI_INSTALL_LIBS} DESTINATION ${CASADI_INSTALL_DEST}) +if(${OPTIMIZER_BACKEND} STREQUAL "casadi") + message(STATUS "Using CasADi optimizer") + include(FetchCasADi) + fetch_casadi() + target_compile_definitions(TrajoptLib PRIVATE OPTIMIZER_BACKEND_CASADI) + target_include_directories(TrajoptLib SYSTEM PRIVATE ${CASADI_INCLUDEDIR}) + target_link_directories(TrajoptLib PRIVATE ${CASADI_LIBDIR}) + target_link_libraries(TrajoptLib PRIVATE casadi) + + # Add current directory to RPATH + if(APPLE) + set_property( + TARGET TrajoptLib + APPEND + PROPERTY INSTALL_RPATH "@loader_path/../lib;@loader_path" + ) + elseif(UNIX) + set_property( + TARGET TrajoptLib + APPEND + PROPERTY INSTALL_RPATH "$ORIGIN/../lib;$ORIGIN" + ) + endif() + # Install CasADi libraries since FetchContent isn't setting that up properly + install(FILES ${CASADI_INSTALL_LIBS} DESTINATION ${CASADI_INSTALL_DEST}) elseif(${OPTIMIZER_BACKEND} STREQUAL "sleipnir") - message(STATUS "Using Sleipnir optimizer") - include(FetchSleipnir) - fetch_sleipnir() - target_compile_definitions(TrajoptLib PRIVATE OPTIMIZER_BACKEND_SLEIPNIR) - target_link_libraries(TrajoptLib PRIVATE Sleipnir) - - # Install Sleipnir manually because EXCLUDE_FROM_ALL was used during import - install(TARGETS Sleipnir) + message(STATUS "Using Sleipnir optimizer") + fetchcontent_declare( + Sleipnir + GIT_REPOSITORY https://github.com/SleipnirGroup/Sleipnir.git + # main on 2024-02-18 + GIT_TAG 78d7673b6a8d8bc352726b95f5319416c0435069 + ) + fetchcontent_makeavailable(Sleipnir) + target_compile_definitions(TrajoptLib PRIVATE OPTIMIZER_BACKEND_SLEIPNIR) + target_link_libraries(TrajoptLib PRIVATE Sleipnir) endif() -target_include_directories(TrajoptLib - PUBLIC - $ - $) - -install(TARGETS TrajoptLib - EXPORT TrajoptLibTargets - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin - INCLUDES DESTINATION include) -export(TARGETS TrajoptLib - FILE TrajoptLib.cmake - NAMESPACE TrajoptLib::) -install(DIRECTORY include/ DESTINATION "include/TrajoptLib") -install(EXPORT TrajoptLibTargets - FILE TrajoptLib.cmake - NAMESPACE TrajoptLib:: - DESTINATION lib/cmake/TrajoptLib) +target_include_directories( + TrajoptLib + PUBLIC + $ + $ +) + +install( + TARGETS TrajoptLib + EXPORT TrajoptLibTargets + LIBRARY + DESTINATION lib + ARCHIVE + DESTINATION lib + RUNTIME + DESTINATION bin + INCLUDES DESTINATION include +) +export(TARGETS TrajoptLib FILE TrajoptLib.cmake NAMESPACE TrajoptLib::) +install(DIRECTORY include/ COMPONENT TrajoptLib DESTINATION "include") +install( + EXPORT TrajoptLibTargets + FILE TrajoptLib.cmake + NAMESPACE TrajoptLib:: + DESTINATION lib/cmake/TrajoptLib +) include(CMakePackageConfigHelpers) # Generate the config file that includes the exports configure_package_config_file( - ${CMAKE_CURRENT_SOURCE_DIR}/TrajoptLibConfig.cmake.in - ${CMAKE_CURRENT_BINARY_DIR}/TrajoptLibConfig.cmake - INSTALL_DESTINATION "lib/cmake/TrajoptLib" - NO_SET_AND_CHECK_MACRO - NO_CHECK_REQUIRED_COMPONENTS_MACRO) + ${CMAKE_CURRENT_SOURCE_DIR}/TrajoptLibConfig.cmake.in + ${CMAKE_CURRENT_BINARY_DIR}/TrajoptLibConfig.cmake + INSTALL_DESTINATION "lib/cmake/TrajoptLib" + NO_SET_AND_CHECK_MACRO + NO_CHECK_REQUIRED_COMPONENTS_MACRO +) # Install the config file -install(FILES - ${CMAKE_CURRENT_BINARY_DIR}/TrajoptLibConfig.cmake - DESTINATION lib/cmake/TrajoptLib) - -# GoogleTest dependency (static linkage) -set(BUILD_SHARED_LIBS_SAVE ${BUILD_SHARED_LIBS}) -set(BUILD_SHARED_LIBS OFF) -option(INSTALL_GMOCK "Install GoogleTest's GMock" OFF) -option(INSTALL_GTEST "Install GoogleTest's GTest" OFF) -FetchContent_Declare( - googletest - GIT_REPOSITORY https://github.com/google/googletest.git - GIT_TAG v1.13.0 - CMAKE_ARGS "-DCMAKE_BUILD_TYPE=ReleaseWithDebInfo" - FIND_PACKAGE_ARGS NAMES GTest +install( + FILES ${CMAKE_CURRENT_BINARY_DIR}/TrajoptLibConfig.cmake + COMPONENT TrajoptLib + DESTINATION lib/cmake/TrajoptLib ) -FetchContent_MakeAvailable(googletest) -set(BUILD_SHARED_LIBS ${BUILD_SHARED_LIBS_SAVE}) -include(CMakeDependentOption) - -cmake_dependent_option(BUILD_TESTING "Enable creation of tests." OFF "PROJECT_IS_TOP_LEVEL" OFF) -if (BUILD_TESTING) - enable_testing() - include(GoogleTest) +if(BUILD_TESTING) + enable_testing() + list(APPEND CMAKE_MODULE_PATH ${Catch2_SOURCE_DIR}/extras) + include(Catch) + if( + ${OPTIMIZER_BACKEND} STREQUAL "casadi" + AND ${CMAKE_SYSTEM_NAME} STREQUAL "Linux" + AND ${CMAKE_CXX_COMPILER_ID} STREQUAL "GNU" + ) + target_compile_definitions(Catch2 PUBLIC "_GLIBCXX_USE_CXX11_ABI=0") + target_compile_definitions( + Catch2WithMain + PUBLIC "_GLIBCXX_USE_CXX11_ABI=0" + ) + endif() endif() # Build TrajoptLib tests -if (BUILD_TESTING) - file(GLOB_RECURSE TrajoptLib_test_src test/src/*.cpp) - add_executable(TrajoptLibTest ${TrajoptLib_test_src}) - TrajoptLib_compiler_flags(TrajoptLibTest) - target_include_directories(TrajoptLibTest - PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/src - ${CMAKE_CURRENT_SOURCE_DIR}/test/include - ${CMAKE_CURRENT_SOURCE_DIR}/thirdparty/units/include) - target_link_libraries(TrajoptLibTest - PRIVATE - TrajoptLib - fmt::fmt - GTest::gtest - GTest::gtest_main) - if (NOT CMAKE_TOOLCHAIN_FILE) - gtest_discover_tests(TrajoptLibTest) - endif() +if(BUILD_TESTING) + file(GLOB_RECURSE TrajoptLib_test_src test/src/*.cpp) + add_executable(TrajoptLibTest ${TrajoptLib_test_src}) + trajoptlib_compiler_flags(TrajoptLibTest) + target_include_directories( + TrajoptLibTest + PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/src + ${CMAKE_CURRENT_SOURCE_DIR}/test/include + ${CMAKE_CURRENT_SOURCE_DIR}/thirdparty/units/include + ) + target_link_libraries(TrajoptLibTest PRIVATE TrajoptLib Catch2::Catch2) + if(NOT CMAKE_TOOLCHAIN_FILE) + catch_discover_tests(TrajoptLibTest) + endif() endif() - - # Build examples and example tests include(TrajoptLibSubdirList) trajoptlib_subdir_list(EXAMPLES ${CMAKE_CURRENT_SOURCE_DIR}/examples) foreach(example ${EXAMPLES}) - # Build example - file(GLOB_RECURSE sources examples/${example}/src/*.cpp) - add_executable(${example} ${sources}) - trajoptlib_compiler_flags(${example}) - target_include_directories(${example} - PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/examples/${example}/include) - target_link_libraries(${example} PRIVATE TrajoptLib) - - # Build example test if files exist for it - if (BUILD_TESTING AND EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/examples/${example}/test) - file(GLOB_RECURSE test_sources examples/${example}/test/*.cpp) - add_executable(${example}Test ${sources} ${test_sources}) - target_include_directories(${example}Test - PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/examples/${example}/src - ${CMAKE_CURRENT_SOURCE_DIR}/examples/${example}/test) - trajoptlib_compiler_flags(${example}Test) - target_compile_definitions(${example}Test PUBLIC RUNNING_TESTS) - target_include_directories(${example}Test - PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/examples/${example}/include) - target_link_libraries(${example}Test - PRIVATE - TrajoptLib - GTest::gtest - GTest::gtest_main) - if (NOT CMAKE_TOOLCHAIN_FILE) - gtest_discover_tests(${example}Test) + # Build example + file(GLOB_RECURSE sources examples/${example}/src/*.cpp) + add_executable(${example} ${sources}) + trajoptlib_compiler_flags(${example}) + target_include_directories( + ${example} + PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/examples/${example}/include + ) + target_link_libraries(${example} PRIVATE TrajoptLib) + + # Build example test if files exist for it + if( + BUILD_TESTING + AND EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/examples/${example}/test + ) + file(GLOB_RECURSE test_sources examples/${example}/test/*.cpp) + add_executable(${example}Test ${sources} ${test_sources}) + target_include_directories( + ${example}Test + PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/examples/${example}/src + ${CMAKE_CURRENT_SOURCE_DIR}/examples/${example}/test + ) + trajoptlib_compiler_flags(${example}Test) + target_compile_definitions(${example}Test PUBLIC RUNNING_TESTS) + target_include_directories( + ${example}Test + PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/examples/${example}/include + ) + target_link_libraries( + ${example}Test + PRIVATE TrajoptLib Catch2::Catch2WithMain + ) + if(NOT CMAKE_TOOLCHAIN_FILE) + catch_discover_tests(${example}Test) + endif() endif() - endif() endforeach() # Build the Java bindings option(WITH_JAVA "Build Java bindings" OFF) -cmake_dependent_option(JAVA_INSTALL_JAR "Install jar file." ON "WITH_JAVA" ON) - -if (WITH_JAVA) - add_library(TrajoptLib-java SHARED ${CMAKE_CURRENT_SOURCE_DIR}/jni/TrajoptLibJNI.cpp) - target_link_libraries(TrajoptLib-java PRIVATE TrajoptLib) - target_include_directories(TrajoptLib-java PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/jni/jni.h) - if (${CMAKE_SYSTEM_NAME} MATCHES "MINGW" OR ${CMAKE_SYSTEM_NAME} MATCHES "MSYS" OR WIN32) - target_include_directories(TrajoptLib-java PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/jni/win32) - elseif (APPLE) - target_include_directories(TrajoptLib-java PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/jni/darwin) - elseif (UNIX) - target_include_directories(TrajoptLib-java PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/jni/linux) - endif() - include(InstallJavaLibs) - installjavalibs() - - # Build Jar containing embedded native libraries - if (JAVA_INSTALL_JAR) - if (UNIX) - install(CODE "execute_process(COMMAND ./gradlew jar WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/java)") - elseif(${CMAKE_SYSTEM_NAME} MATCHES "MINGW" OR ${CMAKE_SYSTEM_NAME} MATCHES "MSYS" OR WIN32) - install(CODE "execute_process(COMMAND .\\gradlew.bat jar WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/java)") +cmake_dependent_option( + JAVA_INSTALL_JAR + "Install jar file." + ON + "WITH_JAVA" + ON +) + +if(WITH_JAVA) + add_library( + TrajoptLib-java + SHARED + ${CMAKE_CURRENT_SOURCE_DIR}/jni/TrajoptLibJNI.cpp + ) + target_link_libraries(TrajoptLib-java PRIVATE TrajoptLib) + target_include_directories( + TrajoptLib-java + PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/jni/jni.h + ) + if( + ${CMAKE_SYSTEM_NAME} MATCHES "MINGW" + OR ${CMAKE_SYSTEM_NAME} MATCHES "MSYS" + OR WIN32 + ) + target_include_directories( + TrajoptLib-java + PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/jni/win32 + ) + elseif(APPLE) + target_include_directories( + TrajoptLib-java + PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/jni/darwin + ) + elseif(UNIX) + target_include_directories( + TrajoptLib-java + PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/jni/linux + ) + endif() + include(InstallJavaLibs) + installjavalibs() + + # Build Jar containing embedded native libraries + if(JAVA_INSTALL_JAR) + if(UNIX) + install( + CODE + "execute_process(COMMAND ./gradlew jar WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/java)" + ) + elseif( + ${CMAKE_SYSTEM_NAME} MATCHES "MINGW" + OR ${CMAKE_SYSTEM_NAME} MATCHES "MSYS" + OR WIN32 + ) + install( + CODE + "execute_process(COMMAND .\\gradlew.bat jar WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/java)" + ) + endif() + file(GLOB jar_files "${CMAKE_CURRENT_SOURCE_DIR}/java/build/libs/*jar") + install(FILES ${jar_files} DESTINATION java) endif() - file(GLOB jar_files "${CMAKE_CURRENT_SOURCE_DIR}/java/build/libs/*jar") - install(FILES ${jar_files} DESTINATION java) - endif() endif() diff --git a/README.md b/README.md index 60ddb844..abbece32 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# HelixTrajectory +# TrajoptLib ![Build](https://github.com/SleipnirGroup/TrajoptLib/actions/workflows/build.yml/badge.svg) [![C++ Documentation](https://img.shields.io/badge/documentation-c%2B%2B-blue)](https://sleipnirgroup.github.io/TrajoptLib/) diff --git a/cmake/modules/FetchCasADi.cmake b/cmake/modules/FetchCasADi.cmake index f620b7b8..17e05b15 100644 --- a/cmake/modules/FetchCasADi.cmake +++ b/cmake/modules/FetchCasADi.cmake @@ -1,75 +1,111 @@ macro(fetch_casadi) - cmake_POLICY(SET CMP0135 NEW) - set(CASADI_LIBDIR ${CMAKE_BINARY_DIR}/_deps/casadi-src/casadi) - set(CASADI_INCLUDEDIR ${CMAKE_BINARY_DIR}/_deps/casadi-src/casadi/include) - if (${CMAKE_SYSTEM_NAME} MATCHES "MINGW" OR ${CMAKE_SYSTEM_NAME} MATCHES "MSYS" OR WIN32) - message(STATUS "Building for Windows") - set(CASADI_URL https://github.com/casadi/casadi/releases/download/3.6.3/casadi-3.6.3-windows64-py311.zip) - set(CASADI_INSTALL_LIBS - ${CASADI_LIBDIR}/libcasadi.dll - ${CASADI_LIBDIR}/libstdc++-6.dll - ${CASADI_LIBDIR}/libcasadi_nlpsol_ipopt.dll - ${CASADI_LIBDIR}/libquadmath-0.dll - ${CASADI_LIBDIR}/libgcc_s_seh-1.dll) - set(CASADI_INSTALL_DEST "bin") - elseif (APPLE) - if (CMAKE_APPLE_SILICON_PROCESSOR MATCHES "arm64") - message(STATUS "Building for macOS arm64") - set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_RPATH};@loader_path/../lib;@loader_path") - set(CASADI_URL https://github.com/casadi/casadi/releases/download/3.6.3/casadi-3.6.3-osx_arm64-py311.zip) - set(CASADI_INSTALL_LIBS - ${CASADI_LIBDIR}/libcasadi.3.7.dylib - ${CASADI_LIBDIR}/libc++.1.0.dylib - ${CASADI_LIBDIR}/libcasadi_nlpsol_ipopt.dylib - ${CASADI_LIBDIR}/libipopt.3.dylib - ${CASADI_LIBDIR}/libcoinmumps.3.dylib - ${CASADI_LIBDIR}/libcoinmetis.2.dylib - ${CASADI_LIBDIR}/libgfortran.5.dylib - ${CASADI_LIBDIR}/libquadmath.0.dylib - ${CASADI_LIBDIR}/libgcc_s.1.1.dylib) - elseif(CMAKE_APPLE_SILICON_PROCESSOR MATCHES "x86_64") - message(STATUS "Building for macOS x86_64") - set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_RPATH};@loader_path/../lib;@loader_path") - set(CASADI_URL https://github.com/casadi/casadi/releases/download/3.6.3/casadi-3.6.3-osx64-py311.zip) - set(CASADI_INSTALL_LIBS - ${CASADI_LIBDIR}/libcasadi.3.7.dylib - ${CASADI_LIBDIR}/libc++.1.0.dylib - ${CASADI_LIBDIR}/libcasadi_nlpsol_ipopt.3.7.dylib - ${CASADI_LIBDIR}/libipopt.3.dylib - ${CASADI_LIBDIR}/libcoinmumps.3.dylib - ${CASADI_LIBDIR}/libcoinmetis.2.dylib - ${CASADI_LIBDIR}/libgfortran.5.dylib - ${CASADI_LIBDIR}/libquadmath.0.dylib - ${CASADI_LIBDIR}/libgcc_s.1.dylib - ${CASADI_LIBDIR}/libgcc_s.1.1.dylib) + cmake_policy(SET CMP0135 NEW) + set(CASADI_LIBDIR ${CMAKE_BINARY_DIR}/_deps/casadi-src/casadi) + set(CASADI_INCLUDEDIR ${CMAKE_BINARY_DIR}/_deps/casadi-src/casadi/include) + if( + ${CMAKE_SYSTEM_NAME} MATCHES "MINGW" + OR ${CMAKE_SYSTEM_NAME} MATCHES "MSYS" + OR WIN32 + ) + message(STATUS "Building for Windows") + set(CASADI_URL + https://github.com/casadi/casadi/releases/download/3.6.4/casadi-3.6.4-windows64-py311.zip + ) + set(CASADI_INSTALL_LIBS + ${CASADI_LIBDIR}/libcasadi-tp-openblas.dll + ${CASADI_LIBDIR}/libcasadi.dll + ${CASADI_LIBDIR}/libcasadi_nlpsol_ipopt.dll + ${CASADI_LIBDIR}/libcoinmetis-2.dll + ${CASADI_LIBDIR}/libcoinmumps-3.dll + ${CASADI_LIBDIR}/libgcc_s_seh-1.dll + ${CASADI_LIBDIR}/libgfortran-5.dll + ${CASADI_LIBDIR}/libipopt-3.dll + ${CASADI_LIBDIR}/libquadmath-0.dll + ${CASADI_LIBDIR}/libstdc++-6.dll + ${CASADI_LIBDIR}/libwinpthread-1.dll + ) + set(CASADI_INSTALL_DEST "bin") + elseif(APPLE) + if( + CMAKE_APPLE_SILICON_PROCESSOR MATCHES "arm64" + OR CMAKE_OSX_ARCHITECTURES MATCHES "arm64" + ) + message(STATUS "Building for macOS arm64") + set(CASADI_URL + https://github.com/casadi/casadi/releases/download/3.6.4/casadi-3.6.4-osx_arm64-py311.zip + ) + set(CASADI_INSTALL_LIBS + ${CASADI_LIBDIR}/libcasadi.3.7.dylib + ${CASADI_LIBDIR}/libc++.1.0.dylib + ${CASADI_LIBDIR}/libcasadi_nlpsol_ipopt.dylib + ${CASADI_LIBDIR}/libipopt.3.dylib + ${CASADI_LIBDIR}/libcoinmumps.3.dylib + ${CASADI_LIBDIR}/libcoinmetis.2.dylib + ${CASADI_LIBDIR}/libgfortran.5.dylib + ${CASADI_LIBDIR}/libquadmath.0.dylib + ${CASADI_LIBDIR}/libgcc_s.1.1.dylib + ) + elseif( + CMAKE_APPLE_SILICON_PROCESSOR MATCHES "x86_64" + OR CMAKE_OSX_ARCHITECTURES MATCHES "x86_64" + ) + message(STATUS "Building for macOS x86_64") + set(CASADI_URL + https://github.com/casadi/casadi/releases/download/3.6.4/casadi-3.6.4-osx64-py311.zip + ) + set(CASADI_INSTALL_LIBS + ${CASADI_LIBDIR}/libcasadi.3.7.dylib + ${CASADI_LIBDIR}/libc++.1.0.dylib + ${CASADI_LIBDIR}/libcasadi_nlpsol_ipopt.dylib + ${CASADI_LIBDIR}/libipopt.3.dylib + ${CASADI_LIBDIR}/libcoinmumps.3.dylib + ${CASADI_LIBDIR}/libcoinmetis.2.dylib + ${CASADI_LIBDIR}/libgfortran.5.dylib + ${CASADI_LIBDIR}/libquadmath.0.dylib + ${CASADI_LIBDIR}/libgcc_s.1.dylib + ${CASADI_LIBDIR}/libgcc_s.1.1.dylib + ) + endif() + set(CASADI_INSTALL_DEST "lib") + elseif(UNIX) + if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "aarch64") + message(STATUS "Building for Linux aarch64") + set(CASADI_URL + https://github.com/casadi/casadi/releases/download/3.6.4/casadi-3.6.4-linux-aarch64-py311.zip + ) + set(CASADI_INSTALL_LIBS + ${CASADI_LIBDIR}/libcasadi.so.3.7 + ${CASADI_LIBDIR}/libcasadi_nlpsol_ipopt.so + ${CASADI_LIBDIR}/libipopt.so.3 + ${CASADI_LIBDIR}/libcoinmumps.so.3 + ${CASADI_LIBDIR}/libcoinmetis.so.2 + ${CASADI_LIBDIR}/libgfortran-040039e1.so.5.0.0 + ${CASADI_LIBDIR}/libquadmath-96973f99.so.0.0.0 + ${CASADI_LIBDIR}/libcasadi-tp-openblas.so.0 + ) + else() + message(STATUS "Building for Linux x86_64") + set(CASADI_URL + https://github.com/casadi/casadi/releases/download/3.6.4/casadi-3.6.4-linux64-py311.zip + ) + set(CASADI_INSTALL_LIBS + ${CASADI_LIBDIR}/libcasadi.so.3.7 + ${CASADI_LIBDIR}/libcasadi_nlpsol_ipopt.so + ${CASADI_LIBDIR}/libipopt.so.3 + ${CASADI_LIBDIR}/libcoinmumps.so.3 + ${CASADI_LIBDIR}/libcoinmetis.so.2 + ${CASADI_LIBDIR}/libgfortran-040039e1.so.5.0.0 + ${CASADI_LIBDIR}/libquadmath-96973f99.so.0.0.0 + ${CASADI_LIBDIR}/libcasadi-tp-openblas.so.0 + ) + endif() + set(CASADI_INSTALL_DEST "lib") endif() - set(CASADI_INSTALL_DEST "lib") - elseif (UNIX) - if (${CMAKE_SYSTEM_PROCESSOR} MATCHES "ARM64") - message(STATUS "Building for Linux arm64") - set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_RPATH};$ORIGIN/../lib;$ORIGIN") - set(CASADI_URL https://github.com/casadi/casadi/releases/download/3.6.3/casadi-3.6.3-linux-aarch64-py311.zip) - set(CASADI_INSTALL_LIBS - ${CASADI_LIBDIR}/libcasadi.so - ${CASADI_LIBDIR}/libcasadi_nlpsol_ipopt.so) - else() - message(STATUS "Building for Linux x64") - set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_RPATH};$ORIGIN/../lib;$ORIGIN") - set(CASADI_URL https://github.com/casadi/casadi/releases/download/3.6.3/casadi-3.6.3-linux64-py311.zip) - set(CASADI_INSTALL_LIBS - ${CASADI_LIBDIR}/libcasadi.so - ${CASADI_LIBDIR}/libcasadi_nlpsol_ipopt.so) - endif() - set(CASADI_INSTALL_DEST "lib") - endif() - message(STATUS "Downloading CasADi from ${CASADI_URL}") + message(STATUS "Downloading CasADi from ${CASADI_URL}") - include(FetchContent) + include(FetchContent) - FetchContent_Declare( - casadi - URL ${CASADI_URL} - ) + fetchcontent_declare(casadi URL ${CASADI_URL}) - FetchContent_MakeAvailable(casadi) + fetchcontent_makeavailable(casadi) endmacro() diff --git a/cmake/modules/FetchSleipnir.cmake b/cmake/modules/FetchSleipnir.cmake deleted file mode 100644 index 7961ca40..00000000 --- a/cmake/modules/FetchSleipnir.cmake +++ /dev/null @@ -1,15 +0,0 @@ -macro(fetch_sleipnir) - include(FetchContent) - - FetchContent_Declare( - Sleipnir - GIT_REPOSITORY https://github.com/SleipnirGroup/Sleipnir.git - GIT_TAG 0d20a9f712f1e997f734038a977392d8bc6a19f4 - ) - - FetchContent_GetProperties(Sleipnir) - if(NOT Sleipnir_POPULATED) - FetchContent_Populate(Sleipnir) - add_subdirectory(${sleipnir_SOURCE_DIR} ${sleipnir_BINARY_DIR} EXCLUDE_FROM_ALL) - endif() -endmacro() diff --git a/cmake/modules/InstallJavaLibs.cmake b/cmake/modules/InstallJavaLibs.cmake index fcc6f5e0..c8f9a8f8 100644 --- a/cmake/modules/InstallJavaLibs.cmake +++ b/cmake/modules/InstallJavaLibs.cmake @@ -1,37 +1,54 @@ macro(installjavalibs) - set(JAVA_LIB_TARGET_PATH ${CMAKE_CURRENT_SOURCE_DIR}/java/src/main/resources) - if (${CMAKE_SYSTEM_NAME} MATCHES "MINGW" OR ${CMAKE_SYSTEM_NAME} MATCHES "MSYS" OR WIN32) - if (CMAKE_SIZEOF_VOID_P EQUAL 8) - set(JAVA_LIB_TARGET_PATH ${JAVA_LIB_TARGET_PATH}/windows/x86_64) - elseif(CMAKE_SIZEOF_VOID_P EQUAL 4) - set(JAVA_LIB_TARGET_PATH ${JAVA_LIB_TARGET_PATH}/windows/x86) + set(JAVA_LIB_TARGET_PATH + ${CMAKE_CURRENT_SOURCE_DIR}/java/src/main/resources + ) + if( + ${CMAKE_SYSTEM_NAME} MATCHES "MINGW" + OR ${CMAKE_SYSTEM_NAME} MATCHES "MSYS" + OR WIN32 + ) + if(CMAKE_SIZEOF_VOID_P EQUAL 8) + set(JAVA_LIB_TARGET_PATH ${JAVA_LIB_TARGET_PATH}/windows/x86_64) + elseif(CMAKE_SIZEOF_VOID_P EQUAL 4) + set(JAVA_LIB_TARGET_PATH ${JAVA_LIB_TARGET_PATH}/windows/x86) + endif() + elseif(APPLE) + if(CMAKE_APPLE_SILICON_PROCESSOR MATCHES "arm64") + set(JAVA_LIB_TARGET_PATH ${JAVA_LIB_TARGET_PATH}/osx/arm64) + elseif(CMAKE_APPLE_SILICON_PROCESSOR MATCHES "x86_64") + set(JAVA_LIB_TARGET_PATH ${JAVA_LIB_TARGET_PATH}/osx/x86_64) + endif() + elseif(UNIX) + set(JAVA_LIB_TARGET_PATH ${JAVA_LIB_TARGET_PATH}/linux/x86_64) endif() - elseif (APPLE) - if (CMAKE_APPLE_SILICON_PROCESSOR MATCHES "arm64") - set(JAVA_LIB_TARGET_PATH ${JAVA_LIB_TARGET_PATH}/osx/arm64) - elseif(CMAKE_APPLE_SILICON_PROCESSOR MATCHES "x86_64") - set(JAVA_LIB_TARGET_PATH ${JAVA_LIB_TARGET_PATH}/osx/x86_64) - endif() - elseif (UNIX) - set(JAVA_LIB_TARGET_PATH ${JAVA_LIB_TARGET_PATH}/linux/x86_64) - endif() - message(STATUS "Path to use for java: ${JAVA_LIB_TARGET_PATH}") - install(TARGETS TrajoptLib - LIBRARY DESTINATION ${JAVA_LIB_TARGET_PATH} - RUNTIME DESTINATION ${JAVA_LIB_TARGET_PATH}) - install(TARGETS TrajoptLib-java - LIBRARY DESTINATION ${JAVA_LIB_TARGET_PATH} - RUNTIME DESTINATION ${JAVA_LIB_TARGET_PATH}) + message(STATUS "Path to use for java: ${JAVA_LIB_TARGET_PATH}") + install( + TARGETS TrajoptLib + LIBRARY + DESTINATION ${JAVA_LIB_TARGET_PATH} + RUNTIME + DESTINATION ${JAVA_LIB_TARGET_PATH} + ) + install( + TARGETS TrajoptLib-java + LIBRARY + DESTINATION ${JAVA_LIB_TARGET_PATH} + RUNTIME + DESTINATION ${JAVA_LIB_TARGET_PATH} + ) - if (OPTIMIZER_BACKEND STREQUAL "casadi") - install(FILES ${CASADI_INSTALL_LIBS} DESTINATION ${JAVA_LIB_TARGET_PATH}) - endif() + if(OPTIMIZER_BACKEND STREQUAL "casadi") + install( + FILES ${CASADI_INSTALL_LIBS} + DESTINATION ${JAVA_LIB_TARGET_PATH} + ) + endif() - configure_file( - "${CMAKE_CURRENT_SOURCE_DIR}/cmake/modules/GenerateJNILibsJSON.cmake.in" - "${CMAKE_BINARY_DIR}/GenerateJNILibsJSON.cmake" - @ONLY - ) - install(SCRIPT "${CMAKE_BINARY_DIR}/GenerateJNILibsJSON.cmake") + configure_file( + "${CMAKE_CURRENT_SOURCE_DIR}/cmake/modules/GenerateJNILibsJSON.cmake.in" + "${CMAKE_BINARY_DIR}/GenerateJNILibsJSON.cmake" + @ONLY + ) + install(SCRIPT "${CMAKE_BINARY_DIR}/GenerateJNILibsJSON.cmake") endmacro() diff --git a/cmake/modules/TrajoptLibCompilerFlags.cmake b/cmake/modules/TrajoptLibCompilerFlags.cmake index b0f94e4f..eecd2ae1 100644 --- a/cmake/modules/TrajoptLibCompilerFlags.cmake +++ b/cmake/modules/TrajoptLibCompilerFlags.cmake @@ -1,44 +1,40 @@ macro(trajoptlib_compiler_flags target) - if (NOT MSVC) - target_compile_options(${target} PRIVATE -Wall -pedantic -Wextra -Werror -Wno-unused-parameter -Wno-missing-braces) + if(NOT MSVC) + target_compile_options( + ${target} + PRIVATE -Wall -pedantic -Wextra -Werror -Wno-unused-parameter + ) - if (${OPTIMIZER_BACKEND} STREQUAL "casadi" AND - ${CMAKE_SYSTEM_NAME} STREQUAL "Linux" AND - ${CMAKE_CXX_COMPILER_ID} STREQUAL "GNU") - target_compile_definitions(${target} PRIVATE _GLIBCXX_USE_CXX11_ABI=0) - endif() - - # Disable warning false positives in Eigen - if (${CMAKE_CXX_COMPILER_ID} STREQUAL "GNU" AND - ${CMAKE_CXX_COMPILER_VERSION} VERSION_GREATER_EQUAL "8") - target_compile_options(${target} PRIVATE -Wno-class-memaccess) - endif() - if (${CMAKE_CXX_COMPILER_ID} STREQUAL "GNU" AND - ${CMAKE_CXX_COMPILER_VERSION} VERSION_GREATER_EQUAL "11") - target_compile_options(${target} PRIVATE -Wno-maybe-uninitialized) - endif() - if (${CMAKE_CXX_COMPILER_ID} STREQUAL "GNU" AND - ${CMAKE_CXX_COMPILER_VERSION} VERSION_GREATER_EQUAL "12") - target_compile_options(${target} PRIVATE -Wno-array-bounds) - endif() + # clang 18 warns on `operator"" _a` in dependencies + if( + ${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang" + AND ${CMAKE_CXX_COMPILER_VERSION} VERSION_GREATER_EQUAL "18" + ) + target_compile_options( + ${target} + PRIVATE -Wno-deprecated-literal-operator + ) + endif() - # Disable deprecated-anon-enum-enum-conversion warning in Eigen - if (${CMAKE_CXX_COMPILER_ID} STREQUAL "AppleClang" AND - ${CMAKE_CXX_COMPILER_VERSION} VERSION_GREATER_EQUAL "13") - target_compile_options(${target} PRIVATE -Wno-deprecated-anon-enum-enum-conversion) + if( + ${OPTIMIZER_BACKEND} STREQUAL "casadi" + AND ${CMAKE_SYSTEM_NAME} STREQUAL "Linux" + AND ${CMAKE_CXX_COMPILER_ID} STREQUAL "GNU" + ) + target_compile_definitions( + ${target} + PRIVATE _GLIBCXX_USE_CXX11_ABI=0 + ) + endif() + else() + target_compile_options( + ${target} + PRIVATE /wd4146 /wd4244 /wd4251 /wd4267 /WX + ) endif() - # Disable warning false positives in fmt - if (${CMAKE_CXX_COMPILER_ID} STREQUAL "GNU" AND - ${CMAKE_CXX_COMPILER_VERSION} VERSION_GREATER_EQUAL "13") - target_compile_options(${target} PRIVATE -Wno-dangling-reference -Wno-stringop-overflow) + target_compile_features(${target} PUBLIC cxx_std_20) + if(MSVC) + target_compile_options(${target} PUBLIC /bigobj) endif() - else() - target_compile_options(${target} PRIVATE /wd4146 /wd4244 /wd4251 /wd4267 /WX) - endif() - - target_compile_features(${target} PUBLIC cxx_std_20) - if (MSVC) - target_compile_options(${target} PUBLIC /bigobj) - endif() endmacro() diff --git a/cmake/modules/TrajoptLibSubdirList.cmake b/cmake/modules/TrajoptLibSubdirList.cmake index d43186cb..e64b797f 100644 --- a/cmake/modules/TrajoptLibSubdirList.cmake +++ b/cmake/modules/TrajoptLibSubdirList.cmake @@ -1,10 +1,10 @@ macro(trajoptlib_subdir_list result curdir) - file(GLOB children RELATIVE ${curdir} ${curdir}/*) - set(dirlist "") - foreach(child ${children}) - if (IS_DIRECTORY ${curdir}/${child}) - LIST(APPEND dirlist ${child}) - endif() - endforeach() - set(${result} ${dirlist}) + file(GLOB children RELATIVE ${curdir} ${curdir}/*) + set(dirlist "") + foreach(child ${children}) + if(IS_DIRECTORY ${curdir}/${child}) + list(APPEND dirlist ${child}) + endif() + endforeach() + set(${result} ${dirlist}) endmacro() diff --git a/docs/Doxyfile b/docs/Doxyfile index c2e6febb..9b7efb41 100644 --- a/docs/Doxyfile +++ b/docs/Doxyfile @@ -1,4 +1,4 @@ -# Doxyfile 1.9.7 +# Doxyfile 1.9.8 # This file describes the settings to be used by the documentation system # doxygen (www.doxygen.org) for a project. @@ -366,9 +366,9 @@ TOC_INCLUDE_HEADINGS = 5 # The MARKDOWN_ID_STYLE tag can be used to specify the algorithm used to # generate identifiers for the Markdown headings. Note: Every identifier is # unique. -# Possible values are: DOXYGEN Use a fixed 'autotoc_md' string followed by a -# sequence number starting at 0. and GITHUB Use the lower case version of title -# with any whitespace replaced by '-' and punctations characters removed.. +# Possible values are: DOXYGEN use a fixed 'autotoc_md' string followed by a +# sequence number starting at 0 and GITHUB use the lower case version of title +# with any whitespace replaced by '-' and punctuation characters removed. # The default value is: DOXYGEN. # This tag requires that the tag MARKDOWN_SUPPORT is set to YES. @@ -978,12 +978,12 @@ INPUT_FILE_ENCODING = # Note the list of default checked file patterns might differ from the list of # default file extension mappings. # -# If left blank the following patterns are tested:*.c, *.cc, *.cxx, *.cpp, -# *.c++, *.java, *.ii, *.ixx, *.ipp, *.i++, *.inl, *.idl, *.ddl, *.odl, *.h, -# *.hh, *.hxx, *.hpp, *.h++, *.l, *.cs, *.d, *.php, *.php4, *.php5, *.phtml, -# *.inc, *.m, *.markdown, *.md, *.mm, *.dox (to be provided as doxygen C -# comment), *.py, *.pyw, *.f90, *.f95, *.f03, *.f08, *.f18, *.f, *.for, *.vhd, -# *.vhdl, *.ucf, *.qsf and *.ice. +# If left blank the following patterns are tested:*.c, *.cc, *.cxx, *.cxxm, +# *.cpp, *.cppm, *.c++, *.c++m, *.java, *.ii, *.ixx, *.ipp, *.i++, *.inl, *.idl, +# *.ddl, *.odl, *.h, *.hh, *.hxx, *.hpp, *.h++, *.ixx, *.l, *.cs, *.d, *.php, +# *.php4, *.php5, *.phtml, *.inc, *.m, *.markdown, *.md, *.mm, *.dox (to be +# provided as doxygen C comment), *.py, *.pyw, *.f90, *.f95, *.f03, *.f08, +# *.f18, *.f, *.for, *.vhd, *.vhdl, *.ucf, *.qsf and *.ice. FILE_PATTERNS = *.c \ *.cc \ @@ -1433,6 +1433,13 @@ HTML_DYNAMIC_MENUS = YES HTML_DYNAMIC_SECTIONS = NO +# If the HTML_CODE_FOLDING tag is set to YES then classes and functions can be +# dynamically folded and expanded in the generated HTML source code. +# The default value is: YES. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_CODE_FOLDING = YES + # With HTML_INDEX_NUM_ENTRIES one can control the preferred number of entries # shown in the various tree structured indices initially; the user can expand # and collapse entries dynamically later on. Doxygen will expand the tree to @@ -2061,7 +2068,7 @@ PDF_HYPERLINKS = YES USE_PDFLATEX = YES -# The LATEX_BATCHMODE tag ignals the behavior of LaTeX in case of an error. +# The LATEX_BATCHMODE tag signals the behavior of LaTeX in case of an error. # Possible values are: NO same as ERROR_STOP, YES same as BATCH, BATCH In batch # mode nothing is printed on the terminal, errors are scrolled as if is # hit at every error; missing files that TeX tries to input or request from @@ -2263,6 +2270,32 @@ DOCBOOK_OUTPUT = docbook GENERATE_AUTOGEN_DEF = NO +#--------------------------------------------------------------------------- +# Configuration options related to Sqlite3 output +#--------------------------------------------------------------------------- + +# If the GENERATE_SQLITE3 tag is set to YES doxygen will generate a Sqlite3 +# database with symbols found by doxygen stored in tables. +# The default value is: NO. + +GENERATE_SQLITE3 = NO + +# The SQLITE3_OUTPUT tag is used to specify where the Sqlite3 database will be +# put. If a relative path is entered the value of OUTPUT_DIRECTORY will be put +# in front of it. +# The default directory is: sqlite3. +# This tag requires that the tag GENERATE_SQLITE3 is set to YES. + +SQLITE3_OUTPUT = sqlite3 + +# The SQLITE3_OVERWRITE_DB tag is set to YES, the existing doxygen_sqlite3.db +# database file will be recreated with each doxygen run. If set to NO, doxygen +# will warn if an a database file is already found and not modify it. +# The default value is: YES. +# This tag requires that the tag GENERATE_SQLITE3 is set to YES. + +SQLITE3_RECREATE_DB = YES + #--------------------------------------------------------------------------- # Configuration options related to the Perl module output #--------------------------------------------------------------------------- @@ -2405,15 +2438,15 @@ TAGFILES = GENERATE_TAGFILE = -# If the ALLEXTERNALS tag is set to YES, all external class will be listed in -# the class index. If set to NO, only the inherited external classes will be -# listed. +# If the ALLEXTERNALS tag is set to YES, all external classes and namespaces +# will be listed in the class and namespace index. If set to NO, only the +# inherited external classes will be listed. # The default value is: NO. ALLEXTERNALS = NO # If the EXTERNAL_GROUPS tag is set to YES, all external groups will be listed -# in the modules index. If set to NO, only the current project's groups will be +# in the topic index. If set to NO, only the current project's groups will be # listed. # The default value is: YES. @@ -2507,15 +2540,21 @@ CLASS_GRAPH = YES # If the COLLABORATION_GRAPH tag is set to YES then doxygen will generate a # graph for each documented class showing the direct and indirect implementation # dependencies (inheritance, containment, and class references variables) of the -# class with other documented classes. +# class with other documented classes. Explicit enabling a collaboration graph, +# when COLLABORATION_GRAPH is set to NO, can be accomplished by means of the +# command \collaborationgraph. Disabling a collaboration graph can be +# accomplished by means of the command \hidecollaborationgraph. # The default value is: YES. # This tag requires that the tag HAVE_DOT is set to YES. COLLABORATION_GRAPH = YES # If the GROUP_GRAPHS tag is set to YES then doxygen will generate a graph for -# groups, showing the direct groups dependencies. See also the chapter Grouping -# in the manual. +# groups, showing the direct groups dependencies. Explicit enabling a group +# dependency graph, when GROUP_GRAPHS is set to NO, can be accomplished by means +# of the command \groupgraph. Disabling a directory graph can be accomplished by +# means of the command \hidegroupgraph. See also the chapter Grouping in the +# manual. # The default value is: YES. # This tag requires that the tag HAVE_DOT is set to YES. @@ -2575,7 +2614,9 @@ TEMPLATE_RELATIONS = NO # If the INCLUDE_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are set to # YES then doxygen will generate a graph for each documented file showing the # direct and indirect include dependencies of the file with other documented -# files. +# files. Explicit enabling an include graph, when INCLUDE_GRAPH is is set to NO, +# can be accomplished by means of the command \includegraph. Disabling an +# include graph can be accomplished by means of the command \hideincludegraph. # The default value is: YES. # This tag requires that the tag HAVE_DOT is set to YES. @@ -2584,7 +2625,10 @@ INCLUDE_GRAPH = YES # If the INCLUDED_BY_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are # set to YES then doxygen will generate a graph for each documented file showing # the direct and indirect include dependencies of the file with other documented -# files. +# files. Explicit enabling an included by graph, when INCLUDED_BY_GRAPH is set +# to NO, can be accomplished by means of the command \includedbygraph. Disabling +# an included by graph can be accomplished by means of the command +# \hideincludedbygraph. # The default value is: YES. # This tag requires that the tag HAVE_DOT is set to YES. @@ -2624,7 +2668,10 @@ GRAPHICAL_HIERARCHY = YES # If the DIRECTORY_GRAPH tag is set to YES then doxygen will show the # dependencies a directory has on other directories in a graphical way. The # dependency relations are determined by the #include relations between the -# files in the directories. +# files in the directories. Explicit enabling a directory graph, when +# DIRECTORY_GRAPH is set to NO, can be accomplished by means of the command +# \directorygraph. Disabling a directory graph can be accomplished by means of +# the command \hidedirectorygraph. # The default value is: YES. # This tag requires that the tag HAVE_DOT is set to YES. diff --git a/examples/.styleguide b/examples/.styleguide new file mode 100644 index 00000000..c65e7ea6 --- /dev/null +++ b/examples/.styleguide @@ -0,0 +1,30 @@ +cppHeaderFileInclude { + \.h$ + \.hpp$ + \.inc$ +} + +cppSrcFileInclude { + \.cpp$ +} + +modifiableFileExclude { + jni/jni\.h$ + jni/darwin/jni_md\.h$ + jni/linux/jni_md\.h$ + jni/win32/jni_md\.h$ + \.jar$ +} + +licenseUpdateExclude { + java/src/main/java/org/team2363/util/DependencyExtractor\.java$ + java/src/main/java/org/team2363/util/RuntimeDetector\.java$ + java/src/main/java/org/team2363/util/RuntimeLoader\.java$ +} + +includeOtherLibs { + ^Eigen/ + ^casadi/ + ^sleipnir/ + ^trajopt/ +} diff --git a/examples/Swerve/src/Main.cpp b/examples/Swerve/src/Main.cpp index a111d289..750f18fb 100644 --- a/examples/Swerve/src/Main.cpp +++ b/examples/Swerve/src/Main.cpp @@ -5,24 +5,23 @@ #include #include -#include - -#include "IncompatibleTrajectoryException.h" -#include "OptimalTrajectoryGenerator.h" -#include "constraint/Constraint.h" -#include "constraint/TranslationConstraint.h" -#include "constraint/holonomic/HolonomicConstraint.h" -#include "drivetrain/SwerveDrivetrain.h" -#include "obstacle/Obstacle.h" -#include "path/InitialGuessPoint.h" -#include "path/Path.h" -#include "path/SwervePathBuilder.h" -#include "set/ConeSet2d.h" -#include "solution/SwerveSolution.h" -#include "trajectory/HolonomicTrajectory.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include int main() { using namespace trajopt; + SwerveDrivetrain swerveDrivetrain{.mass = 45, .moi = 6, .modules = {{+0.6, +0.6, 0.04, 70, 2}, @@ -42,12 +41,6 @@ int main() { path.ControlIntervalCounts({4}); // SOLVE - try { - SwerveSolution solution = OptimalTrajectoryGenerator::Generate(path); - fmt::print("[{}]\n", fmt::join(path.CalculateInitialGuess().x, ",")); - fmt::print("{}\n", solution); - // fmt::print("{}\n", HolonomicTrajectory(solution)); - } catch (const std::exception& e) { - fmt::print("{}", e.what()); - } + [[maybe_unused]] SwerveSolution solution = + OptimalTrajectoryGenerator::Generate(path); } diff --git a/include/constraint/AngularVelocityConstraint.h b/include/constraint/AngularVelocityConstraint.h deleted file mode 100644 index 7f4c913f..00000000 --- a/include/constraint/AngularVelocityConstraint.h +++ /dev/null @@ -1,59 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include - -#include - -#include "SymbolExports.h" -#include "set/IntervalSet1d.h" -#include "solution/SolutionChecking.h" - -namespace trajopt { - -/** - * Angular velocity constraint. - */ -struct TRAJOPT_DLLEXPORT AngularVelocityConstraint { - /// The angular velocity bounds. - IntervalSet1d angularVelocityBound; - - /** - * Returns an error if the angular velocity is outside the bounds. - * - * @param angularVelocity The angular velocity. - * @param tolerances The tolerances considered to satisfy the constraint. - */ - std::optional CheckAngularVelocity( - double angularVelocity, - const SolutionTolerances& tolerances) const noexcept; -}; - -} // namespace trajopt - -/** - * Formatter for AngularVelocityConstraint. - */ -//! @cond Doxygen_Suppress -template <> -struct fmt::formatter { - //! @endcond - /** - * Format string parser. - * - * @param ctx Format string context. - */ - constexpr auto parse(fmt::format_parse_context& ctx) { return ctx.begin(); } - - /** - * Writes out a formatted AngularVelocityConstraint. - * - * @param constraint AngularVelocityConstraint instance. - * @param ctx Format string context. - */ - auto format(const trajopt::AngularVelocityConstraint& constraint, - fmt::format_context& ctx) const { - return fmt::format_to(ctx.out(), "ω {}", constraint.angularVelocityBound); - } -}; diff --git a/include/constraint/Constraint.h b/include/constraint/Constraint.h deleted file mode 100644 index 42d7073f..00000000 --- a/include/constraint/Constraint.h +++ /dev/null @@ -1,112 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include -#include - -#include - -#include "SymbolExports.h" -#include "constraint/AngularVelocityConstraint.h" -#include "constraint/HeadingConstraint.h" -#include "constraint/LinePointConstraint.h" -#include "constraint/PointLineConstraint.h" -#include "constraint/PointPointConstraint.h" -#include "constraint/TranslationConstraint.h" -#include "solution/SolutionChecking.h" - -namespace trajopt { - -/** - * @brief Enumerates the various coordinate systems used in trajectory - * optimization. The field coordinate system is the base system that is fixed to - * the field with some arbitrary center. Other coordinate systems defined do not - * have a relative velocity to the field coordinate system, but their position - * and orientation depends on the current position of the robot. For example, if - * the robot is spinning, then the robot has angular velocity relative to all - * the systems defined here, and the magnitude of the robot's velocity is the - * same in all the systems but the direction varies. - * - * We define these other systems: the robot coordinate system, the - * robot nonrotating coordinate system, and the robot velocity coordinate - * system. The robot coordinate system is centered on and oriented with the - * front of the robot towards the x-axis. The robot nonrotating coordinate - * system is centered on the robot but is oriented with the field. The robot - * velocity coordinate system is centered on the robot and it is oriented with - * the robot's velocity in the x-direction. - * - * Note that in differential drivetrains, the robot coordinate system - * is equivalent ot the robot velocity coordinate system. - */ -enum class CoordinateSystem { - /** - * @brief the coordinate system of the field - */ - kField, - /** - * @brief the coordinate system of the robot - */ - kRobot, -}; - -using Constraint = - std::variant; - -/** - * Returns an error if the state doesn't satisfy the constraint. - * - * @param x The x coordinate. - * @param y The y coordinate. - * @param heading The heading. - * @param tolerances The tolerances considered to satisfy the constraint. - */ -std::optional CheckState( - const Constraint& constraint, double x, double y, double heading, - const SolutionTolerances& tolerances) noexcept; -} // namespace trajopt - -/** - * Formatter for Constraint. - */ -//! @cond Doxygen_Suppress -template <> -struct fmt::formatter { - //! @endcond - /** - * Format string parser. - * - * @param ctx Format string context. - */ - constexpr auto parse(fmt::format_parse_context& ctx) { return ctx.begin(); } - - /** - * Writes out a formatted Constraint. - * - * @param constraint Constraint instance. - * @param ctx Format string context. - */ - auto format(const trajopt::Constraint& constraint, - fmt::format_context& ctx) const { - using namespace trajopt; - if (std::holds_alternative(constraint)) { - return fmt::format_to(ctx.out(), "constraint: {}", - std::get(constraint)); - } else if (std::holds_alternative(constraint)) { - return fmt::format_to(ctx.out(), "constraint: {}", - std::get(constraint)); - } else if (std::holds_alternative(constraint)) { - return fmt::format_to(ctx.out(), "constraint: {}", - std::get(constraint)); - } else if (std::holds_alternative(constraint)) { - return fmt::format_to(ctx.out(), "constraint: {}", - std::get(constraint)); - } else if (std::holds_alternative(constraint)) { - return fmt::format_to(ctx.out(), "constraint: {}", - std::get(constraint)); - } else { - return ctx.out(); - } - } -}; diff --git a/include/constraint/HeadingConstraint.h b/include/constraint/HeadingConstraint.h deleted file mode 100644 index 14100acd..00000000 --- a/include/constraint/HeadingConstraint.h +++ /dev/null @@ -1,58 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include - -#include - -#include "SymbolExports.h" -#include "set/IntervalSet1d.h" -#include "solution/SolutionChecking.h" - -namespace trajopt { - -/** - * Heading constraint. - */ -struct TRAJOPT_DLLEXPORT HeadingConstraint { - /// The heading bound. - IntervalSet1d headingBound; - - /** - * Returns an error if the given heading isn't in the heading region. - * - * @param theta The heading. - * @param tolerances The tolerances considered to satisfy the constraint. - */ - std::optional CheckHeading( - double theta, const SolutionTolerances& tolerances) const noexcept; -}; - -} // namespace trajopt - -/** - * Formatter for HeadingConstraint. - */ -//! @cond Doxygen_Suppress -template <> -struct fmt::formatter { - //! @endcond - /** - * Format string parser. - * - * @param ctx Format string context. - */ - constexpr auto parse(fmt::format_parse_context& ctx) { return ctx.begin(); } - - /** - * Writes out a formatted HeadingConstraint. - * - * @param constraint HeadingConstraint instance. - * @param ctx Format string context. - */ - auto format(const trajopt::HeadingConstraint& constraint, - fmt::format_context& ctx) const { - return fmt::format_to(ctx.out(), "heading {}", constraint.headingBound); - } -}; diff --git a/include/constraint/LinePointConstraint.h b/include/constraint/LinePointConstraint.h deleted file mode 100644 index 4affa00d..00000000 --- a/include/constraint/LinePointConstraint.h +++ /dev/null @@ -1,55 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include "set/IntervalSet1d.h" - -namespace trajopt { - -/** - * Specifies the required minimum distance between a line segment on the - * robot's frame and a point on the field. - */ -struct LinePointConstraint { - /// robot line start x - double robotLineStartX; - /// robot line start y - double robotLineStartY; - /// robot line end x - double robotLineEndX; - /// robot line end y - double robotLineEndY; - /// field point x - double fieldPointX; - /// field point y - double fieldPointY; - /// the required minimum distance between the line and point, must be positive - IntervalSet1d distance; -}; -} // namespace trajopt - -/** - * Formatter for LinePointConstraint. - */ -//! @cond Doxygen_Suppress -template <> -struct fmt::formatter { - //! @endcond - /** - * Format string parser. - * - * @param ctx Format string context. - */ - constexpr auto parse(fmt::format_parse_context& ctx) { return ctx.begin(); } - - /** - * Writes out a formatted LinePointConstraint. - * - * @param constraint PointLineConstraint instance. - * @param ctx Format string context. - */ - auto format(const trajopt::LinePointConstraint& constraint, - fmt::format_context& ctx) const { - return fmt::format_to(ctx.out(), "line point constraint"); - } -}; diff --git a/include/constraint/PointLineConstraint.h b/include/constraint/PointLineConstraint.h deleted file mode 100644 index 8f9b690b..00000000 --- a/include/constraint/PointLineConstraint.h +++ /dev/null @@ -1,55 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include "set/IntervalSet1d.h" - -namespace trajopt { - -/** - * Specifies the required minimum distance between a point on the robot's - * frame and a line segment on the field. - */ -struct PointLineConstraint { - /// robot point x - double robotPointX; - /// robot point y - double robotPointY; - /// field line start x - double fieldLineStartX; - /// field line start y - double fieldLineStartY; - /// field line end x - double fieldLineEndX; - /// field line end y - double fieldLineEndY; - /// the required minimum distance between the point and line, must be positive - IntervalSet1d distance; -}; -} // namespace trajopt - -/** - * Formatter for PointLineConstraint. - */ -//! @cond Doxygen_Suppress -template <> -struct fmt::formatter { - //! @endcond - /** - * Format string parser. - * - * @param ctx Format string context. - */ - constexpr auto parse(fmt::format_parse_context& ctx) { return ctx.begin(); } - - /** - * Writes out a formatted PointLineConstraint. - * - * @param constraint PointLineConstraint instance. - * @param ctx Format string context. - */ - auto format(const trajopt::PointLineConstraint& constraint, - fmt::format_context& ctx) const { - return fmt::format_to(ctx.out(), "point line constraint"); - } -}; diff --git a/include/constraint/PointPointConstraint.h b/include/constraint/PointPointConstraint.h deleted file mode 100644 index ffa50a17..00000000 --- a/include/constraint/PointPointConstraint.h +++ /dev/null @@ -1,51 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include "set/IntervalSet1d.h" - -namespace trajopt { - -/** - * Specifies the required distance between a point on the robot's frame - * and a point on the field. - */ -struct PointPointConstraint { - /// robot point x - double robotPointX; - /// robot point y - double robotPointY; - /// field point x - double fieldPointX; - /// field point y - double fieldPointY; - /// the required distance between the point and point, must be positive - IntervalSet1d distance; -}; -} // namespace trajopt - -/** - * Formatter for PointPointConstraint. - */ -//! @cond Doxygen_Suppress -template <> -struct fmt::formatter { - //! @endcond - /** - * Format string parser. - * - * @param ctx Format string context. - */ - constexpr auto parse(fmt::format_parse_context& ctx) { return ctx.begin(); } - - /** - * Writes out a formatted PointPointConstraint. - * - * @param constraint PointPointConstraint instance. - * @param ctx Format string context. - */ - auto format(const trajopt::PointPointConstraint& constraint, - fmt::format_context& ctx) const { - return fmt::format_to(ctx.out(), "point point constraint"); - } -}; diff --git a/include/constraint/TranslationConstraint.h b/include/constraint/TranslationConstraint.h deleted file mode 100644 index 0a6bd652..00000000 --- a/include/constraint/TranslationConstraint.h +++ /dev/null @@ -1,59 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include - -#include - -#include "SymbolExports.h" -#include "set/Set2d.h" -#include "solution/SolutionChecking.h" - -namespace trajopt { - -/** - * Translation constraint. - */ -struct TRAJOPT_DLLEXPORT TranslationConstraint { - /// Translation bound. - Set2d translationBound; - - /** - * Returns an error if the given position doesn't satisfy the constraint. - * - * @param x The position's x component. - * @param y The position's y component. - * @param tolerances The tolerances considered to satisfy the constraint. - */ - std::optional CheckTranslation( - double x, double y, const SolutionTolerances& tolerances) const noexcept; -}; -} // namespace trajopt - -/** - * Formatter for TranslationConstraint. - */ -//! @cond Doxygen_Suppress -template <> -struct fmt::formatter { - //! @endcond - /** - * Format string parser. - * - * @param ctx Format string context. - */ - constexpr auto parse(fmt::format_parse_context& ctx) { return ctx.begin(); } - - /** - * Writes out a formatted TranslationConstraint. - * - * @param constraint TranslationConstraint instance. - * @param ctx Format string context. - */ - auto format(const trajopt::TranslationConstraint& constraint, - fmt::format_context& ctx) const { - return fmt::format_to(ctx.out(), "translation {}", - constraint.translationBound); - } -}; diff --git a/include/constraint/differential/DifferentialCentripetalAccelerationConstraint.h b/include/constraint/differential/DifferentialCentripetalAccelerationConstraint.h deleted file mode 100644 index 1b309e0d..00000000 --- a/include/constraint/differential/DifferentialCentripetalAccelerationConstraint.h +++ /dev/null @@ -1,50 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include - -#include - -#include "SymbolExports.h" -#include "constraint/Constraint.h" -#include "set/IntervalSet1d.h" -#include "solution/SolutionChecking.h" - -namespace trajopt { - -/** - * Differential centripetal acceleration constraint - */ -struct TRAJOPT_DLLEXPORT DifferentialCentripetalAccelerationConstraint { - /// Acceleration bound. - IntervalSet1d accelerationBound; -}; -} // namespace trajopt - -/** - * Formatter for DifferentialCentripetalAccelerationConstraint. - */ -//! @cond Doxygen_Suppress -template <> -struct fmt::formatter { - //! @endcond - /** - * Format string parser. - * - * @param ctx Format string context. - */ - constexpr auto parse(fmt::format_parse_context& ctx) { return ctx.begin(); } - - /** - * Writes out a formatted DifferentialCentripetalAccelerationConstraint. - * - * @param constraint DifferentialCentripetalAccelerationConstraint instance. - * @param ctx Format string context. - */ - auto format( - const trajopt::DifferentialCentripetalAccelerationConstraint& constraint, - fmt::format_context& ctx) const { - return fmt::format_to(ctx.out(), "diff centrip acceleration bound"); - } -}; diff --git a/include/constraint/differential/DifferentialConstraint.h b/include/constraint/differential/DifferentialConstraint.h deleted file mode 100644 index 76e61655..00000000 --- a/include/constraint/differential/DifferentialConstraint.h +++ /dev/null @@ -1,65 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include - -#include - -#include "SymbolExports.h" -#include "constraint/AngularVelocityConstraint.h" -#include "constraint/Constraint.h" -#include "constraint/differential/DifferentialCentripetalAccelerationConstraint.h" -#include "constraint/differential/DifferentialTangentialVelocityConstraint.h" -#include "solution/SolutionChecking.h" -#include "util/AppendVariant.h" - -namespace trajopt { - -using DifferentialConstraint = - decltype(_append_variant(Constraint{}, AngularVelocityConstraint{}, - DifferentialTangentialVelocityConstraint{}, - DifferentialCentripetalAccelerationConstraint{})); -} // namespace trajopt - -/** - * Formatter for HolonomicConstraint. - */ -//! @cond Doxygen_Suppress -template <> -struct fmt::formatter { - //! @endcond - /** - * Format string parser. - * - * @param ctx Format string context. - */ - constexpr auto parse(fmt::format_parse_context& ctx) { return ctx.begin(); } - - /** - * Writes out a formatted HolonomicConstraint. - * - * @param constraint HolonomicConstraint instance. - * @param ctx Format string context. - */ - auto format(const trajopt::DifferentialConstraint& constraint, - fmt::format_context& ctx) const { - using namespace trajopt; - if (std::holds_alternative(constraint)) { - return fmt::format_to(ctx.out(), "constraint: {}", - std::get(constraint)); - } else if (std::holds_alternative( - constraint)) { - return fmt::format_to( - ctx.out(), "constraint: {}", - std::get(constraint)); - } else if (std::holds_alternative< - DifferentialCentripetalAccelerationConstraint>(constraint)) { - return fmt::format_to( - ctx.out(), "constraint: {}", - std::get(constraint)); - } else { - return ctx.out(); - } - } -}; diff --git a/include/constraint/differential/DifferentialTangentialVelocityConstraint.h b/include/constraint/differential/DifferentialTangentialVelocityConstraint.h deleted file mode 100644 index 44ca6bca..00000000 --- a/include/constraint/differential/DifferentialTangentialVelocityConstraint.h +++ /dev/null @@ -1,51 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include - -#include - -#include "SymbolExports.h" -#include "constraint/Constraint.h" -#include "set/IntervalSet1d.h" -#include "solution/SolutionChecking.h" - -namespace trajopt { - -/** - * Differential Tangential Velocity constraint. - */ -struct TRAJOPT_DLLEXPORT DifferentialTangentialVelocityConstraint { - /// Velocity bound. - IntervalSet1d velocityBound; -}; -} // namespace trajopt - -/** - * Formatter for VelocityConstraint. - */ -//! @cond Doxygen_Suppress -template <> -struct fmt::formatter { - //! @endcond - /** - * Format string parser. - * - * @param ctx Format string context. - */ - constexpr auto parse(fmt::format_parse_context& ctx) { return ctx.begin(); } - - /** - * Writes out a formatted VelocityConstraint. - * - * @param constraint VelocityConstraint instance. - * @param ctx Format string context. - */ - auto format( - const trajopt::DifferentialTangentialVelocityConstraint& constraint, - fmt::format_context& ctx) const { - return fmt::format_to(ctx.out(), "diff velocity magnitude {}", - constraint.velocityBound); - } -}; diff --git a/include/constraint/holonomic/HolonomicConstraint.h b/include/constraint/holonomic/HolonomicConstraint.h deleted file mode 100644 index 8ab7149e..00000000 --- a/include/constraint/holonomic/HolonomicConstraint.h +++ /dev/null @@ -1,76 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include - -#include - -#include "SymbolExports.h" -#include "constraint/AngularVelocityConstraint.h" -#include "constraint/holonomic/HolonomicVelocityConstraint.h" -#include "solution/SolutionChecking.h" -#include "util/AppendVariant.h" - -namespace trajopt { - -using HolonomicConstraint = decltype(_append_variant( - Constraint{}, AngularVelocityConstraint{}, HolonomicVelocityConstraint{})); - -/** - * Returns an error if the given state doesn't satisfy the constraint. - * - * @param x The x coordinate. - * @param y The y coordinate. - * @param heading The heading. - * @param velocityX The velocity's x component. - * @param velocityY The velocity's y component. - * @param angularVelocity The angular velocity. - * @param accelerationX The acceleration's x component. - * @param accelerationY The acceleration's y component. - * @param angularAcceleration The angular acceleration. - * @param tolerances The tolerances considered to satisfy the constraint. - */ -std::optional CheckState( - const HolonomicConstraint& constraint, double x, double y, double heading, - double velocityX, double velocityY, double angularVelocity, - double accelerationX, double accelerationY, double angularAcceleration, - const SolutionTolerances& tolerances) noexcept; - -} // namespace trajopt - -/** - * Formatter for HolonomicConstraint. - */ -//! @cond Doxygen_Suppress -template <> -struct fmt::formatter { - //! @endcond - /** - * Format string parser. - * - * @param ctx Format string context. - */ - constexpr auto parse(fmt::format_parse_context& ctx) { return ctx.begin(); } - - /** - * Writes out a formatted HolonomicConstraint. - * - * @param constraint HolonomicConstraint instance. - * @param ctx Format string context. - */ - auto format(const trajopt::HolonomicConstraint& constraint, - fmt::format_context& ctx) const { - using namespace trajopt; - if (std::holds_alternative(constraint)) { - return fmt::format_to(ctx.out(), "constraint: {}", - std::get(constraint)); - } else if (std::holds_alternative( - constraint)) { - return fmt::format_to(ctx.out(), "constraint: {}", - std::get(constraint)); - } else { - return ctx.out(); - } - } -}; diff --git a/include/constraint/holonomic/HolonomicVelocityConstraint.h b/include/constraint/holonomic/HolonomicVelocityConstraint.h deleted file mode 100644 index 23230738..00000000 --- a/include/constraint/holonomic/HolonomicVelocityConstraint.h +++ /dev/null @@ -1,63 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include - -#include - -#include "SymbolExports.h" -#include "constraint/Constraint.h" -#include "set/Set2d.h" -#include "solution/SolutionChecking.h" - -namespace trajopt { - -/** - * Velocity constraint. - */ -struct TRAJOPT_DLLEXPORT HolonomicVelocityConstraint { - /// Velocity bound. - Set2d velocityBound; - - /// Coordinate system. - CoordinateSystem coordinateSystem; - - /** - * Returns an error if the given velocity doesn't satisfy the constraint. - * - * @param velocityX The velocity's x component. - * @param velocityY The velocity's y component. - * @param tolerances The tolerances considered to satisfy the constraint. - */ - std::optional CheckVelocity( - double velocityX, double velocityY, - const SolutionTolerances& tolerances) const noexcept; -}; -} // namespace trajopt - -/** - * Formatter for VelocityConstraint. - */ -//! @cond Doxygen_Suppress -template <> -struct fmt::formatter { - //! @endcond - /** - * Format string parser. - * - * @param ctx Format string context. - */ - constexpr auto parse(fmt::format_parse_context& ctx) { return ctx.begin(); } - - /** - * Writes out a formatted VelocityConstraint. - * - * @param constraint VelocityConstraint instance. - * @param ctx Format string context. - */ - auto format(const trajopt::HolonomicVelocityConstraint& constraint, - fmt::format_context& ctx) const { - return fmt::format_to(ctx.out(), "velocity {}", constraint.velocityBound); - } -}; diff --git a/include/drivetrain/SwerveDrivetrain.h b/include/drivetrain/SwerveDrivetrain.h deleted file mode 100644 index 550374e0..00000000 --- a/include/drivetrain/SwerveDrivetrain.h +++ /dev/null @@ -1,63 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include - -#include - -#include "SymbolExports.h" -#include "drivetrain/SwerveModule.h" -#include "obstacle/Obstacle.h" -#include "trajectory/HolonomicTrajectory.h" - -namespace trajopt { - -/** - * @brief This class represents a swerve drivetrain robot. It includes the - * physical properties necessary to accurately model the dynamics of the system. - * An arbitrary number of swerve modules can be specified, but typically it will - * be four. The order the swerve modules are listed does not matter. - */ -struct TRAJOPT_DLLEXPORT SwerveDrivetrain { - /// the mass of the robot - double mass; - /// the moment of inertial of the robot about the origin - double moi; - /// The list of swerve modules that make the robot move, usually one in each - /// corner. - std::vector modules; -}; - -} // namespace trajopt - -/** - * Formatter for SwerveDrivetrain. - */ -//! @cond Doxygen_Suppress -template <> -struct fmt::formatter { - //! @endcond - /** - * Format string parser. - * - * @param ctx Format string context. - */ - constexpr auto parse(fmt::format_parse_context& ctx) { return ctx.begin(); } - - /** - * Writes out a formatted SwerveDrivetrain. - * - * @param swerveDrivetrain SwerveDrivetrain instance. - * @param ctx Format string context. - */ - auto format(const trajopt::SwerveDrivetrain& swerveDrivetrain, - fmt::format_context& ctx) const { - return fmt::format_to(ctx.out(), - "swerve drivetrain:\n" - " mass = {},\n" - " moi = {},\n" - " modules = (no impl yet)", - swerveDrivetrain.mass, swerveDrivetrain.moi); - } -}; diff --git a/include/drivetrain/SwerveModule.h b/include/drivetrain/SwerveModule.h deleted file mode 100644 index 27aefe10..00000000 --- a/include/drivetrain/SwerveModule.h +++ /dev/null @@ -1,71 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include - -#include "SymbolExports.h" -#include "constraint/Constraint.h" - -namespace trajopt { - -/** - * @brief This class represents a single swerve module in a swerve drivetrain. - * It is defined by the module diagonal, which is the line connecting the origin - * of the robot coordinate system to the center of the module. The wheel radius, - * max speed, and max torque must also be specified per module. - */ -struct TRAJOPT_DLLEXPORT SwerveModule { - /** - * @brief x-coordinate of swerve module relative to robot coordinate system - */ - double x; - /** - * @brief y-coordinate of swerve module relative to robot coordinate system - */ - double y; - /** - * @brief radius of wheel - */ - double wheelRadius; - /** - * @brief maximum angular velocity of wheel - */ - double wheelMaxAngularVelocity; - /** - * @brief maximum torque applied to wheel - */ - double wheelMaxTorque; -}; - -} // namespace trajopt - -/** - * Formatter for SwerveModule. - */ -//! @cond Doxygen_Suppress -template <> -struct fmt::formatter { - //! @endcond - /** - * Format string parser. - * - * @param ctx Format string context. - */ - constexpr auto parse(fmt::format_parse_context& ctx) { return ctx.begin(); } - - /** - * Writes out a formatted SwerveModule. - * - * @param swerveModule SwerveModule instance. - * @param ctx Format string context. - */ - auto format(const trajopt::SwerveModule& swerveModule, - fmt::format_context& ctx) const { - return fmt::format_to( - ctx.out(), - "swerve module: (x, y) = ({}, {}), r = {}, ωₘₐₓ = {}, τₘₐₓ = {}", - swerveModule.x, swerveModule.y, swerveModule.wheelRadius, - swerveModule.wheelMaxAngularVelocity, swerveModule.wheelMaxTorque); - } -}; diff --git a/include/path/InitialGuessPoint.h b/include/path/InitialGuessPoint.h deleted file mode 100644 index 21f39b71..00000000 --- a/include/path/InitialGuessPoint.h +++ /dev/null @@ -1,52 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include - -#include "SymbolExports.h" - -namespace trajopt { - -/** - * @brief An initial guess of a possible state the robot may be in during the - * trajectory. - */ -struct TRAJOPT_DLLEXPORT InitialGuessPoint { - /// The initial guess of the x-coordinate of the robot. - double x; - - /// The initial guess of the y-coordinate of the robot. - double y; - - /// The initial guess of the heading of the robot. - double heading; -}; - -} // namespace trajopt - -/** - * Formatter for InitialGuessPoint. - */ -//! @cond Doxygen_Suppress -template <> -struct fmt::formatter { - //! @endcond - /** - * Format string parser. - * - * @param ctx Format string context. - */ - constexpr auto parse(fmt::format_parse_context& ctx) { return ctx.begin(); } - - /** - * Writes out a formatted InitialGuessPoint. - * - * @param initialGuessPoint InitialGuessPoint instance. - * @param ctx Format string context. - */ - auto format(const trajopt::InitialGuessPoint& initialGuessPoint, - fmt::format_context& ctx) const { - return fmt::format_to(ctx.out(), "Initial Guess Point"); - } -}; diff --git a/include/set/ConeSet2d.h b/include/set/ConeSet2d.h deleted file mode 100644 index 251c3311..00000000 --- a/include/set/ConeSet2d.h +++ /dev/null @@ -1,62 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include - -#include "SymbolExports.h" -#include "set/IntervalSet1d.h" -#include "solution/SolutionChecking.h" - -namespace trajopt { - -/** - * Conical 2D set. - */ -struct TRAJOPT_DLLEXPORT ConeSet2d { - /// The heading bounds of the cone. - IntervalSet1d thetaBound; - - /** - * Returns an error if the given coordinate is outside the cone. - * - * @param xComp The x coordinate. - * @param yComp The y coordinate. - * @param tolerances The tolerances considered to satisfy the constraint. - */ - std::optional CheckVector( - double xComp, double yComp, - const SolutionTolerances& tolerances) const noexcept; - - /** - * Returns true if the set is valid. - */ - bool IsValid() const noexcept; -}; -} // namespace trajopt - -/** - * Formatter for ConeSet2d. - */ -//! @cond Doxygen_Suppress -template <> -struct fmt::formatter { - //! @endcond - /** - * Format string parser. - * - * @param ctx Format string context. - */ - constexpr auto parse(fmt::format_parse_context& ctx) { return ctx.begin(); } - - /** - * Writes out a formatted ConeSet2d. - * - * @param coneSet ConeSet2d instance. - * @param ctx Format string context. - */ - auto format(const trajopt::ConeSet2d& coneSet, - fmt::format_context& ctx) const { - return fmt::format_to(ctx.out(), "cone: θ = {}", coneSet.thetaBound); - } -}; diff --git a/include/set/EllipticalSet2d.h b/include/set/EllipticalSet2d.h deleted file mode 100644 index 221308ea..00000000 --- a/include/set/EllipticalSet2d.h +++ /dev/null @@ -1,123 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include -#include - -#include - -#include "SymbolExports.h" -#include "solution/SolutionChecking.h" - -namespace trajopt { - -/** - * Elliptical 2D set. - */ -struct TRAJOPT_DLLEXPORT EllipticalSet2d { - /** - * FIXME What does this do? - */ - enum class Direction { - /// FIXME What does this do? - kInside, - /// FIXME What does this do? - kCentered, - /// FIXME What does this do? - kOutside - }; - - /// The x radius. - double xRadius; - - /// The y radius. - double yRadius; - - /// The direction. - Direction direction; - - /** - * Construct a circular EllipticalSet2d from a radius. - * - * @param radius The radius. - * @param direction The direction. - */ - static EllipticalSet2d CircularSet2d( - double radius, Direction direction = Direction::kInside); - - /** - * Returns true if the ellipse is a circle. - */ - bool IsCircular() const noexcept; - - /** - * Returns true if the set spans R². - */ - bool IsR2() const noexcept; - - /** - * Returns an error if the given coordinate is outside the ellipse. - * - * @param xComp The x coordinate. - * @param yComp The y coordinate. - * @param tolerances The tolerances considered to satisfy the constraint. - */ - std::optional CheckVector( - double xComp, double yComp, - const SolutionTolerances& tolerances) const noexcept; - - /** - * Returns true if the set is valid. - */ - bool IsValid() const noexcept; -}; - -} // namespace trajopt - -/** - * Formatter for EllipticalSet2d. - */ -//! @cond Doxygen_Suppress -template <> -struct fmt::formatter { - //! @endcond - /** - * Format string parser. - * - * @param ctx Format string context. - */ - constexpr auto parse(fmt::format_parse_context& ctx) { return ctx.begin(); } - - /** - * Writes out a formatted EllipticalSet2d. - * - * @param ellipticalSet EllipticalSet2d instance. - * @param ctx Format string context. - */ - auto format(const trajopt::EllipticalSet2d& ellipticalSet, - fmt::format_context& ctx) const { - std::string shape; - if (ellipticalSet.IsCircular()) { - shape = "circle"; - } else { - shape = "ellipse"; - } - using enum trajopt::EllipticalSet2d::Direction; - std::string direction; - switch (ellipticalSet.direction) { - case kInside: - direction = "inside"; - break; - case kCentered: - direction = "centered"; - break; - case kOutside: - direction = "outside"; - break; - } - return fmt::format_to(ctx.out(), "{}: {}, rₓ = {}, rᵧ = {}", shape, - direction, ellipticalSet.xRadius, - ellipticalSet.yRadius); - } -}; diff --git a/include/set/LinearSet2d.h b/include/set/LinearSet2d.h deleted file mode 100644 index 233f867f..00000000 --- a/include/set/LinearSet2d.h +++ /dev/null @@ -1,67 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include - -#include "SymbolExports.h" -#include "set/IntervalSet1d.h" -#include "set/RectangularSet2d.h" -#include "solution/SolutionChecking.h" - -namespace trajopt { - -/** - * Linear 2D set. - */ -struct TRAJOPT_DLLEXPORT LinearSet2d { - /// FIXME What does this do? - double theta; - - /** - * FIXME What does this do? - * - * @param xComp The x coordinate. - * @param yComp The y coordinate. - * @param tolerances The tolerances considered to satisfy the constraint. - */ - std::optional CheckVector( - double xComp, double yComp, - const SolutionTolerances& tolerances) const noexcept; - - /** - * FIXME What does this do? - * - * @param theta FIXME What does this do? - * @param rBound FIXME What does this do? - */ - static RectangularSet2d RBoundToRectangular(double theta, - const IntervalSet1d& rBound); -}; -} // namespace trajopt - -/** - * Formatter for LinearSet2d. - */ -//! @cond Doxygen_Suppress -template <> -struct fmt::formatter { - //! @endcond - /** - * Format string parser. - * - * @param ctx Format string context. - */ - constexpr auto parse(fmt::format_parse_context& ctx) { return ctx.begin(); } - - /** - * Writes out a formatted LinearSet2d. - * - * @param linearSet LinearSet2d instance. - * @param ctx Format string context. - */ - auto format(const trajopt::LinearSet2d& linearSet, - fmt::format_context& ctx) const { - return fmt::format_to(ctx.out(), "polar line: θ = {}", linearSet.theta); - } -}; diff --git a/include/set/RectangularSet2d.h b/include/set/RectangularSet2d.h deleted file mode 100644 index e3a38854..00000000 --- a/include/set/RectangularSet2d.h +++ /dev/null @@ -1,84 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include - -#include "SymbolExports.h" -#include "set/IntervalSet1d.h" -#include "solution/SolutionChecking.h" - -namespace trajopt { - -/** - * Rectangular 2D set. - */ -struct TRAJOPT_DLLEXPORT RectangularSet2d { - /// The x boundary. - IntervalSet1d xBound; - - /// The y boundary. - IntervalSet1d yBound; - - /** - * Construct a RectangularSet2d from polar coordinates. - * - * @param r The distance. - * @param theta The heading. - */ - static RectangularSet2d PolarExactSet2d(double r, double theta); - - /** - * Construct a RectangularSet2d spanning R². - */ - static RectangularSet2d R2(); - - /** - * Returns an error if the given vector isn't in the region. - * - * @param xComp The x coordinate. - * @param yComp The y coordinate. - * @param tolerances The tolerances considered to satisfy the constraint. - */ - std::optional CheckVector( - double xComp, double yComp, - const SolutionTolerances& tolerances) const noexcept; - - /** - * @brief Check if this planar bound is valid. A planar bound is valid when - * the bounds on a0 and a1 are valid, and additionally for planar bounds, a0 - * is contained within the interval [0, inf] and a1 is contained within the - * interval [-pi, pi]. - * - * @return true if and only if this planar bound is valid - */ - bool IsValid() const noexcept; -}; -} // namespace trajopt - -/** - * Formatter for RectangularSet2d. - */ -//! @cond Doxygen_Suppress -template <> -struct fmt::formatter { - //! @endcond - /** - * Format string parser. - * - * @param ctx Format string context. - */ - constexpr auto parse(fmt::format_parse_context& ctx) { return ctx.begin(); } - - /** - * Writes out a formatted RectangularSet2d. - * - * @param rectangularSet RectangularSet2d instance. - * @param ctx Format string context. - */ - auto format(const trajopt::RectangularSet2d& rectangularSet, - fmt::format_context& ctx) const { - return fmt::format_to(ctx.out(), "x {}, y {}", rectangularSet.xBound, - rectangularSet.yBound); - } -}; diff --git a/include/set/Set2d.h b/include/set/Set2d.h deleted file mode 100644 index 16e00a15..00000000 --- a/include/set/Set2d.h +++ /dev/null @@ -1,81 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include -#include - -#include "SymbolExports.h" -#include "set/ConeSet2d.h" -#include "set/EllipticalSet2d.h" -#include "set/LinearSet2d.h" -#include "set/RectangularSet2d.h" -#include "solution/SolutionChecking.h" - -namespace trajopt { - -/** - * @brief This class represents a bounded region of abstract 2D space. The - * bounds are specified with inequalities for each coordinate, a0 and a1. The - * interpretation of the abstract coordinates a0 and a1 changes when - * coordinateType changes. If coordinateType is kRectangular, then a0 and a1 - * represent the rectangular x-coordinate and y-coordinate bounds, respectively. - * If coordinateType is kPolar, then a0 and a1 represent the polar r-coordinate - * and theta-coordinate bounds, respectively. Polar theta coordinates must be - * specified between -pi and pi, inclusive, and all lower bounds must be less - * than or equal to their respective upper bounds. - * - * The following are some common use cases: - * A straight line theta = theta_0 bound can be created as a polar bound - * bounding theta within [theta_0, theta_0], and a straight line x = x_0 or y = - * y_0 bound can be created by bounding x within [x_0, x_0] or y within [y_0, - * y_0]. Bounding the magnitude of a vector can be achieved by bounding r within - * [r_lower, r_upper]. - * - * Bounding theta within [-pi, -pi] or within [pi, pi] is equivalent; both will - * force the vector to be colinear with the x-axis. - */ -using Set2d = - std::variant; - -std::optional CheckVector(const Set2d& set2d, double xComp, - double yComp, - const SolutionTolerances& tolerances); - -} // namespace trajopt - -/** - * Formatter for Set2d. - */ -//! @cond Doxygen_Suppress -template <> -struct fmt::formatter { - //! @endcond - /** - * Format string parser. - * - * @param ctx Format string context. - */ - constexpr auto parse(fmt::format_parse_context& ctx) { return ctx.begin(); } - - /** - * Writes out a formatted Set2d. - * - * @param set2d Set2d instance. - * @param ctx Format string context. - */ - auto format(const trajopt::Set2d& set2d, fmt::format_context& ctx) const { - using namespace trajopt; - if (std::holds_alternative(set2d)) { - return fmt::format_to(ctx.out(), "2d {}", - std::get(set2d)); - } else if (std::holds_alternative(set2d)) { - return fmt::format_to(ctx.out(), "2d {}", std::get(set2d)); - } else if (std::holds_alternative(set2d)) { - return fmt::format_to(ctx.out(), "2d {}", - std::get(set2d)); - } else /*if (set2d.IsCone())*/ { - return fmt::format_to(ctx.out(), "2d {}", std::get(set2d)); - } - } -}; diff --git a/include/solution/SolutionChecking.h b/include/solution/SolutionChecking.h deleted file mode 100644 index 83a7ecb5..00000000 --- a/include/solution/SolutionChecking.h +++ /dev/null @@ -1,24 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include -#include - -#include "SymbolExports.h" - -/** - * Solution error. - */ -struct TRAJOPT_DLLEXPORT SolutionError { - /// The error message. - std::string errorMessage; -}; - -/** - * Solution tolerances. - */ -struct TRAJOPT_DLLEXPORT SolutionTolerances { - /// The error margin. - double errorMargin; -}; diff --git a/include/solution/SwerveSolution.h b/include/solution/SwerveSolution.h deleted file mode 100644 index 0d2a8820..00000000 --- a/include/solution/SwerveSolution.h +++ /dev/null @@ -1,93 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include -#include -#include - -#include - -#include "SymbolExports.h" -#include "solution/HolonomicSolution.h" - -namespace trajopt { - -/** - * The swerve drive trajectory optimization solution. - */ -struct TRAJOPT_DLLEXPORT SwerveSolution : HolonomicSolution { - /// The x forces for each module. - std::vector> moduleFX; - - /// The y forces for each module. - std::vector> moduleFY; -}; - -} // namespace trajopt - -/** - * Formatter for SwerveSolution. - */ -//! @cond Doxygen_Suppress -template <> -struct fmt::formatter { - //! @endcond - /** - * Format string parser. - * - * @param ctx Format string context. - */ - constexpr auto parse(fmt::format_parse_context& ctx) { return ctx.begin(); } - - /** - * Writes out a formatted SwerveSolution. - * - * @param swerveSolution SwerveSolution instance. - * @param ctx Format string context. - */ - auto format(const trajopt::SwerveSolution& swerveSolution, - fmt::format_context& ctx) const { - std::string tableEntries; - for (size_t index = 1; index < swerveSolution.x.size(); index++) { - tableEntries += fmt::format( - " {2:>{0}.{1}f} ║ {3:>{0}.{1}f} ║ {4:>{0}.{1}f} ║ {5:>{0}.{1}f} \n", - 12, 6, swerveSolution.dt[index - 1], swerveSolution.x[index], - swerveSolution.y[index], swerveSolution.theta[index]); - } - return fmt::format_to( - ctx.out(), - " {2:<{1}} ║ {3:<{1}} ║ {4:<{1}} ║ {5:<{1}} \n" - "══{0:═^{1}}═╬═{0:═^{1}}═╬═{0:═^{1}}═╬═{0:═^{1}}══\n" - " {0:>{1}} ║ {12:>{1}.6f} ║ {13:>{1}.6f} ║ {14:>{1}.6f} \n" - "{21}", - "", // 0 - 12, // 1 - "dt", // 2 - "x", // 3 - "y", // 4 - "θ", // 5 - "vₓ", // 6 - "vᵧ", // 7 - "ω", // 8 - "aₓ", // 9 - "aᵧ", // 10 - "α", // 11 - swerveSolution.x[0], // 12 - swerveSolution.y[0], // 13 - swerveSolution.theta[0], // 14 - swerveSolution.vx[0], // 15 - swerveSolution.vy[0], // 16 - swerveSolution.omega[0], // 17 - swerveSolution.ax[0], // 18 - swerveSolution.ay[0], // 19 - swerveSolution.alpha[0], // 20 - tableEntries); - } -}; -/* -╔ ╦ ╗ -║ ║ ║ -╠ ╬ ╣ -╚ ╩ ╝ -*/ diff --git a/include/trajectory/HolonomicTrajectory.h b/include/trajectory/HolonomicTrajectory.h deleted file mode 100644 index f7a62b72..00000000 --- a/include/trajectory/HolonomicTrajectory.h +++ /dev/null @@ -1,69 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include -#include - -#include - -#include "SymbolExports.h" -#include "solution/HolonomicSolution.h" -#include "trajectory/HolonomicTrajectorySample.h" - -namespace trajopt { - -/** - * Holonomic trajectory. - */ -class TRAJOPT_DLLEXPORT HolonomicTrajectory { - public: - /// Trajectory samples. - std::vector samples; - - /** - * Construct a HolonomicTrajectory from samples. - * - * @param samples The samples. - */ - explicit HolonomicTrajectory(std::vector samples); - - /** - * Construct a HolonomicTrajectory from a solution. - * - * @param solution The solution. - */ - explicit HolonomicTrajectory(const HolonomicSolution& solution); -}; - -} // namespace trajopt - -/** - * Formatter for HolonomicTrajectory. - */ -//! @cond Doxygen_Suppress -template <> -struct fmt::formatter { - //! @endcond - /** - * Format string parser. - * - * @param ctx Format string context. - */ - constexpr auto parse(fmt::format_parse_context& ctx) { return ctx.begin(); } - - /** - * Writes out a formatted HolonomicTrajectory. - * - * @param trajectory HolonomicTrajectory instance. - * @param ctx Format string context. - */ - auto format(const trajopt::HolonomicTrajectory& trajectory, - fmt::format_context& ctx) const { - std::string sampsStr = fmt::format("{}", trajectory.samples[0]); - for (size_t i = 1; i < trajectory.samples.size(); i++) { - sampsStr += fmt::format(", {}", trajectory.samples[i]); - } - return fmt::format_to(ctx.out(), "[{}]", sampsStr); - } -}; diff --git a/include/trajectory/HolonomicTrajectorySample.h b/include/trajectory/HolonomicTrajectorySample.h deleted file mode 100644 index 3e8a5195..00000000 --- a/include/trajectory/HolonomicTrajectorySample.h +++ /dev/null @@ -1,84 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include - -#include "SymbolExports.h" - -namespace trajopt { - -/** - * Holonomic trajectory sample. - */ -class TRAJOPT_DLLEXPORT HolonomicTrajectorySample { - public: - /// The timestamp. - double timestamp; - - /// The x coordinate. - double x; - - /// The y coordinate. - double y; - - /// The heading. - double heading; - - /// The velocity's x component. - double velocityX; - - /// The velocity's y component. - double velocityY; - - /// The angular velocity. - double angularVelocity; - - /** - * Construct a HolonomicTrajectorySample. - * - * @param timestamp The timestamp. - * @param x The x coordinate. - * @param y The y coordinate. - * @param heading The heading. - * @param velocityX The velocity's x component. - * @param velocityY The velocity's y component. - * @param angularVelocity The angular velocity. - */ - HolonomicTrajectorySample(double timestamp, double x, double y, - double heading, double velocityX, double velocityY, - double angularVelocity); -}; - -} // namespace trajopt - -/** - * Formatter for HolonomicTrajectorySample. - */ -//! @cond Doxygen_Suppress -template <> -struct fmt::formatter { - //! @endcond - /** - * Format string parser. - * - * @param ctx Format string context. - */ - constexpr auto parse(fmt::format_parse_context& ctx) { return ctx.begin(); } - - /** - * Writes out a formatted HolonomicTrajectorySample. - * - * @param sample HolonomicTrajectorySample instance. - * @param ctx Format string context. - */ - auto format(const trajopt::HolonomicTrajectorySample& sample, - fmt::format_context& ctx) const { - return fmt::format_to( - ctx.out(), - "{{\"timestamp\": {}, \"x\": {}, \"y\": {}, \"heading\": {}, " - "\"velocityX\": {}, \"velocityY\": {}, \"angularVelocity\": {}}}", - sample.timestamp, sample.x, sample.y, sample.heading, sample.velocityX, - sample.velocityY, sample.angularVelocity); - } -}; diff --git a/include/IncompatibleTrajectoryException.h b/include/trajopt/IncompatibleTrajectoryException.h similarity index 95% rename from include/IncompatibleTrajectoryException.h rename to include/trajopt/IncompatibleTrajectoryException.h index 3e36f8da..9e439aef 100644 --- a/include/IncompatibleTrajectoryException.h +++ b/include/trajopt/IncompatibleTrajectoryException.h @@ -5,7 +5,7 @@ #include #include -#include "SymbolExports.h" +#include "trajopt/SymbolExports.h" namespace trajopt { diff --git a/include/InvalidPathException.h b/include/trajopt/InvalidPathException.h similarity index 95% rename from include/InvalidPathException.h rename to include/trajopt/InvalidPathException.h index 10f6691b..f1e8af46 100644 --- a/include/InvalidPathException.h +++ b/include/trajopt/InvalidPathException.h @@ -5,7 +5,7 @@ #include #include -#include "SymbolExports.h" +#include "trajopt/SymbolExports.h" namespace trajopt { diff --git a/include/OptimalTrajectoryGenerator.h b/include/trajopt/OptimalTrajectoryGenerator.h similarity index 82% rename from include/OptimalTrajectoryGenerator.h rename to include/trajopt/OptimalTrajectoryGenerator.h index 17a35b21..4df79445 100644 --- a/include/OptimalTrajectoryGenerator.h +++ b/include/trajopt/OptimalTrajectoryGenerator.h @@ -2,10 +2,10 @@ #pragma once -#include "SymbolExports.h" -#include "path/Path.h" -#include "path/SwervePathBuilder.h" -#include "solution/SwerveSolution.h" +#include "trajopt/SymbolExports.h" +#include "trajopt/path/Path.h" +#include "trajopt/path/SwervePathBuilder.h" +#include "trajopt/solution/SwerveSolution.h" namespace trajopt { diff --git a/include/SymbolExports.h b/include/trajopt/SymbolExports.h similarity index 100% rename from include/SymbolExports.h rename to include/trajopt/SymbolExports.h diff --git a/include/TrajectoryGenerationException.h b/include/trajopt/TrajectoryGenerationException.h similarity index 84% rename from include/TrajectoryGenerationException.h rename to include/trajopt/TrajectoryGenerationException.h index 516d7576..6b48d6f8 100644 --- a/include/TrajectoryGenerationException.h +++ b/include/trajopt/TrajectoryGenerationException.h @@ -3,9 +3,9 @@ #pragma once #include -#include +#include -#include "SymbolExports.h" +#include "trajopt/SymbolExports.h" namespace trajopt { @@ -22,7 +22,8 @@ class TRAJOPT_DLLEXPORT TrajectoryGenerationException * * @param message the string message */ - explicit TrajectoryGenerationException(const std::string& message); + explicit TrajectoryGenerationException(std::string_view message); + /** * @brief Construct a new Trajectory Generation Exception object with a string * message. diff --git a/include/trajopt/constraint/AngularVelocityConstraint.h b/include/trajopt/constraint/AngularVelocityConstraint.h new file mode 100644 index 00000000..921017e2 --- /dev/null +++ b/include/trajopt/constraint/AngularVelocityConstraint.h @@ -0,0 +1,18 @@ +// Copyright (c) TrajoptLib contributors + +#pragma once + +#include "trajopt/SymbolExports.h" +#include "trajopt/set/IntervalSet1d.h" + +namespace trajopt { + +/** + * Angular velocity constraint. + */ +struct TRAJOPT_DLLEXPORT AngularVelocityConstraint { + /// The angular velocity bounds. + IntervalSet1d angularVelocityBound; +}; + +} // namespace trajopt diff --git a/include/trajopt/constraint/Constraint.h b/include/trajopt/constraint/Constraint.h new file mode 100644 index 00000000..60c73837 --- /dev/null +++ b/include/trajopt/constraint/Constraint.h @@ -0,0 +1,51 @@ +// Copyright (c) TrajoptLib contributors + +#pragma once + +#include + +#include "trajopt/constraint/HeadingConstraint.h" +#include "trajopt/constraint/LinePointConstraint.h" +#include "trajopt/constraint/PointLineConstraint.h" +#include "trajopt/constraint/PointPointConstraint.h" +#include "trajopt/constraint/TranslationConstraint.h" + +namespace trajopt { + +/** + * @brief Enumerates the various coordinate systems used in trajectory + * optimization. The field coordinate system is the base system that is fixed to + * the field with some arbitrary center. Other coordinate systems defined do not + * have a relative velocity to the field coordinate system, but their position + * and orientation depends on the current position of the robot. For example, if + * the robot is spinning, then the robot has angular velocity relative to all + * the systems defined here, and the magnitude of the robot's velocity is the + * same in all the systems but the direction varies. + * + * We define these other systems: the robot coordinate system, the + * robot nonrotating coordinate system, and the robot velocity coordinate + * system. The robot coordinate system is centered on and oriented with the + * front of the robot towards the x-axis. The robot nonrotating coordinate + * system is centered on the robot but is oriented with the field. The robot + * velocity coordinate system is centered on the robot and it is oriented with + * the robot's velocity in the x-direction. + * + * Note that in differential drivetrains, the robot coordinate system + * is equivalent ot the robot velocity coordinate system. + */ +enum class CoordinateSystem { + /** + * @brief the coordinate system of the field + */ + kField, + /** + * @brief the coordinate system of the robot + */ + kRobot, +}; + +using Constraint = + std::variant; + +} // namespace trajopt diff --git a/include/trajopt/constraint/HeadingConstraint.h b/include/trajopt/constraint/HeadingConstraint.h new file mode 100644 index 00000000..3c5aa589 --- /dev/null +++ b/include/trajopt/constraint/HeadingConstraint.h @@ -0,0 +1,18 @@ +// Copyright (c) TrajoptLib contributors + +#pragma once + +#include "trajopt/SymbolExports.h" +#include "trajopt/set/IntervalSet1d.h" + +namespace trajopt { + +/** + * Heading constraint. + */ +struct TRAJOPT_DLLEXPORT HeadingConstraint { + /// The heading bound. + IntervalSet1d headingBound; +}; + +} // namespace trajopt diff --git a/include/trajopt/constraint/LinePointConstraint.h b/include/trajopt/constraint/LinePointConstraint.h new file mode 100644 index 00000000..08e91fbf --- /dev/null +++ b/include/trajopt/constraint/LinePointConstraint.h @@ -0,0 +1,30 @@ +// Copyright (c) TrajoptLib contributors + +#pragma once + +#include "trajopt/set/IntervalSet1d.h" + +namespace trajopt { + +/** + * Specifies the required minimum distance between a line segment on the + * robot's frame and a point on the field. + */ +struct LinePointConstraint { + /// robot line start x + double robotLineStartX; + /// robot line start y + double robotLineStartY; + /// robot line end x + double robotLineEndX; + /// robot line end y + double robotLineEndY; + /// field point x + double fieldPointX; + /// field point y + double fieldPointY; + /// the allowed distances between the line segment and point + IntervalSet1d distance; +}; + +} // namespace trajopt diff --git a/include/trajopt/constraint/PointLineConstraint.h b/include/trajopt/constraint/PointLineConstraint.h new file mode 100644 index 00000000..0552b5b2 --- /dev/null +++ b/include/trajopt/constraint/PointLineConstraint.h @@ -0,0 +1,30 @@ +// Copyright (c) TrajoptLib contributors + +#pragma once + +#include "trajopt/set/IntervalSet1d.h" + +namespace trajopt { + +/** + * Specifies the required minimum distance between a point on the robot's + * frame and a line segment on the field. + */ +struct PointLineConstraint { + /// robot point x + double robotPointX; + /// robot point y + double robotPointY; + /// field line start x + double fieldLineStartX; + /// field line start y + double fieldLineStartY; + /// field line end x + double fieldLineEndX; + /// field line end y + double fieldLineEndY; + /// the required minimum distance between the point and line, must be positive + IntervalSet1d distance; +}; + +} // namespace trajopt diff --git a/include/trajopt/constraint/PointPointConstraint.h b/include/trajopt/constraint/PointPointConstraint.h new file mode 100644 index 00000000..335d9e87 --- /dev/null +++ b/include/trajopt/constraint/PointPointConstraint.h @@ -0,0 +1,26 @@ +// Copyright (c) TrajoptLib contributors + +#pragma once + +#include "trajopt/set/IntervalSet1d.h" + +namespace trajopt { + +/** + * Specifies the required distance between a point on the robot's frame + * and a point on the field. + */ +struct PointPointConstraint { + /// robot point x + double robotPointX; + /// robot point y + double robotPointY; + /// field point x + double fieldPointX; + /// field point y + double fieldPointY; + /// the required distance between the point and point, must be positive + IntervalSet1d distance; +}; + +} // namespace trajopt diff --git a/include/trajopt/constraint/TranslationConstraint.h b/include/trajopt/constraint/TranslationConstraint.h new file mode 100644 index 00000000..ab78ef5a --- /dev/null +++ b/include/trajopt/constraint/TranslationConstraint.h @@ -0,0 +1,18 @@ +// Copyright (c) TrajoptLib contributors + +#pragma once + +#include "trajopt/SymbolExports.h" +#include "trajopt/set/Set2d.h" + +namespace trajopt { + +/** + * Translation constraint. + */ +struct TRAJOPT_DLLEXPORT TranslationConstraint { + /// Translation bound. + Set2d translationBound; +}; + +} // namespace trajopt diff --git a/include/trajopt/constraint/differential/DifferentialCentripetalAccelerationConstraint.h b/include/trajopt/constraint/differential/DifferentialCentripetalAccelerationConstraint.h new file mode 100644 index 00000000..99da1673 --- /dev/null +++ b/include/trajopt/constraint/differential/DifferentialCentripetalAccelerationConstraint.h @@ -0,0 +1,18 @@ +// Copyright (c) TrajoptLib contributors + +#pragma once + +#include "trajopt/SymbolExports.h" +#include "trajopt/set/IntervalSet1d.h" + +namespace trajopt { + +/** + * Differential centripetal acceleration constraint + */ +struct TRAJOPT_DLLEXPORT DifferentialCentripetalAccelerationConstraint { + /// Acceleration bound. + IntervalSet1d accelerationBound; +}; + +} // namespace trajopt diff --git a/include/trajopt/constraint/differential/DifferentialConstraint.h b/include/trajopt/constraint/differential/DifferentialConstraint.h new file mode 100644 index 00000000..189beac8 --- /dev/null +++ b/include/trajopt/constraint/differential/DifferentialConstraint.h @@ -0,0 +1,18 @@ +// Copyright (c) TrajoptLib contributors + +#pragma once + +#include "trajopt/constraint/AngularVelocityConstraint.h" +#include "trajopt/constraint/Constraint.h" +#include "trajopt/constraint/differential/DifferentialCentripetalAccelerationConstraint.h" +#include "trajopt/constraint/differential/DifferentialTangentialVelocityConstraint.h" +#include "trajopt/util/AppendVariant.h" + +namespace trajopt { + +using DifferentialConstraint = + decltype(_append_variant(Constraint{}, AngularVelocityConstraint{}, + DifferentialTangentialVelocityConstraint{}, + DifferentialCentripetalAccelerationConstraint{})); + +} // namespace trajopt diff --git a/include/trajopt/constraint/differential/DifferentialTangentialVelocityConstraint.h b/include/trajopt/constraint/differential/DifferentialTangentialVelocityConstraint.h new file mode 100644 index 00000000..7e8adccf --- /dev/null +++ b/include/trajopt/constraint/differential/DifferentialTangentialVelocityConstraint.h @@ -0,0 +1,18 @@ +// Copyright (c) TrajoptLib contributors + +#pragma once + +#include "trajopt/SymbolExports.h" +#include "trajopt/set/IntervalSet1d.h" + +namespace trajopt { + +/** + * Differential Tangential Velocity constraint. + */ +struct TRAJOPT_DLLEXPORT DifferentialTangentialVelocityConstraint { + /// Velocity bound. + IntervalSet1d velocityBound; +}; + +} // namespace trajopt diff --git a/include/trajopt/constraint/holonomic/HolonomicConstraint.h b/include/trajopt/constraint/holonomic/HolonomicConstraint.h new file mode 100644 index 00000000..6ecd7bee --- /dev/null +++ b/include/trajopt/constraint/holonomic/HolonomicConstraint.h @@ -0,0 +1,15 @@ +// Copyright (c) TrajoptLib contributors + +#pragma once + +#include "trajopt/constraint/AngularVelocityConstraint.h" +#include "trajopt/constraint/Constraint.h" +#include "trajopt/constraint/holonomic/HolonomicVelocityConstraint.h" +#include "trajopt/util/AppendVariant.h" + +namespace trajopt { + +using HolonomicConstraint = decltype(_append_variant( + Constraint{}, AngularVelocityConstraint{}, HolonomicVelocityConstraint{})); + +} // namespace trajopt diff --git a/include/trajopt/constraint/holonomic/HolonomicVelocityConstraint.h b/include/trajopt/constraint/holonomic/HolonomicVelocityConstraint.h new file mode 100644 index 00000000..850cb874 --- /dev/null +++ b/include/trajopt/constraint/holonomic/HolonomicVelocityConstraint.h @@ -0,0 +1,22 @@ +// Copyright (c) TrajoptLib contributors + +#pragma once + +#include "trajopt/SymbolExports.h" +#include "trajopt/constraint/Constraint.h" +#include "trajopt/set/Set2d.h" + +namespace trajopt { + +/** + * Velocity constraint. + */ +struct TRAJOPT_DLLEXPORT HolonomicVelocityConstraint { + /// Velocity bound. + Set2d velocityBound; + + /// Coordinate system. + CoordinateSystem coordinateSystem; +}; + +} // namespace trajopt diff --git a/include/drivetrain/DifferentialDrivetrain.h b/include/trajopt/drivetrain/DifferentialDrivetrain.h similarity index 95% rename from include/drivetrain/DifferentialDrivetrain.h rename to include/trajopt/drivetrain/DifferentialDrivetrain.h index 582d138e..94c796c6 100644 --- a/include/drivetrain/DifferentialDrivetrain.h +++ b/include/trajopt/drivetrain/DifferentialDrivetrain.h @@ -2,7 +2,7 @@ #pragma once -#include "SymbolExports.h" +#include "trajopt/SymbolExports.h" namespace trajopt { @@ -34,4 +34,5 @@ struct TRAJOPT_DLLEXPORT DifferentialDrivetrain { /// the right driverail DifferentialDriverail right; }; + } // namespace trajopt diff --git a/include/trajopt/drivetrain/SwerveDrivetrain.h b/include/trajopt/drivetrain/SwerveDrivetrain.h new file mode 100644 index 00000000..5bc9346e --- /dev/null +++ b/include/trajopt/drivetrain/SwerveDrivetrain.h @@ -0,0 +1,28 @@ +// Copyright (c) TrajoptLib contributors + +#pragma once + +#include + +#include "trajopt/SymbolExports.h" +#include "trajopt/drivetrain/SwerveModule.h" + +namespace trajopt { + +/** + * @brief This class represents a swerve drivetrain robot. It includes the + * physical properties necessary to accurately model the dynamics of the system. + * An arbitrary number of swerve modules can be specified, but typically it will + * be four. The order the swerve modules are listed does not matter. + */ +struct TRAJOPT_DLLEXPORT SwerveDrivetrain { + /// the mass of the robot + double mass; + /// the moment of inertial of the robot about the origin + double moi; + /// The list of swerve modules that make the robot move, usually one in each + /// corner. + std::vector modules; +}; + +} // namespace trajopt diff --git a/include/trajopt/drivetrain/SwerveModule.h b/include/trajopt/drivetrain/SwerveModule.h new file mode 100644 index 00000000..35e70f94 --- /dev/null +++ b/include/trajopt/drivetrain/SwerveModule.h @@ -0,0 +1,38 @@ +// Copyright (c) TrajoptLib contributors + +#pragma once + +#include "trajopt/SymbolExports.h" + +namespace trajopt { + +/** + * @brief This class represents a single swerve module in a swerve drivetrain. + * It is defined by the module diagonal, which is the line connecting the origin + * of the robot coordinate system to the center of the module. The wheel radius, + * max speed, and max torque must also be specified per module. + */ +struct TRAJOPT_DLLEXPORT SwerveModule { + /** + * @brief x-coordinate of swerve module relative to robot coordinate system + */ + double x; + /** + * @brief y-coordinate of swerve module relative to robot coordinate system + */ + double y; + /** + * @brief radius of wheel + */ + double wheelRadius; + /** + * @brief maximum angular velocity of wheel + */ + double wheelMaxAngularVelocity; + /** + * @brief maximum torque applied to wheel + */ + double wheelMaxTorque; +}; + +} // namespace trajopt diff --git a/include/obstacle/Bumpers.h b/include/trajopt/obstacle/Bumpers.h similarity index 59% rename from include/obstacle/Bumpers.h rename to include/trajopt/obstacle/Bumpers.h index 820652b4..1abe7fdc 100644 --- a/include/obstacle/Bumpers.h +++ b/include/trajopt/obstacle/Bumpers.h @@ -2,11 +2,8 @@ #pragma once -#include - -#include "SymbolExports.h" -#include "obstacle/Obstacle.h" -#include "obstacle/ObstaclePoint.h" +#include "trajopt/obstacle/Obstacle.h" +#include "trajopt/obstacle/ObstaclePoint.h" namespace trajopt { diff --git a/include/obstacle/Obstacle.h b/include/trajopt/obstacle/Obstacle.h similarity index 89% rename from include/obstacle/Obstacle.h rename to include/trajopt/obstacle/Obstacle.h index 146fa713..12501e80 100644 --- a/include/obstacle/Obstacle.h +++ b/include/trajopt/obstacle/Obstacle.h @@ -4,8 +4,8 @@ #include -#include "SymbolExports.h" -#include "obstacle/ObstaclePoint.h" +#include "trajopt/SymbolExports.h" +#include "trajopt/obstacle/ObstaclePoint.h" namespace trajopt { diff --git a/include/obstacle/ObstaclePoint.h b/include/trajopt/obstacle/ObstaclePoint.h similarity index 91% rename from include/obstacle/ObstaclePoint.h rename to include/trajopt/obstacle/ObstaclePoint.h index e6c57e45..5c91ee8c 100644 --- a/include/obstacle/ObstaclePoint.h +++ b/include/trajopt/obstacle/ObstaclePoint.h @@ -2,7 +2,7 @@ #pragma once -#include "SymbolExports.h" +#include "trajopt/SymbolExports.h" namespace trajopt { diff --git a/include/trajopt/path/InitialGuessPoint.h b/include/trajopt/path/InitialGuessPoint.h new file mode 100644 index 00000000..f2914781 --- /dev/null +++ b/include/trajopt/path/InitialGuessPoint.h @@ -0,0 +1,24 @@ +// Copyright (c) TrajoptLib contributors + +#pragma once + +#include "trajopt/SymbolExports.h" + +namespace trajopt { + +/** + * @brief An initial guess of a possible state the robot may be in during the + * trajectory. + */ +struct TRAJOPT_DLLEXPORT InitialGuessPoint { + /// The initial guess of the x-coordinate of the robot. + double x; + + /// The initial guess of the y-coordinate of the robot. + double y; + + /// The initial guess of the heading of the robot. + double heading; +}; + +} // namespace trajopt diff --git a/include/path/Path.h b/include/trajopt/path/Path.h similarity index 73% rename from include/path/Path.h rename to include/trajopt/path/Path.h index baae5fa2..68675ae6 100644 --- a/include/path/Path.h +++ b/include/trajopt/path/Path.h @@ -2,19 +2,13 @@ #pragma once -#include -#include -#include #include -#include "SymbolExports.h" -#include "constraint/differential/DifferentialConstraint.h" -#include "constraint/holonomic/HolonomicConstraint.h" -#include "drivetrain/DifferentialDrivetrain.h" -#include "drivetrain/SwerveDrivetrain.h" -#include "path/InitialGuessPoint.h" -#include "solution/DifferentialSolution.h" -#include "solution/SwerveSolution.h" +#include "trajopt/SymbolExports.h" +#include "trajopt/constraint/differential/DifferentialConstraint.h" +#include "trajopt/constraint/holonomic/HolonomicConstraint.h" +#include "trajopt/drivetrain/DifferentialDrivetrain.h" +#include "trajopt/drivetrain/SwerveDrivetrain.h" namespace trajopt { @@ -57,4 +51,5 @@ struct TRAJOPT_DLLEXPORT DifferentialPath { /// drivetrain of the robot DifferentialDrivetrain drivetrain; }; + } // namespace trajopt diff --git a/include/path/SwervePathBuilder.h b/include/trajopt/path/SwervePathBuilder.h similarity index 95% rename from include/path/SwervePathBuilder.h rename to include/trajopt/path/SwervePathBuilder.h index fdcc3446..460e513f 100644 --- a/include/path/SwervePathBuilder.h +++ b/include/trajopt/path/SwervePathBuilder.h @@ -2,15 +2,17 @@ #pragma once +#include #include -#include "drivetrain/SwerveDrivetrain.h" -#include "obstacle/Bumpers.h" -#include "obstacle/Obstacle.h" -#include "path/InitialGuessPoint.h" -#include "path/Path.h" -#include "set/IntervalSet1d.h" -#include "set/Set2d.h" +#include "trajopt/drivetrain/SwerveDrivetrain.h" +#include "trajopt/obstacle/Bumpers.h" +#include "trajopt/obstacle/Obstacle.h" +#include "trajopt/path/InitialGuessPoint.h" +#include "trajopt/path/Path.h" +#include "trajopt/set/IntervalSet1d.h" +#include "trajopt/set/Set2d.h" +#include "trajopt/solution/Solution.h" namespace trajopt { @@ -21,6 +23,11 @@ namespace trajopt { */ class TRAJOPT_DLLEXPORT SwervePathBuilder { public: + /** + * Cancel all currently generating SwervePathBuilders. + */ + void CancelAll(); + /** * Get the SwervePath being constructed * @@ -47,6 +54,7 @@ class TRAJOPT_DLLEXPORT SwervePathBuilder { * @param heading the heading */ void PoseWpt(size_t idx, double x, double y, double heading); + /** * Create a translation waypoint constraint on the waypoint at the * provided index, and add an initial guess point with the same translation. @@ -118,6 +126,7 @@ class TRAJOPT_DLLEXPORT SwervePathBuilder { * @param vtheta velocity vector polar angle */ void WptVelocityPolar(size_t idx, double vr, double vtheta); + /** * Specify the required angular velocity of the robot to be zero * at a waypoint diff --git a/include/trajopt/set/ConeSet2d.h b/include/trajopt/set/ConeSet2d.h new file mode 100644 index 00000000..c4848954 --- /dev/null +++ b/include/trajopt/set/ConeSet2d.h @@ -0,0 +1,23 @@ +// Copyright (c) TrajoptLib contributors + +#pragma once + +#include "trajopt/SymbolExports.h" +#include "trajopt/set/IntervalSet1d.h" + +namespace trajopt { + +/** + * Conical 2D set. + */ +struct TRAJOPT_DLLEXPORT ConeSet2d { + /// The heading bounds of the cone. + IntervalSet1d thetaBound; + + /** + * Returns true if the set is valid. + */ + bool IsValid() const noexcept; +}; + +} // namespace trajopt diff --git a/include/trajopt/set/EllipticalSet2d.h b/include/trajopt/set/EllipticalSet2d.h new file mode 100644 index 00000000..4eeadf0d --- /dev/null +++ b/include/trajopt/set/EllipticalSet2d.h @@ -0,0 +1,59 @@ +// Copyright (c) TrajoptLib contributors + +#pragma once + +#include "trajopt/SymbolExports.h" + +namespace trajopt { + +/** + * Elliptical 2D set. + */ +struct TRAJOPT_DLLEXPORT EllipticalSet2d { + /** + * FIXME What does this do? + */ + enum class Direction { + /// FIXME What does this do? + kInside, + /// FIXME What does this do? + kCentered, + /// FIXME What does this do? + kOutside + }; + + /// The x radius. + double xRadius; + + /// The y radius. + double yRadius; + + /// The direction. + Direction direction; + + /** + * Construct a circular EllipticalSet2d from a radius. + * + * @param radius The radius. + * @param direction The direction. + */ + static EllipticalSet2d CircularSet2d( + double radius, Direction direction = Direction::kInside); + + /** + * Returns true if the ellipse is a circle. + */ + bool IsCircular() const noexcept; + + /** + * Returns true if the set spans R². + */ + bool IsR2() const noexcept; + + /** + * Returns true if the set is valid. + */ + bool IsValid() const noexcept; +}; + +} // namespace trajopt diff --git a/include/set/IntervalSet1d.h b/include/trajopt/set/IntervalSet1d.h similarity index 71% rename from include/set/IntervalSet1d.h rename to include/trajopt/set/IntervalSet1d.h index 47f005d3..1f387721 100644 --- a/include/set/IntervalSet1d.h +++ b/include/trajopt/set/IntervalSet1d.h @@ -2,12 +2,7 @@ #pragma once -#include - -#include - -#include "SymbolExports.h" -#include "solution/SolutionChecking.h" +#include "trajopt/SymbolExports.h" namespace trajopt { @@ -107,15 +102,6 @@ struct TRAJOPT_DLLEXPORT IntervalSet1d { */ bool IsUpperBounded() const noexcept; - /** - * Returns an error if the given scalar isn't in the set. - * - * @param scalar The scalar. - * @param tolerances The tolerances considered to satisfy the constraint. - */ - std::optional CheckScalar( - double scalar, const SolutionTolerances& tolerances) const noexcept; - /** * Check if this scalar bound is valid. A scalar bound is valid * if and only if the lower bound is less than or equal to the upper @@ -125,34 +111,5 @@ struct TRAJOPT_DLLEXPORT IntervalSet1d { */ bool IsValid() const noexcept; }; -} // namespace trajopt - -/** - * Formatter for IntervalSet1d. - */ -//! @cond Doxygen_Suppress -template <> -struct fmt::formatter { - //! @endcond - /** - * Format string parser. - * - * @param ctx Format string context. - */ - constexpr auto parse(fmt::format_parse_context& ctx) { return ctx.begin(); } - /** - * Writes out a formatted IntervalSet1d. - * - * @param set1d IntervalSet1d instance. - * @param ctx Format string context. - */ - auto format(const trajopt::IntervalSet1d& set1d, - fmt::format_context& ctx) const { - if (set1d.IsExact()) { - return fmt::format_to(ctx.out(), "= {}", set1d.lower); - } else { - return fmt::format_to(ctx.out(), "∈ [{}, {}]", set1d.lower, set1d.upper); - } - } -}; +} // namespace trajopt diff --git a/include/trajopt/set/LinearSet2d.h b/include/trajopt/set/LinearSet2d.h new file mode 100644 index 00000000..6ffcc73d --- /dev/null +++ b/include/trajopt/set/LinearSet2d.h @@ -0,0 +1,28 @@ +// Copyright (c) TrajoptLib contributors + +#pragma once + +#include "trajopt/SymbolExports.h" +#include "trajopt/set/IntervalSet1d.h" +#include "trajopt/set/RectangularSet2d.h" + +namespace trajopt { + +/** + * Linear 2D set. + */ +struct TRAJOPT_DLLEXPORT LinearSet2d { + /// FIXME What does this do? + double theta; + + /** + * FIXME What does this do? + * + * @param theta FIXME What does this do? + * @param rBound FIXME What does this do? + */ + static RectangularSet2d RBoundToRectangular(double theta, + const IntervalSet1d& rBound); +}; + +} // namespace trajopt diff --git a/include/trajopt/set/RectangularSet2d.h b/include/trajopt/set/RectangularSet2d.h new file mode 100644 index 00000000..9e359b0c --- /dev/null +++ b/include/trajopt/set/RectangularSet2d.h @@ -0,0 +1,44 @@ +// Copyright (c) TrajoptLib contributors + +#pragma once + +#include "trajopt/SymbolExports.h" +#include "trajopt/set/IntervalSet1d.h" + +namespace trajopt { + +/** + * Rectangular 2D set. + */ +struct TRAJOPT_DLLEXPORT RectangularSet2d { + /// The x boundary. + IntervalSet1d xBound; + + /// The y boundary. + IntervalSet1d yBound; + + /** + * Construct a RectangularSet2d from polar coordinates. + * + * @param r The distance. + * @param theta The heading. + */ + static RectangularSet2d PolarExactSet2d(double r, double theta); + + /** + * Construct a RectangularSet2d spanning R². + */ + static RectangularSet2d R2(); + + /** + * @brief Check if this planar bound is valid. A planar bound is valid when + * the bounds on a0 and a1 are valid, and additionally for planar bounds, a0 + * is contained within the interval [0, inf] and a1 is contained within the + * interval [-pi, pi]. + * + * @return true if and only if this planar bound is valid + */ + bool IsValid() const noexcept; +}; + +} // namespace trajopt diff --git a/include/trajopt/set/Set2d.h b/include/trajopt/set/Set2d.h new file mode 100644 index 00000000..cbafddad --- /dev/null +++ b/include/trajopt/set/Set2d.h @@ -0,0 +1,39 @@ +// Copyright (c) TrajoptLib contributors + +#pragma once + +#include + +#include "trajopt/SymbolExports.h" +#include "trajopt/set/ConeSet2d.h" +#include "trajopt/set/EllipticalSet2d.h" +#include "trajopt/set/LinearSet2d.h" +#include "trajopt/set/RectangularSet2d.h" + +namespace trajopt { + +/** + * @brief This class represents a bounded region of abstract 2D space. The + * bounds are specified with inequalities for each coordinate, a0 and a1. The + * interpretation of the abstract coordinates a0 and a1 changes when + * coordinateType changes. If coordinateType is kRectangular, then a0 and a1 + * represent the rectangular x-coordinate and y-coordinate bounds, respectively. + * If coordinateType is kPolar, then a0 and a1 represent the polar r-coordinate + * and theta-coordinate bounds, respectively. Polar theta coordinates must be + * specified between -pi and pi, inclusive, and all lower bounds must be less + * than or equal to their respective upper bounds. + * + * The following are some common use cases: + * A straight line theta = theta_0 bound can be created as a polar bound + * bounding theta within [theta_0, theta_0], and a straight line x = x_0 or y = + * y_0 bound can be created by bounding x within [x_0, x_0] or y within [y_0, + * y_0]. Bounding the magnitude of a vector can be achieved by bounding r within + * [r_lower, r_upper]. + * + * Bounding theta within [-pi, -pi] or within [pi, pi] is equivalent; both will + * force the vector to be colinear with the x-axis. + */ +using Set2d = + std::variant; + +} // namespace trajopt diff --git a/include/solution/DifferentialSolution.h b/include/trajopt/solution/DifferentialSolution.h similarity index 87% rename from include/solution/DifferentialSolution.h rename to include/trajopt/solution/DifferentialSolution.h index abb24c70..9bad9b02 100644 --- a/include/solution/DifferentialSolution.h +++ b/include/trajopt/solution/DifferentialSolution.h @@ -4,8 +4,8 @@ #include -#include "SymbolExports.h" -#include "solution/Solution.h" +#include "trajopt/SymbolExports.h" +#include "trajopt/solution/Solution.h" namespace trajopt { diff --git a/include/solution/HolonomicSolution.h b/include/trajopt/solution/HolonomicSolution.h similarity index 88% rename from include/solution/HolonomicSolution.h rename to include/trajopt/solution/HolonomicSolution.h index 560cae14..67f5dc5f 100644 --- a/include/solution/HolonomicSolution.h +++ b/include/trajopt/solution/HolonomicSolution.h @@ -4,8 +4,8 @@ #include -#include "SymbolExports.h" -#include "solution/Solution.h" +#include "trajopt/SymbolExports.h" +#include "trajopt/solution/Solution.h" namespace trajopt { diff --git a/include/solution/Solution.h b/include/trajopt/solution/Solution.h similarity index 91% rename from include/solution/Solution.h rename to include/trajopt/solution/Solution.h index 754dcb8d..08c2923d 100644 --- a/include/solution/Solution.h +++ b/include/trajopt/solution/Solution.h @@ -4,7 +4,7 @@ #include -#include "SymbolExports.h" +#include "trajopt/SymbolExports.h" namespace trajopt { diff --git a/include/trajopt/solution/SwerveSolution.h b/include/trajopt/solution/SwerveSolution.h new file mode 100644 index 00000000..78e7950d --- /dev/null +++ b/include/trajopt/solution/SwerveSolution.h @@ -0,0 +1,23 @@ +// Copyright (c) TrajoptLib contributors + +#pragma once + +#include + +#include "trajopt/SymbolExports.h" +#include "trajopt/solution/HolonomicSolution.h" + +namespace trajopt { + +/** + * The swerve drive trajectory optimization solution. + */ +struct TRAJOPT_DLLEXPORT SwerveSolution : HolonomicSolution { + /// The x forces for each module. + std::vector> moduleFX; + + /// The y forces for each module. + std::vector> moduleFY; +}; + +} // namespace trajopt diff --git a/include/trajopt/trajectory/HolonomicTrajectory.h b/include/trajopt/trajectory/HolonomicTrajectory.h new file mode 100644 index 00000000..0ab97d66 --- /dev/null +++ b/include/trajopt/trajectory/HolonomicTrajectory.h @@ -0,0 +1,50 @@ +// Copyright (c) TrajoptLib contributors + +#pragma once + +#include +#include + +#include "trajopt/SymbolExports.h" +#include "trajopt/solution/HolonomicSolution.h" +#include "trajopt/trajectory/HolonomicTrajectorySample.h" + +namespace trajopt { + +/** + * Holonomic trajectory. + */ +class TRAJOPT_DLLEXPORT HolonomicTrajectory { + public: + /// Trajectory samples. + std::vector samples; + + HolonomicTrajectory() = default; + + /** + * Construct a HolonomicTrajectory from samples. + * + * @param samples The samples. + */ + explicit HolonomicTrajectory(std::vector samples) + : samples{std::move(samples)} {} + + /** + * Construct a HolonomicTrajectory from a solution. + * + * @param solution The solution. + */ + explicit HolonomicTrajectory(const HolonomicSolution& solution) { + double ts = 0.0; + for (size_t samp = 0; samp < solution.x.size(); ++samp) { + if (samp != 0) { + ts += solution.dt[samp - 1]; + } + samples.emplace_back(ts, solution.x[samp], solution.y[samp], + solution.theta[samp], solution.vx[samp], + solution.vy[samp], solution.omega[samp]); + } + } +}; + +} // namespace trajopt diff --git a/include/trajopt/trajectory/HolonomicTrajectorySample.h b/include/trajopt/trajectory/HolonomicTrajectorySample.h new file mode 100644 index 00000000..767cebeb --- /dev/null +++ b/include/trajopt/trajectory/HolonomicTrajectorySample.h @@ -0,0 +1,60 @@ +// Copyright (c) TrajoptLib contributors + +#pragma once + +#include "trajopt/SymbolExports.h" + +namespace trajopt { + +/** + * Holonomic trajectory sample. + */ +class TRAJOPT_DLLEXPORT HolonomicTrajectorySample { + public: + /// The timestamp. + double timestamp = 0.0; + + /// The x coordinate. + double x = 0.0; + + /// The y coordinate. + double y = 0.0; + + /// The heading. + double heading = 0.0; + + /// The velocity's x component. + double velocityX = 0.0; + + /// The velocity's y component. + double velocityY = 0.0; + + /// The angular velocity. + double angularVelocity = 0.0; + + constexpr HolonomicTrajectorySample() = default; + + /** + * Construct a HolonomicTrajectorySample. + * + * @param timestamp The timestamp. + * @param x The x coordinate. + * @param y The y coordinate. + * @param heading The heading. + * @param velocityX The velocity's x component. + * @param velocityY The velocity's y component. + * @param angularVelocity The angular velocity. + */ + constexpr HolonomicTrajectorySample(double timestamp, double x, double y, + double heading, double velocityX, + double velocityY, double angularVelocity) + : timestamp{timestamp}, + x{x}, + y{y}, + heading{heading}, + velocityX{velocityX}, + velocityY{velocityY}, + angularVelocity{angularVelocity} {} +}; + +} // namespace trajopt diff --git a/include/util/AppendVariant.h b/include/trajopt/util/AppendVariant.h similarity index 100% rename from include/util/AppendVariant.h rename to include/trajopt/util/AppendVariant.h diff --git a/jni/TrajoptLibJNI.cpp b/jni/TrajoptLibJNI.cpp index 5e09795a..1a3cad44 100644 --- a/jni/TrajoptLibJNI.cpp +++ b/jni/TrajoptLibJNI.cpp @@ -5,27 +5,27 @@ #include #include -#include "InvalidPathException.h" -#include "OptimalTrajectoryGenerator.h" -#include "TrajectoryGenerationException.h" -#include "constraint/AngularVelocityConstraint.h" -#include "constraint/Constraint.h" -#include "constraint/HeadingConstraint.h" -#include "constraint/holonomic/HolonomicConstraint.h" -#include "constraint/holonomic/HolonomicVelocityConstraint.h" -#include "drivetrain/SwerveDrivetrain.h" -#include "drivetrain/SwerveModule.h" #include "jni.h" -#include "obstacle/Obstacle.h" -#include "obstacle/ObstaclePoint.h" #include "org_sleipnirgroup_trajopt_OptimalTrajectoryGenerator.h" -#include "path/InitialGuessPoint.h" -#include "path/Path.h" -#include "path/SwervePathBuilder.h" -#include "set/EllipticalSet2d.h" -#include "set/IntervalSet1d.h" -#include "set/RectangularSet2d.h" -#include "trajectory/HolonomicTrajectory.h" +#include "trajopt/InvalidPathException.h" +#include "trajopt/OptimalTrajectoryGenerator.h" +#include "trajopt/TrajectoryGenerationException.h" +#include "trajopt/constraint/AngularVelocityConstraint.h" +#include "trajopt/constraint/Constraint.h" +#include "trajopt/constraint/HeadingConstraint.h" +#include "trajopt/constraint/holonomic/HolonomicConstraint.h" +#include "trajopt/constraint/holonomic/HolonomicVelocityConstraint.h" +#include "trajopt/drivetrain/SwerveDrivetrain.h" +#include "trajopt/drivetrain/SwerveModule.h" +#include "trajopt/obstacle/Obstacle.h" +#include "trajopt/obstacle/ObstaclePoint.h" +#include "trajopt/path/InitialGuessPoint.h" +#include "trajopt/path/Path.h" +#include "trajopt/path/SwervePathBuilder.h" +#include "trajopt/set/EllipticalSet2d.h" +#include "trajopt/set/IntervalSet1d.h" +#include "trajopt/set/RectangularSet2d.h" +#include "trajopt/trajectory/HolonomicTrajectory.h" using namespace trajopt; diff --git a/node/CMakeLists.txt b/node/CMakeLists.txt index 437260b5..be89d59e 100644 --- a/node/CMakeLists.txt +++ b/node/CMakeLists.txt @@ -6,7 +6,7 @@ set(CMAKE_CXX_STANDARD 20) set(CMAKE_EXPORT_COMPILE_COMMANDS ON) -project (trajoptlib-node) +project(trajoptlib-node) add_definitions(-DNAPI_VERSION=7) @@ -23,33 +23,29 @@ target_link_libraries(${PROJECT_NAME} ${CMAKE_JS_LIB}) set(OPTIMIZER_BACKEND casadi) set(CMAKE_APPLE_SILICON_PROCESSOR arm64) - -include(FetchContent) - -FetchContent_Declare( - TrajoptLib - GIT_REPOSITORY https://github.com/SleipnirGroup/TrajoptLib.git - GIT_TAG 431b82ffb82e13506fd4320f4e2c3117ef505838 +add_subdirectory( + ${CMAKE_CURRENT_SOURCE_DIR}/.. + ${CMAKE_CURRENT_SOURCE_DIR}/../build + EXCLUDE_FROM_ALL ) -FetchContent_GetProperties(TrajoptLib) -if(NOT TrajoptLib_POPULATED) - FetchContent_Populate(TrajoptLib) - add_subdirectory(${trajoptlib_SOURCE_DIR} ${trajoptlib_BINARY_DIR} EXCLUDE_FROM_ALL) -endif() - target_link_libraries(${PROJECT_NAME} PRIVATE TrajoptLib) if(MSVC AND CMAKE_JS_NODELIB_DEF AND CMAKE_JS_NODELIB_TARGET) - # Generate node.lib - execute_process(COMMAND ${CMAKE_AR} /def:${CMAKE_JS_NODELIB_DEF} /out:${CMAKE_JS_NODELIB_TARGET} ${CMAKE_STATIC_LINKER_FLAGS}) + # Generate node.lib + execute_process( + COMMAND + ${CMAKE_AR} /def:${CMAKE_JS_NODELIB_DEF} + /out:${CMAKE_JS_NODELIB_TARGET} ${CMAKE_STATIC_LINKER_FLAGS} + ) endif() # Include N-API wrappers -execute_process(COMMAND node -p "require('node-addon-api').include" - WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} - OUTPUT_VARIABLE NODE_ADDON_API_DIR - ) +execute_process( + COMMAND node -p "require('node-addon-api').include" + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + OUTPUT_VARIABLE NODE_ADDON_API_DIR +) # strip `"` and `\n` from the output above string(REPLACE "\n" "" NODE_ADDON_API_DIR ${NODE_ADDON_API_DIR}) diff --git a/node/src/SwervePathBuilderWrap.cc b/node/src/SwervePathBuilderWrap.cc index 13cc9634..67bda683 100644 --- a/node/src/SwervePathBuilderWrap.cc +++ b/node/src/SwervePathBuilderWrap.cc @@ -5,12 +5,12 @@ #include -#include -#include -#include -#include +#include +#include +#include +#include #include -#include +#include SwervePathBuilderWrap::SwervePathBuilderWrap(const Napi::CallbackInfo& info) : ObjectWrap(info) { } diff --git a/node/src/SwervePathBuilderWrap.h b/node/src/SwervePathBuilderWrap.h index 33655b60..b31b34db 100644 --- a/node/src/SwervePathBuilderWrap.h +++ b/node/src/SwervePathBuilderWrap.h @@ -3,7 +3,7 @@ #pragma once #include -#include +#include class SwervePathBuilderWrap : public Napi::ObjectWrap { public: diff --git a/rust/Cargo.lock b/rust/Cargo.lock new file mode 100644 index 00000000..335d31dc --- /dev/null +++ b/rust/Cargo.lock @@ -0,0 +1,656 @@ +# This file is automatically @generated by Cargo. +# It is not intended for manual editing. +version = 3 + +[[package]] +name = "addr2line" +version = "0.19.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a76fd60b23679b7d19bd066031410fb7e458ccc5e958eb5c325888ce4baedc97" +dependencies = [ + "gimli", +] + +[[package]] +name = "adler" +version = "1.0.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f26201604c87b1e01bd3d98f8d5d9a8fcbb815e8cedb41ffccbeb4bf593a35fe" + +[[package]] +name = "ahash" +version = "0.7.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fcb51a0695d8f838b1ee009b3fbf66bda078cd64590202a864a8f3e8c4315c47" +dependencies = [ + "getrandom", + "once_cell", + "version_check", +] + +[[package]] +name = "aho-corasick" +version = "1.0.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "43f6cb1bf222025340178f382c426f13757b2960e89779dfcb319c32542a5a41" +dependencies = [ + "memchr", +] + +[[package]] +name = "backtrace" +version = "0.3.67" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "233d376d6d185f2a3093e58f283f60f880315b6c60075b01f36b3b85154564ca" +dependencies = [ + "addr2line", + "cc", + "cfg-if", + "libc", + "miniz_oxide", + "object", + "rustc-demangle", +] + +[[package]] +name = "backtrace-ext" +version = "0.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "537beee3be4a18fb023b570f80e3ae28003db9167a751266b259926e25539d50" +dependencies = [ + "backtrace", +] + +[[package]] +name = "bitflags" +version = "1.3.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a" + +[[package]] +name = "cc" +version = "1.0.79" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "50d30906286121d95be3d479533b458f87493b30a4b5f79a607db8f5d11aa91f" + +[[package]] +name = "cfg-if" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd" + +[[package]] +name = "cmake" +version = "0.1.50" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a31c789563b815f77f4250caee12365734369f942439b7defd71e18a48197130" +dependencies = [ + "cc", +] + +[[package]] +name = "codespan-reporting" +version = "0.11.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3538270d33cc669650c4b093848450d380def10c331d38c768e34cac80576e6e" +dependencies = [ + "termcolor", + "unicode-width", +] + +[[package]] +name = "cxx" +version = "1.0.97" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e88abab2f5abbe4c56e8f1fb431b784d710b709888f35755a160e62e33fe38e8" +dependencies = [ + "cc", + "cxxbridge-flags", + "cxxbridge-macro", + "link-cplusplus", +] + +[[package]] +name = "cxx-build" +version = "1.0.97" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5c0c11acd0e63bae27dcd2afced407063312771212b7a823b4fd72d633be30fb" +dependencies = [ + "cc", + "codespan-reporting", + "once_cell", + "proc-macro2", + "quote", + "scratch", + "syn", +] + +[[package]] +name = "cxxbridge-flags" +version = "1.0.97" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8d3816ed957c008ccd4728485511e3d9aaf7db419aa321e3d2c5a2f3411e36c8" + +[[package]] +name = "cxxbridge-macro" +version = "1.0.97" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a26acccf6f445af85ea056362561a24ef56cdc15fcc685f03aec50b9c702cb6d" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "errno" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4bcfec3a70f97c962c307b2d2c56e358cf1d00b558d74262b5f929ee8cc7e73a" +dependencies = [ + "errno-dragonfly", + "libc", + "windows-sys", +] + +[[package]] +name = "errno-dragonfly" +version = "0.1.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "aa68f1b12764fab894d2755d2518754e71b4fd80ecfb822714a1206c2aab39bf" +dependencies = [ + "cc", + "libc", +] + +[[package]] +name = "getrandom" +version = "0.2.10" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "be4136b2a15dd319360be1c07d9933517ccf0be8f16bf62a3bee4f0d618df427" +dependencies = [ + "cfg-if", + "libc", + "wasi", +] + +[[package]] +name = "gimli" +version = "0.27.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b6c80984affa11d98d1b88b66ac8853f143217b399d3c74116778ff8fdb4ed2e" + +[[package]] +name = "hashbrown" +version = "0.12.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8a9ee70c43aaf417c914396645a0fa852624801b24ebb7ae78fe8272889ac888" +dependencies = [ + "ahash", +] + +[[package]] +name = "hermit-abi" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fed44880c466736ef9a5c5b5facefb5ed0785676d0c02d612db14e54f0d84286" + +[[package]] +name = "io-lifetimes" +version = "1.0.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "eae7b9aee968036d54dce06cebaefd919e4472e753296daccd6d344e3e2df0c2" +dependencies = [ + "hermit-abi", + "libc", + "windows-sys", +] + +[[package]] +name = "is-terminal" +version = "0.4.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "adcf93614601c8129ddf72e2d5633df827ba6551541c6d8c59520a371475be1f" +dependencies = [ + "hermit-abi", + "io-lifetimes", + "rustix", + "windows-sys", +] + +[[package]] +name = "is_ci" +version = "1.1.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "616cde7c720bb2bb5824a224687d8f77bfd38922027f01d825cd7453be5099fb" + +[[package]] +name = "itoa" +version = "1.0.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "453ad9f582a441959e5f0d088b02ce04cfe8d51a8eaf077f12ac6d3e94164ca6" + +[[package]] +name = "libc" +version = "0.2.147" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b4668fb0ea861c1df094127ac5f1da3409a82116a4ba74fca2e58ef927159bb3" + +[[package]] +name = "link-cplusplus" +version = "1.0.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ecd207c9c713c34f95a097a5b029ac2ce6010530c7b49d7fea24d977dede04f5" +dependencies = [ + "cc", +] + +[[package]] +name = "linux-raw-sys" +version = "0.3.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ef53942eb7bf7ff43a617b3e2c1c4a5ecf5944a7c1bc12d7ee39bbb15e5c1519" + +[[package]] +name = "memchr" +version = "2.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2dffe52ecf27772e601905b7522cb4ef790d2cc203488bbd0e2fe85fcb74566d" + +[[package]] +name = "miette" +version = "5.9.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a236ff270093b0b67451bc50a509bd1bad302cb1d3c7d37d5efe931238581fa9" +dependencies = [ + "backtrace", + "backtrace-ext", + "is-terminal", + "miette-derive", + "once_cell", + "owo-colors", + "supports-color", + "supports-hyperlinks", + "supports-unicode", + "terminal_size", + "textwrap", + "thiserror", + "unicode-width", +] + +[[package]] +name = "miette-derive" +version = "5.9.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4901771e1d44ddb37964565c654a3223ba41a594d02b8da471cc4464912b5cfa" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "miniz_oxide" +version = "0.6.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b275950c28b37e794e8c55d88aeb5e139d0ce23fdbbeda68f8d7174abdf9e8fa" +dependencies = [ + "adler", +] + +[[package]] +name = "object" +version = "0.30.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "03b4680b86d9cfafba8fc491dc9b6df26b68cf40e9e6cd73909194759a63c385" +dependencies = [ + "memchr", +] + +[[package]] +name = "once_cell" +version = "1.18.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dd8b5dd2ae5ed71462c540258bedcb51965123ad7e7ccf4b9a8cafaa4a63576d" + +[[package]] +name = "owo-colors" +version = "3.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c1b04fb49957986fdce4d6ee7a65027d55d4b6d2265e5848bbb507b58ccfdb6f" + +[[package]] +name = "proc-macro2" +version = "1.0.63" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7b368fba921b0dce7e60f5e04ec15e565b3303972b42bcfde1d0713b881959eb" +dependencies = [ + "unicode-ident", +] + +[[package]] +name = "quote" +version = "1.0.28" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1b9ab9c7eadfd8df19006f1cf1a4aed13540ed5cbc047010ece5826e10825488" +dependencies = [ + "proc-macro2", +] + +[[package]] +name = "regex" +version = "1.8.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d0ab3ca65655bb1e41f2a8c8cd662eb4fb035e67c3f78da1d61dffe89d07300f" +dependencies = [ + "aho-corasick", + "memchr", + "regex-syntax", +] + +[[package]] +name = "regex-syntax" +version = "0.7.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "436b050e76ed2903236f032a59761c1eb99e1b0aead2c257922771dab1fc8c78" + +[[package]] +name = "rustc-demangle" +version = "0.1.23" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d626bb9dae77e28219937af045c257c28bfd3f69333c512553507f5f9798cb76" + +[[package]] +name = "rustix" +version = "0.37.20" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b96e891d04aa506a6d1f318d2771bcb1c7dfda84e126660ace067c9b474bb2c0" +dependencies = [ + "bitflags", + "errno", + "io-lifetimes", + "libc", + "linux-raw-sys", + "windows-sys", +] + +[[package]] +name = "ryu" +version = "1.0.13" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f91339c0467de62360649f8d3e185ca8de4224ff281f66000de5eb2a77a79041" + +[[package]] +name = "scratch" +version = "1.0.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1792db035ce95be60c3f8853017b3999209281c24e2ba5bc8e59bf97a0c590c1" + +[[package]] +name = "serde" +version = "1.0.164" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9e8c8cf938e98f769bc164923b06dce91cea1751522f46f8466461af04c9027d" +dependencies = [ + "serde_derive", +] + +[[package]] +name = "serde_derive" +version = "1.0.164" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d9735b638ccc51c28bf6914d90a2e9725b377144fc612c49a611fddd1b631d68" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "serde_json" +version = "1.0.99" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "46266871c240a00b8f503b877622fe33430b3c7d963bdc0f2adc511e54a1eae3" +dependencies = [ + "itoa", + "ryu", + "serde", +] + +[[package]] +name = "smawk" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f67ad224767faa3c7d8b6d91985b78e70a1324408abcb1cfcc2be4c06bc06043" + +[[package]] +name = "supports-color" +version = "2.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4950e7174bffabe99455511c39707310e7e9b440364a2fcb1cc21521be57b354" +dependencies = [ + "is-terminal", + "is_ci", +] + +[[package]] +name = "supports-hyperlinks" +version = "2.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f84231692eb0d4d41e4cdd0cabfdd2e6cd9e255e65f80c9aa7c98dd502b4233d" +dependencies = [ + "is-terminal", +] + +[[package]] +name = "supports-unicode" +version = "2.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4b6c2cb240ab5dd21ed4906895ee23fe5a48acdbd15a3ce388e7b62a9b66baf7" +dependencies = [ + "is-terminal", +] + +[[package]] +name = "syn" +version = "2.0.22" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2efbeae7acf4eabd6bcdcbd11c92f45231ddda7539edc7806bd1a04a03b24616" +dependencies = [ + "proc-macro2", + "quote", + "unicode-ident", +] + +[[package]] +name = "termcolor" +version = "1.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "be55cf8942feac5c765c2c993422806843c9a9a45d4d5c407ad6dd2ea95eb9b6" +dependencies = [ + "winapi-util", +] + +[[package]] +name = "terminal_size" +version = "0.1.17" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "633c1a546cee861a1a6d0dc69ebeca693bf4296661ba7852b9d21d159e0506df" +dependencies = [ + "libc", + "winapi", +] + +[[package]] +name = "textwrap" +version = "0.15.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b7b3e525a49ec206798b40326a44121291b530c963cfb01018f63e135bac543d" +dependencies = [ + "smawk", + "unicode-linebreak", + "unicode-width", +] + +[[package]] +name = "thiserror" +version = "1.0.40" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "978c9a314bd8dc99be594bc3c175faaa9794be04a5a5e153caba6915336cebac" +dependencies = [ + "thiserror-impl", +] + +[[package]] +name = "thiserror-impl" +version = "1.0.40" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f9456a42c5b0d803c8cd86e73dd7cc9edd429499f37a3550d286d5e86720569f" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "trajoptlib" +version = "0.1.0" +dependencies = [ + "cmake", + "cxx", + "cxx-build", + "miette", + "serde", + "serde_json", +] + +[[package]] +name = "unicode-ident" +version = "1.0.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b15811caf2415fb889178633e7724bad2509101cde276048e013b9def5e51fa0" + +[[package]] +name = "unicode-linebreak" +version = "0.1.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c5faade31a542b8b35855fff6e8def199853b2da8da256da52f52f1316ee3137" +dependencies = [ + "hashbrown", + "regex", +] + +[[package]] +name = "unicode-width" +version = "0.1.10" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c0edd1e5b14653f783770bce4a4dabb4a5108a5370a5f5d8cfe8710c361f6c8b" + +[[package]] +name = "version_check" +version = "0.9.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "49874b5167b65d7193b8aba1567f5c7d93d001cafc34600cee003eda787e483f" + +[[package]] +name = "wasi" +version = "0.11.0+wasi-snapshot-preview1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9c8d87e72b64a3b4db28d11ce29237c246188f4f51057d65a7eab63b7987e423" + +[[package]] +name = "winapi" +version = "0.3.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5c839a674fcd7a98952e593242ea400abe93992746761e38641405d28b00f419" +dependencies = [ + "winapi-i686-pc-windows-gnu", + "winapi-x86_64-pc-windows-gnu", +] + +[[package]] +name = "winapi-i686-pc-windows-gnu" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ac3b87c63620426dd9b991e5ce0329eff545bccbbb34f3be09ff6fb6ab51b7b6" + +[[package]] +name = "winapi-util" +version = "0.1.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "70ec6ce85bb158151cae5e5c87f95a8e97d2c0c4b001223f33a334e3ce5de178" +dependencies = [ + "winapi", +] + +[[package]] +name = "winapi-x86_64-pc-windows-gnu" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "712e227841d057c1ee1cd2fb22fa7e5a5461ae8e48fa2ca79ec42cfc1931183f" + +[[package]] +name = "windows-sys" +version = "0.48.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "677d2418bec65e3338edb076e806bc1ec15693c5d0104683f2efe857f61056a9" +dependencies = [ + "windows-targets", +] + +[[package]] +name = "windows-targets" +version = "0.48.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7b1eb6f0cd7c80c79759c929114ef071b87354ce476d9d94271031c0497adfd5" +dependencies = [ + "windows_aarch64_gnullvm", + "windows_aarch64_msvc", + "windows_i686_gnu", + "windows_i686_msvc", + "windows_x86_64_gnu", + "windows_x86_64_gnullvm", + "windows_x86_64_msvc", +] + +[[package]] +name = "windows_aarch64_gnullvm" +version = "0.48.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "91ae572e1b79dba883e0d315474df7305d12f569b400fcf90581b06062f7e1bc" + +[[package]] +name = "windows_aarch64_msvc" +version = "0.48.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b2ef27e0d7bdfcfc7b868b317c1d32c641a6fe4629c171b8928c7b08d98d7cf3" + +[[package]] +name = "windows_i686_gnu" +version = "0.48.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "622a1962a7db830d6fd0a69683c80a18fda201879f0f447f065a3b7467daa241" + +[[package]] +name = "windows_i686_msvc" +version = "0.48.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4542c6e364ce21bf45d69fdd2a8e455fa38d316158cfd43b3ac1c5b1b19f8e00" + +[[package]] +name = "windows_x86_64_gnu" +version = "0.48.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ca2b8a661f7628cbd23440e50b05d705db3686f894fc9580820623656af974b1" + +[[package]] +name = "windows_x86_64_gnullvm" +version = "0.48.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7896dbc1f41e08872e9d5e8f8baa8fdd2677f29468c4e156210174edc7f7b953" + +[[package]] +name = "windows_x86_64_msvc" +version = "0.48.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1a515f5799fe4961cb532f983ce2b23082366b898e52ffbce459c86f67c8378a" diff --git a/rust/Cargo.toml b/rust/Cargo.toml new file mode 100644 index 00000000..b2f68114 --- /dev/null +++ b/rust/Cargo.toml @@ -0,0 +1,25 @@ +[package] +name = "trajoptlib" +version = "0.1.0" +edition = "2021" + +[dependencies] +cxx = "1.0" +serde = { version = "1.0", features = ["derive"] } +serde_json = "1.0" + +[build-dependencies] +cxx-build = "1.0" +miette = { version = "5", features = ["fancy"] } # gives nicer error messages! +cmake = "0.1" + +[lib] +name = "trajoptlib" + +[features] +default = [] +sleipnir = [] +casadi = [] + +[[example]] +name = "swerve" diff --git a/rust/README.md b/rust/README.md new file mode 100644 index 00000000..4b13c247 --- /dev/null +++ b/rust/README.md @@ -0,0 +1,11 @@ +# TrajoptLib Rust + +This library crate enables TrajoptLib usage in Rust through a foreign function interface. + +## Windows Building + +On windows, it is required to use the gnu toolchain: + +```console +cargo build --target x86_64-pc-windows-gnu +``` diff --git a/rust/build.rs b/rust/build.rs new file mode 100644 index 00000000..b98259ba --- /dev/null +++ b/rust/build.rs @@ -0,0 +1,75 @@ +use cmake::Config; + +fn main() -> miette::Result<()> { + let mut cmake_config = Config::new(".."); + + cmake_config + .profile("RelWithDebInfo") + .define("BUILD_TESTING", "OFF"); + + if cfg!(feature = "sleipnir") && cfg!(feature = "casadi") { + panic!("Only select one optimizer backend via cargo `--features sleipnir` or `--features casadi`."); + } + + if cfg!(feature = "sleipnir") { + cmake_config + .define("OPTIMIZER_BACKEND", "sleipnir") + .define("BUILD_SHARED_LIBS", "OFF"); + + if cfg!(target_os = "windows") { + cmake_config + .generator("Visual Studio 17 2022") + .define("CMAKE_GENERATOR_PLATFORM", "x64") + .cxxflag("/EHsc"); + } else if cfg!(target_os = "linux") { + cmake_config + .define("CMAKE_CXX_COMPILER", "g++") + .define("CMAKE_C_COMPILER", "gcc"); + } + } else if cfg!(feature = "casadi") { + cmake_config.define("OPTIMIZER_BACKEND", "casadi"); + + if cfg!(target_os = "windows") { + cmake_config + .generator("MinGW Makefiles") + .define("CMAKE_CXX_COMPILER", "x86_64-w64-mingw32-g++") + .define("CMAKE_C_COMPILER", "x86_64-w64-mingw32-gcc") + .define( + "CMAKE_SHARED_LINKER_FLAGS", + "-static-libgcc -static-libstdc++", + ) + .define("CMAKE_EXE_LINKER_FLAGS", "-static-libgcc -static-libstdc++"); + } else if cfg!(target_os = "linux") { + cmake_config + .define("CMAKE_CXX_COMPILER", "g++") + .define("CMAKE_C_COMPILER", "gcc"); + } + } else { + panic!( + "Select an optimizer backend via cargo `--features sleipnir` or `--features casadi`." + ); + } + + let dst = cmake_config.build(); + + println!("cargo:rustc-link-search=native={}/bin", dst.display()); + println!("cargo:rustc-link-search=native={}/lib", dst.display()); + println!("cargo:rustc-link-lib=TrajoptLib"); + if cfg!(feature = "sleipnir") { + println!("cargo:rustc-link-lib=Sleipnir"); + println!("cargo:rustc-link-lib=fmt"); + } + + cxx_build::bridge("src/lib.rs") // returns a cc::Build + .file("src/trajoptlib.cc") + .include("include") + .include(format!("{}/include", dst.display())) + .flag_if_supported("/std:c++20") + .flag_if_supported("-std=c++20") + .compile("trajoptlib-rust"); + + println!("cargo:rerun-if-changed=include/trajoptlib.h"); + println!("cargo:rerun-if-changed=src/trajoptlib.cc"); + println!("cargo:rerun-if-changed=src/lib.rs"); + Ok(()) +} diff --git a/rust/examples/swerve.rs b/rust/examples/swerve.rs new file mode 100644 index 00000000..451afff5 --- /dev/null +++ b/rust/examples/swerve.rs @@ -0,0 +1,52 @@ +use trajoptlib::{SwerveDrivetrain, SwerveModule, SwervePathBuilder}; + +fn main() { + let drivetrain = SwerveDrivetrain { + mass: 45.0, + moi: 6.0, + modules: vec![ + SwerveModule { + x: 0.6, + y: 0.6, + wheel_radius: 0.04, + wheel_max_angular_velocity: 70.0, + wheel_max_torque: 2.0, + }, + SwerveModule { + x: 0.6, + y: -0.6, + wheel_radius: 0.04, + wheel_max_angular_velocity: 70.0, + wheel_max_torque: 2.0, + }, + SwerveModule { + x: -0.6, + y: 0.6, + wheel_radius: 0.04, + wheel_max_angular_velocity: 70.0, + wheel_max_torque: 2.0, + }, + SwerveModule { + x: -0.6, + y: -0.6, + wheel_radius: 0.04, + wheel_max_angular_velocity: 70.0, + wheel_max_torque: 2.0, + }, + ], + }; + + let mut path = SwervePathBuilder::new(); + path.set_drivetrain(&drivetrain); + path.set_bumpers(1.3, 1.3); + path.pose_wpt(0, 0.0, 0.0, 0.0); + path.pose_wpt(1, 1.0, 0.0, 0.0); + // path.wpt_linear_velocity_polar(0, 0.0, 0.0); + // path.wpt_linear_velocity_polar(1, 0.0, 0.0); + // path.wpt_angular_velocity(0, 0.0); + // path.wpt_angular_velocity(1, 0.0); + // path.sgmt_circle_obstacle(0, 1, 0.5, 0.1, 0.2); + path.set_control_interval_counts(vec![40]); + println!("setup complete"); + println!("{:?}", path.generate()); +} diff --git a/rust/include/trajoptlib.h b/rust/include/trajoptlib.h new file mode 100644 index 00000000..b17312b3 --- /dev/null +++ b/rust/include/trajoptlib.h @@ -0,0 +1,67 @@ +// Copyright (c) TrajoptLib contributors + +#pragma once + +#include +#include + +#include +#include + +namespace trajoptlibrust { + +struct HolonomicTrajectory; +struct InitialGuessPoint; +struct SwerveDrivetrain; + +class SwervePathBuilderImpl { + public: + void set_drivetrain(const SwerveDrivetrain& drivetrain); + void set_bumpers(double length, double width); + void set_control_interval_counts(const rust::Vec counts); + + void pose_wpt(size_t idx, double x, double y, double heading); + void translation_wpt(size_t idx, double x, double y, double heading_guess); + void empty_wpt(size_t idx, double x_guess, double y_guess, + double heading_guess); + + void sgmt_initial_guess_points( + size_t from_idx, const rust::Vec& guess_points); + + void wpt_linear_velocity_direction(size_t idx, double angle); + void wpt_linear_velocity_max_magnitude(size_t idx, double magnitude); + void wpt_linear_velocity_polar(size_t idx, double magnitude, double angle); + void wpt_angular_velocity(size_t idx, double angular_velocity); + void wpt_x(size_t idx, double x); + void wpt_y(size_t idx, double y); + void wpt_heading(size_t idx, double heading); + + void sgmt_linear_velocity_direction(size_t from_idx, size_t to_idx, + double angle); + void sgmt_linear_velocity_max_magnitude(size_t from_idx, size_t to_idx, + double magnitude); + void sgmt_linear_velocity_polar(size_t from_idx, size_t to_idx, + double magnitude, double angle); + void sgmt_angular_velocity(size_t from_idx, size_t to_idx, + double angular_velocity); + void sgmt_x(size_t from_idx, size_t to_idx, double x); + void sgmt_y(size_t from_idx, size_t to_idx, double y); + void sgmt_heading(size_t from_idx, size_t to_idx, double heading); + + void sgmt_circle_obstacle(size_t from_idx, size_t to_idx, double x, double y, + double radius); + void sgmt_polygon_obstacle(size_t from_idx, size_t to_idx, + rust::Vec x, rust::Vec y, + double radius); + + HolonomicTrajectory generate() const; + void cancel_all(); + + SwervePathBuilderImpl() = default; + + private: + trajopt::SwervePathBuilder path; +}; + +std::unique_ptr new_swerve_path_builder_impl(); +} // namespace trajoptlibrust diff --git a/rust/src/lib.rs b/rust/src/lib.rs new file mode 100644 index 00000000..3e660ddf --- /dev/null +++ b/rust/src/lib.rs @@ -0,0 +1,384 @@ +#[cxx::bridge(namespace = "trajoptlibrust")] +mod ffi { + #[derive(Debug, Deserialize, Serialize)] + struct SwerveModule { + x: f64, + y: f64, + wheel_radius: f64, + wheel_max_angular_velocity: f64, + wheel_max_torque: f64, + } + + #[derive(Debug, Deserialize, Serialize)] + struct SwerveDrivetrain { + mass: f64, + moi: f64, + modules: Vec, + } + + #[derive(Debug, Deserialize, Serialize)] + struct InitialGuessPoint { + x: f64, + y: f64, + heading: f64, + } + + #[derive(Debug, Deserialize, Serialize)] + struct HolonomicTrajectorySample { + timestamp: f64, + x: f64, + y: f64, + heading: f64, + velocity_x: f64, + velocity_y: f64, + angular_velocity: f64, + } + + #[derive(Debug, Deserialize, Serialize)] + struct HolonomicTrajectory { + samples: Vec, + } + + unsafe extern "C++" { + include!("trajoptlib/include/trajoptlib.h"); + + type SwervePathBuilderImpl; + + fn cancel_all(self: Pin<&mut SwervePathBuilderImpl>); + fn set_drivetrain(self: Pin<&mut SwervePathBuilderImpl>, drivetrain: &SwerveDrivetrain); + fn set_bumpers(self: Pin<&mut SwervePathBuilderImpl>, length: f64, width: f64); + fn set_control_interval_counts(self: Pin<&mut SwervePathBuilderImpl>, counts: Vec); + + fn pose_wpt( + self: Pin<&mut SwervePathBuilderImpl>, + idx: usize, + x: f64, + y: f64, + heading: f64, + ); + fn translation_wpt( + self: Pin<&mut SwervePathBuilderImpl>, + idx: usize, + x: f64, + y: f64, + heading_guess: f64, + ); + fn empty_wpt( + self: Pin<&mut SwervePathBuilderImpl>, + idx: usize, + x_guess: f64, + y_guess: f64, + heading_guess: f64, + ); + + fn sgmt_initial_guess_points( + self: Pin<&mut SwervePathBuilderImpl>, + from_idx: usize, + guess_points: &Vec, + ); + + fn wpt_linear_velocity_direction( + self: Pin<&mut SwervePathBuilderImpl>, + idx: usize, + angle: f64, + ); + fn wpt_linear_velocity_max_magnitude( + self: Pin<&mut SwervePathBuilderImpl>, + idx: usize, + magnitude: f64, + ); + fn wpt_linear_velocity_polar( + self: Pin<&mut SwervePathBuilderImpl>, + idx: usize, + magnitude: f64, + angle: f64, + ); + fn wpt_angular_velocity( + self: Pin<&mut SwervePathBuilderImpl>, + idx: usize, + angular_velocity: f64, + ); + fn wpt_x(self: Pin<&mut SwervePathBuilderImpl>, idx: usize, x: f64); + fn wpt_y(self: Pin<&mut SwervePathBuilderImpl>, idx: usize, y: f64); + fn wpt_heading(self: Pin<&mut SwervePathBuilderImpl>, idx: usize, heading: f64); + + fn sgmt_linear_velocity_direction( + self: Pin<&mut SwervePathBuilderImpl>, + from_idx: usize, + to_idx: usize, + angle: f64, + ); + fn sgmt_linear_velocity_max_magnitude( + self: Pin<&mut SwervePathBuilderImpl>, + from_idx: usize, + to_idx: usize, + magnitude: f64, + ); + fn sgmt_linear_velocity_polar( + self: Pin<&mut SwervePathBuilderImpl>, + from_idx: usize, + to_idx: usize, + magnitude: f64, + angle: f64, + ); + fn sgmt_angular_velocity( + self: Pin<&mut SwervePathBuilderImpl>, + from_idx: usize, + to_idx: usize, + angular_velocity: f64, + ); + fn sgmt_x(self: Pin<&mut SwervePathBuilderImpl>, from_idx: usize, to_idx: usize, x: f64); + fn sgmt_y(self: Pin<&mut SwervePathBuilderImpl>, from_idx: usize, to_idx: usize, y: f64); + fn sgmt_heading( + self: Pin<&mut SwervePathBuilderImpl>, + from_idx: usize, + to_idx: usize, + heading: f64, + ); + + fn sgmt_circle_obstacle( + self: Pin<&mut SwervePathBuilderImpl>, + from_idx: usize, + to_idx: usize, + x: f64, + y: f64, + radius: f64, + ); + + fn sgmt_polygon_obstacle( + self: Pin<&mut SwervePathBuilderImpl>, + from_idx: usize, + to_idx: usize, + x: Vec, + y: Vec, + radius: f64, + ); + + fn generate(self: &SwervePathBuilderImpl) -> Result; + + fn new_swerve_path_builder_impl() -> UniquePtr; + } +} + +pub struct SwervePathBuilder { + path: cxx::UniquePtr, +} + +impl SwervePathBuilder { + pub fn new() -> SwervePathBuilder { + SwervePathBuilder { + path: crate::ffi::new_swerve_path_builder_impl(), + } + } + + pub fn set_drivetrain(&mut self, drivetrain: &crate::ffi::SwerveDrivetrain) { + crate::ffi::SwervePathBuilderImpl::set_drivetrain(self.path.pin_mut(), drivetrain); + } + + pub fn set_bumpers(&mut self, length: f64, width: f64) { + crate::ffi::SwervePathBuilderImpl::set_bumpers(self.path.pin_mut(), length, width); + } + + pub fn set_control_interval_counts(&mut self, counts: Vec) { + crate::ffi::SwervePathBuilderImpl::set_control_interval_counts(self.path.pin_mut(), counts); + } + + pub fn pose_wpt(&mut self, idx: usize, x: f64, y: f64, heading: f64) { + crate::ffi::SwervePathBuilderImpl::pose_wpt(self.path.pin_mut(), idx, x, y, heading); + } + + pub fn translation_wpt(&mut self, idx: usize, x: f64, y: f64, heading_guess: f64) { + crate::ffi::SwervePathBuilderImpl::translation_wpt( + self.path.pin_mut(), + idx, + x, + y, + heading_guess, + ); + } + + pub fn empty_wpt(&mut self, idx: usize, x_guess: f64, y_guess: f64, heading_guess: f64) { + crate::ffi::SwervePathBuilderImpl::empty_wpt( + self.path.pin_mut(), + idx, + x_guess, + y_guess, + heading_guess, + ); + } + + pub fn sgmt_initial_guess_points( + &mut self, + from_idx: usize, + guess_points: &Vec, + ) { + crate::ffi::SwervePathBuilderImpl::sgmt_initial_guess_points( + self.path.pin_mut(), + from_idx, + guess_points, + ); + } + + pub fn wpt_linear_velocity_direction(&mut self, idx: usize, angle: f64) { + crate::ffi::SwervePathBuilderImpl::wpt_linear_velocity_direction( + self.path.pin_mut(), + idx, + angle, + ); + } + + pub fn wpt_linear_velocity_max_magnitude(&mut self, idx: usize, magnitude: f64) { + crate::ffi::SwervePathBuilderImpl::wpt_linear_velocity_max_magnitude( + self.path.pin_mut(), + idx, + magnitude, + ); + } + + pub fn wpt_linear_velocity_polar(&mut self, idx: usize, magnitude: f64, angle: f64) { + crate::ffi::SwervePathBuilderImpl::wpt_linear_velocity_polar( + self.path.pin_mut(), + idx, + magnitude, + angle, + ); + } + + pub fn wpt_angular_velocity(&mut self, idx: usize, angular_velocity: f64) { + crate::ffi::SwervePathBuilderImpl::wpt_angular_velocity( + self.path.pin_mut(), + idx, + angular_velocity, + ); + } + + pub fn wpt_x(&mut self, idx: usize, x: f64) { + crate::ffi::SwervePathBuilderImpl::wpt_x(self.path.pin_mut(), idx, x); + } + + pub fn wpt_y(&mut self, idx: usize, y: f64) { + crate::ffi::SwervePathBuilderImpl::wpt_y(self.path.pin_mut(), idx, y); + } + + pub fn wpt_heading(&mut self, idx: usize, heading: f64) { + crate::ffi::SwervePathBuilderImpl::wpt_heading(self.path.pin_mut(), idx, heading); + } + + pub fn sgmt_linear_velocity_direction(&mut self, from_idx: usize, to_idx: usize, angle: f64) { + crate::ffi::SwervePathBuilderImpl::sgmt_linear_velocity_direction( + self.path.pin_mut(), + from_idx, + to_idx, + angle, + ); + } + + pub fn sgmt_linear_velocity_max_magnitude( + &mut self, + from_idx: usize, + to_idx: usize, + magnitude: f64, + ) { + crate::ffi::SwervePathBuilderImpl::sgmt_linear_velocity_max_magnitude( + self.path.pin_mut(), + from_idx, + to_idx, + magnitude, + ); + } + + pub fn sgmt_linear_velocity_polar( + &mut self, + from_idx: usize, + to_idx: usize, + magnitude: f64, + angle: f64, + ) { + crate::ffi::SwervePathBuilderImpl::sgmt_linear_velocity_polar( + self.path.pin_mut(), + from_idx, + to_idx, + magnitude, + angle, + ); + } + + pub fn sgmt_angular_velocity(&mut self, from_idx: usize, to_idx: usize, angular_velocity: f64) { + crate::ffi::SwervePathBuilderImpl::sgmt_angular_velocity( + self.path.pin_mut(), + from_idx, + to_idx, + angular_velocity, + ); + } + + pub fn sgmt_x(&mut self, from_idx: usize, to_idx: usize, x: f64) { + crate::ffi::SwervePathBuilderImpl::sgmt_x(self.path.pin_mut(), from_idx, to_idx, x); + } + + pub fn sgmt_y(&mut self, from_idx: usize, to_idx: usize, y: f64) { + crate::ffi::SwervePathBuilderImpl::sgmt_y(self.path.pin_mut(), from_idx, to_idx, y); + } + + pub fn sgmt_heading(&mut self, from_idx: usize, to_idx: usize, heading: f64) { + crate::ffi::SwervePathBuilderImpl::sgmt_heading( + self.path.pin_mut(), + from_idx, + to_idx, + heading, + ); + } + + pub fn sgmt_circle_obstacle( + &mut self, + from_idx: usize, + to_idx: usize, + x: f64, + y: f64, + radius: f64, + ) { + crate::ffi::SwervePathBuilderImpl::sgmt_circle_obstacle( + self.path.pin_mut(), + from_idx, + to_idx, + x, + y, + radius, + ); + } + + pub fn sgmt_polygon_obstacle( + &mut self, + from_idx: usize, + to_idx: usize, + x: Vec, + y: Vec, + radius: f64, + ) { + crate::ffi::SwervePathBuilderImpl::sgmt_polygon_obstacle( + self.path.pin_mut(), + from_idx, + to_idx, + x, + y, + radius, + ); + } + + pub fn generate(&self) -> Result { + match self.path.generate() { + Ok(traj) => Ok(traj), + Err(msg) => Err(msg.what().to_string()), + } + } + + pub fn cancel_all(&mut self) { + crate::ffi::SwervePathBuilderImpl::cancel_all(self.path.pin_mut()); + } +} + +pub use ffi::HolonomicTrajectory; +pub use ffi::HolonomicTrajectorySample; +pub use ffi::InitialGuessPoint; +pub use ffi::SwerveDrivetrain; +pub use ffi::SwerveModule; diff --git a/rust/src/trajoptlib.cc b/rust/src/trajoptlib.cc new file mode 100644 index 00000000..f9b90994 --- /dev/null +++ b/rust/src/trajoptlib.cc @@ -0,0 +1,268 @@ +#include "trajoptlib/include/trajoptlib.h" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "trajoptlib/src/lib.rs.h" + +namespace trajoptlibrust { + +template +ToVecType _convert_generic_vec(const FromVecType& fromVec) { + ToVecType toVec; + toVec.reserve(fromVec.size()); + for (const FromType& item : fromVec) { + toVec.emplace_back(Converter(item)); + } + return toVec; +} + +template +std::vector _rust_vec_to_cpp_vector(const rust::Vec& rustVec) { + return _convert_generic_vec< + RustType, + CppType, + rust::Vec, + std::vector, + Converter>(rustVec); +} + +template +rust::Vec _cpp_vector_to_rust_vec(const std::vector& cppVec) { + return _convert_generic_vec< + CppType, + RustType, + std::vector, + rust::Vec, + Converter>(cppVec); +} + +trajopt::SwerveModule _convert_swerve_module(const SwerveModule& swerveModule) { + return trajopt::SwerveModule{ + .x = swerveModule.x, + .y = swerveModule.y, + .wheelRadius = swerveModule.wheel_radius, + .wheelMaxAngularVelocity = swerveModule.wheel_max_angular_velocity, + .wheelMaxTorque = swerveModule.wheel_max_torque + }; +} + +trajopt::SwerveDrivetrain _convert_swerve_drivetrain(const SwerveDrivetrain& drivetrain) { + return trajopt::SwerveDrivetrain{ + .mass = drivetrain.mass, + .moi = drivetrain.moi, + .modules = _rust_vec_to_cpp_vector(drivetrain.modules) + }; +} + +trajopt::InitialGuessPoint _convert_initial_guess_point(const InitialGuessPoint& initialGuessPoint) { + return trajopt::InitialGuessPoint{ + .x = initialGuessPoint.x, + .y = initialGuessPoint.y, + .heading = initialGuessPoint.heading + }; +} + +void SwervePathBuilderImpl::set_drivetrain(const SwerveDrivetrain& drivetrain) { + path.SetDrivetrain(_convert_swerve_drivetrain(drivetrain)); +} + +size_t _convert_count(const size_t& count) { + return count; +} + +void SwervePathBuilderImpl::set_control_interval_counts( + const rust::Vec counts) { + std::vector converted_counts = + _rust_vec_to_cpp_vector + (counts); + path.ControlIntervalCounts(std::move(converted_counts)); +} + +void SwervePathBuilderImpl::set_bumpers(double length, double width) { + path.AddBumpers(trajopt::Bumpers{ + .safetyDistance = 0.01, + .points = { + {+length / 2, +width / 2}, + {-length / 2, +width / 2}, + {-length / 2, -width / 2}, + {+length / 2, -width / 2} + } + }); +} + +void SwervePathBuilderImpl::pose_wpt(size_t idx, double x, double y, double heading) { + path.PoseWpt(idx, x, y, heading); +} + +void SwervePathBuilderImpl::translation_wpt(size_t idx, double x, double y, double heading_guess) { + path.TranslationWpt(idx, x, y, heading_guess); +} + +void SwervePathBuilderImpl::empty_wpt(size_t idx, double x_guess, double y_guess, double heading_guess) { + path.WptInitialGuessPoint(idx, {x_guess, y_guess, heading_guess}); +} + +void SwervePathBuilderImpl::sgmt_initial_guess_points( + size_t from_idx, const rust::Vec& guess_points) { + std::vector convertedGuessPoints = + _rust_vec_to_cpp_vector(guess_points); + path.SgmtInitialGuessPoints(from_idx, convertedGuessPoints); +} + +void SwervePathBuilderImpl::wpt_linear_velocity_direction(size_t idx, double angle) { + path.WptVelocityDirection(idx, angle); +} + +void SwervePathBuilderImpl::wpt_linear_velocity_max_magnitude(size_t idx, double magnitude) { + path.WptVelocityMagnitude(idx, magnitude); +} + +void SwervePathBuilderImpl::wpt_linear_velocity_polar(size_t idx, double magnitude, double angle) { + path.WptVelocityPolar(idx, magnitude, angle); +} + +void SwervePathBuilderImpl::wpt_angular_velocity(size_t idx, double angular_velocity) { + // this probably ought to be added to SwervePathBuilder in the C++ API + path.WptConstraint(idx, trajopt::AngularVelocityConstraint{angular_velocity}); +} + +void SwervePathBuilderImpl::wpt_x(size_t idx, double x) { + path.WptConstraint(idx, trajopt::TranslationConstraint{ + trajopt::RectangularSet2d{ + .xBound = x, + .yBound = trajopt::IntervalSet1d::R1() + } + }); +} + +void SwervePathBuilderImpl::wpt_y(size_t idx, double y) { + path.WptConstraint(idx, trajopt::TranslationConstraint{ + trajopt::RectangularSet2d{ + .xBound = trajopt::IntervalSet1d::R1(), + .yBound = y + } + }); +} + +void SwervePathBuilderImpl::wpt_heading(size_t idx, double heading) { + path.WptConstraint(idx, trajopt::HeadingConstraint{ + trajopt::IntervalSet1d(heading) + }); +} + +void SwervePathBuilderImpl::sgmt_linear_velocity_direction(size_t from_idx, size_t to_idx, double angle) { + path.SgmtVelocityDirection(from_idx, to_idx, angle); +} + +void SwervePathBuilderImpl::sgmt_linear_velocity_max_magnitude(size_t from_idx, size_t to_idx, double magnitude) { + path.SgmtVelocityMagnitude(from_idx, to_idx, magnitude); +} + +void SwervePathBuilderImpl::sgmt_linear_velocity_polar(size_t from_idx, size_t to_idx, double magnitude, double angle) { + path.SgmtConstraint(from_idx, to_idx, trajopt::HolonomicVelocityConstraint{ + trajopt::RectangularSet2d::PolarExactSet2d(magnitude, angle), + trajopt::CoordinateSystem::kField + }); +} + +void SwervePathBuilderImpl::sgmt_angular_velocity(size_t from_idx, size_t to_idx, double angular_velocity) { + path.SgmtConstraint(from_idx, to_idx, trajopt::AngularVelocityConstraint{angular_velocity}); +} + +void SwervePathBuilderImpl::sgmt_x(size_t from_idx, size_t to_idx, double x) { + path.SgmtConstraint(from_idx, to_idx, trajopt::TranslationConstraint{ + trajopt::RectangularSet2d{ + .xBound = x, + .yBound = trajopt::IntervalSet1d::R1() + } + }); +} + +void SwervePathBuilderImpl::sgmt_y(size_t from_idx, size_t to_idx, double y) { + path.SgmtConstraint(from_idx, to_idx, trajopt::TranslationConstraint{ + trajopt::RectangularSet2d{ + .xBound = trajopt::IntervalSet1d::R1(), + .yBound = y + } + }); +} + +void SwervePathBuilderImpl::sgmt_heading(size_t from_idx, size_t to_idx, double heading) { + path.SgmtConstraint(from_idx, to_idx, trajopt::HeadingConstraint{heading}); +} + +void SwervePathBuilderImpl::sgmt_circle_obstacle(size_t from_idx, size_t to_idx, double x, double y, double radius) { + auto obstacle = trajopt::Obstacle{ + .safetyDistance = radius, + .points = {{x, y}} + }; + path.SgmtObstacle(from_idx, to_idx, obstacle); +} + +void SwervePathBuilderImpl::sgmt_polygon_obstacle(size_t from_idx, size_t to_idx, const rust::Vec x, const rust::Vec y, double radius) { + std::vector points; + if (x.size() != y.size()) return; + for (size_t i = 0; i < x.size(); i++) + { + points.push_back({x.at(i), y.at(i)}); + } + auto obstacle = trajopt::Obstacle{ + .safetyDistance = radius, + .points = points + }; + path.SgmtObstacle(from_idx, to_idx, obstacle); +} + +HolonomicTrajectorySample _convert_holonomic_trajectory_sample(const trajopt::HolonomicTrajectorySample& sample) { + return HolonomicTrajectorySample{ + .timestamp = sample.timestamp, + .x = sample.x, + .y = sample.y, + .heading = sample.heading, + .velocity_x = sample.velocityX, + .velocity_y = sample.velocityY, + .angular_velocity = sample.angularVelocity, + }; +} + +HolonomicTrajectory _convert_holonomic_trajectory(const trajopt::HolonomicTrajectory& trajectory) { + return HolonomicTrajectory{ + .samples = _cpp_vector_to_rust_vec< + trajopt::HolonomicTrajectorySample, + HolonomicTrajectorySample, + &_convert_holonomic_trajectory_sample>(trajectory.samples) + }; +} + +HolonomicTrajectory SwervePathBuilderImpl::generate() const { + return _convert_holonomic_trajectory( + trajopt::HolonomicTrajectory{trajopt::OptimalTrajectoryGenerator::Generate(path)}); +} + +std::unique_ptr new_swerve_path_builder_impl() { + return std::make_unique(SwervePathBuilderImpl()); +} + +void SwervePathBuilderImpl::cancel_all() { + path.CancelAll(); +} +} diff --git a/src/IncompatibleTrajectoryException.cpp b/src/IncompatibleTrajectoryException.cpp index 2bab810f..b8a80f4c 100644 --- a/src/IncompatibleTrajectoryException.cpp +++ b/src/IncompatibleTrajectoryException.cpp @@ -1,6 +1,6 @@ // Copyright (c) TrajoptLib contributors -#include "IncompatibleTrajectoryException.h" +#include "trajopt/IncompatibleTrajectoryException.h" namespace trajopt { diff --git a/src/InvalidPathException.cpp b/src/InvalidPathException.cpp index 35525d73..a837424a 100644 --- a/src/InvalidPathException.cpp +++ b/src/InvalidPathException.cpp @@ -1,6 +1,6 @@ // Copyright (c) TrajoptLib contributors -#include "InvalidPathException.h" +#include "trajopt/InvalidPathException.h" namespace trajopt { diff --git a/src/OptimalTrajectoryGenerator.cpp b/src/OptimalTrajectoryGenerator.cpp index eef0b79c..aa47fc64 100644 --- a/src/OptimalTrajectoryGenerator.cpp +++ b/src/OptimalTrajectoryGenerator.cpp @@ -1,8 +1,8 @@ // Copyright (c) TrajoptLib contributors -#include "OptimalTrajectoryGenerator.h" +#include "trajopt/OptimalTrajectoryGenerator.h" -#include "path/SwervePathBuilder.h" +#include "trajopt/path/SwervePathBuilder.h" #if defined(OPTIMIZER_BACKEND_CASADI) #include "optimization/CasADiOpti.h" @@ -12,10 +12,10 @@ #define _OPTI_BACKEND SleipnirExpr, SleipnirOpti #endif #include "DebugOptions.h" -#include "InvalidPathException.h" -#include "drivetrain/SwerveDrivetrain.h" +#include "trajopt/InvalidPathException.h" +#include "trajopt/drivetrain/SwerveDrivetrain.h" #include "optimization/algorithms/SwerveDiscreteOptimal.h" -#include "solution/SwerveSolution.h" +#include "trajopt/solution/SwerveSolution.h" namespace trajopt { diff --git a/src/TrajectoryGenerationException.cpp b/src/TrajectoryGenerationException.cpp index b5670c97..e25e7435 100644 --- a/src/TrajectoryGenerationException.cpp +++ b/src/TrajectoryGenerationException.cpp @@ -1,17 +1,16 @@ // Copyright (c) TrajoptLib contributors -#include "TrajectoryGenerationException.h" +#include "trajopt/TrajectoryGenerationException.h" -#include #include namespace trajopt { TrajectoryGenerationException::TrajectoryGenerationException( - const std::string& message) - : logic_error(message) {} + std::string_view message) + : std::logic_error{std::string{message}} {} TrajectoryGenerationException::TrajectoryGenerationException( const char* message) - : logic_error(message) {} + : std::logic_error{message} {} } // namespace trajopt diff --git a/src/constraint/AngularVelocityConstraint.cpp b/src/constraint/AngularVelocityConstraint.cpp deleted file mode 100644 index eb4b66d9..00000000 --- a/src/constraint/AngularVelocityConstraint.cpp +++ /dev/null @@ -1,23 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#include "constraint/AngularVelocityConstraint.h" - -#include - -#include - -#include "set/IntervalSet1d.h" -#include "solution/SolutionChecking.h" - -namespace trajopt { - -std::optional AngularVelocityConstraint::CheckAngularVelocity( - double angularVelocity, - const SolutionTolerances& tolerances) const noexcept { - auto check = angularVelocityBound.CheckScalar(angularVelocity, tolerances); - if (check.has_value()) { - return SolutionError{fmt::format("ω {}", check->errorMessage)}; - } - return std::nullopt; -} -} // namespace trajopt diff --git a/src/constraint/Constraint.cpp b/src/constraint/Constraint.cpp deleted file mode 100644 index b33eec50..00000000 --- a/src/constraint/Constraint.cpp +++ /dev/null @@ -1,40 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#include "constraint/Constraint.h" - -#include -#include - -#include - -#include "constraint/HeadingConstraint.h" -#include "constraint/TranslationConstraint.h" -#include "solution/SolutionChecking.h" - -namespace trajopt { - -std::optional CheckState( - const Constraint& constraint, double x, double y, double heading, - const SolutionTolerances& tolerances) noexcept { - if (std::holds_alternative(constraint)) { - std::optional check = - std::get(constraint) - .CheckTranslation(x, y, tolerances); - if (check.has_value()) { - return SolutionError{ - fmt::format("({}) violated: {}", "GetTranslationConstraint()", - check->errorMessage)}; // <<< causes error: "Cannot - // format a const argument." - } - } else if (std::holds_alternative(constraint)) { - std::optional check = std::get(constraint) - .CheckHeading(heading, tolerances); - if (check.has_value()) { - return SolutionError{fmt::format( - "({}) violated: {}", "GetHeadingConstraint()", check->errorMessage)}; - } - } - return std::nullopt; -} - -} // namespace trajopt diff --git a/src/constraint/HeadingConstraint.cpp b/src/constraint/HeadingConstraint.cpp deleted file mode 100644 index b3f69742..00000000 --- a/src/constraint/HeadingConstraint.cpp +++ /dev/null @@ -1,22 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#include "constraint/HeadingConstraint.h" - -#include - -#include - -#include "set/IntervalSet1d.h" -#include "solution/SolutionChecking.h" - -namespace trajopt { - -std::optional HeadingConstraint::CheckHeading( - double theta, const SolutionTolerances& tolerances) const noexcept { - auto check = headingBound.CheckScalar(theta, tolerances); - if (check.has_value()) { - return SolutionError{fmt::format("θ = {}: {}", theta, check->errorMessage)}; - } - return std::nullopt; -} -} // namespace trajopt diff --git a/src/constraint/TranslationConstraint.cpp b/src/constraint/TranslationConstraint.cpp deleted file mode 100644 index 913b7a7c..00000000 --- a/src/constraint/TranslationConstraint.cpp +++ /dev/null @@ -1,24 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#include "constraint/TranslationConstraint.h" - -#include - -#include - -#include "set/Set2d.h" -#include "solution/SolutionChecking.h" - -namespace trajopt { - -std::optional TranslationConstraint::CheckTranslation( - double x, double y, const SolutionTolerances& tolerances) const noexcept { - auto check = CheckVector(translationBound, x, y, tolerances); - if (check.has_value()) { - return SolutionError{ - fmt::format("translation = (x, y) = ({}, {}): x-component: ", x, y, - check->errorMessage)}; - } - return std::nullopt; -} -} // namespace trajopt diff --git a/src/constraint/differential/DifferentialCentripetalAccelerationConstraint.cpp b/src/constraint/differential/DifferentialCentripetalAccelerationConstraint.cpp deleted file mode 100644 index da03b1a0..00000000 --- a/src/constraint/differential/DifferentialCentripetalAccelerationConstraint.cpp +++ /dev/null @@ -1,24 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#include "constraint/differential/DifferentialCentripetalAccelerationConstraint.h" - -#include - -#include - -#include "set/IntervalSet1d.h" -#include "solution/SolutionChecking.h" - -namespace trajopt { - -// std::optional -// DifferentialCentripetalAccelerationConstraint::CheckHeading( -// double theta, const SolutionTolerances& tolerances) const noexcept { -// auto check = headingBound.CheckScalar(theta, tolerances); -// if (check.has_value()) { -// return SolutionError{fmt::format("θ = {}: {}", theta, -// check->errorMessage)}; -// } -// return std::nullopt; -// } -} // namespace trajopt diff --git a/src/constraint/holonomic/HolonomicConstraint.cpp b/src/constraint/holonomic/HolonomicConstraint.cpp deleted file mode 100644 index 5e9a0bd0..00000000 --- a/src/constraint/holonomic/HolonomicConstraint.cpp +++ /dev/null @@ -1,41 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#include "constraint/holonomic/HolonomicConstraint.h" - -#include -#include - -#include - -#include "constraint/AngularVelocityConstraint.h" -#include "constraint/holonomic/HolonomicVelocityConstraint.h" -#include "solution/SolutionChecking.h" - -namespace trajopt { - -std::optional CheckState( - const HolonomicConstraint& constraint, double x, double y, double heading, - double velocityX, double velocityY, double angularVelocity, - double accelerationX, double accelerationY, double angularAcceleration, - const SolutionTolerances& tolerances) noexcept { - if (std::holds_alternative(constraint)) { - std::optional check = - std::get(constraint) - .CheckVelocity(velocityX, velocityY, tolerances); - if (check.has_value()) { - return SolutionError{ - fmt::format("({}) violated: {}", "*this", check->errorMessage)}; - } - } else if (std::holds_alternative(constraint)) { - std::optional check = - std::get(constraint) - .CheckAngularVelocity(angularVelocity, tolerances); - if (check.has_value()) { - return SolutionError{ - fmt::format("({}) violated: {}", "*this", check->errorMessage)}; - } - } - return std::nullopt; -} - -} // namespace trajopt diff --git a/src/constraint/holonomic/HolonomicVelocityConstraint.cpp b/src/constraint/holonomic/HolonomicVelocityConstraint.cpp deleted file mode 100644 index 21724865..00000000 --- a/src/constraint/holonomic/HolonomicVelocityConstraint.cpp +++ /dev/null @@ -1,23 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#include "constraint/holonomic/HolonomicVelocityConstraint.h" - -#include - -#include - -#include "set/Set2d.h" -#include "solution/SolutionChecking.h" - -namespace trajopt { - -std::optional HolonomicVelocityConstraint::CheckVelocity( - double velocityX, double velocityY, - const SolutionTolerances& tolerances) const noexcept { - auto check = CheckVector(velocityBound, velocityX, velocityY, tolerances); - if (check.has_value()) { - return SolutionError{fmt::format("velocity {}", check->errorMessage)}; - } - return std::nullopt; -} -} // namespace trajopt diff --git a/src/optimization/Cancellation.cpp b/src/optimization/Cancellation.cpp new file mode 100644 index 00000000..9c4a1ce2 --- /dev/null +++ b/src/optimization/Cancellation.cpp @@ -0,0 +1,14 @@ +// Copyright (c) TrajoptLib contributors + +#include "optimization/Cancellation.h" + +#include + +namespace trajopt { + +std::atomic& GetCancellationFlag() { + static std::atomic flag{0}; + return flag; +} + +} // namespace trajopt diff --git a/src/optimization/Cancellation.h b/src/optimization/Cancellation.h new file mode 100644 index 00000000..8740cbbf --- /dev/null +++ b/src/optimization/Cancellation.h @@ -0,0 +1,11 @@ +// Copyright (c) TrajoptLib contributors + +#pragma once + +#include + +namespace trajopt { + +std::atomic& GetCancellationFlag(); + +} // namespace trajopt diff --git a/src/optimization/CasADiIterCallback.h b/src/optimization/CasADiIterCallback.h new file mode 100644 index 00000000..9762d0b5 --- /dev/null +++ b/src/optimization/CasADiIterCallback.h @@ -0,0 +1,61 @@ +// Copyright (c) TrajoptLib contributors + +#pragma once + +#include +#include + +#include +#include +#include + +#include "optimization/Cancellation.h" + +namespace trajopt { +using namespace casadi; +class CasADiIterCallback : public Callback { + // Data members + casadi_int nx; + casadi_int ng; + casadi_int np; + + public: + // Constructor + CasADiIterCallback(const std::string& name, casadi_int nx, casadi_int ng, + casadi_int np, const Dict& opts = Dict()) + : nx(nx), ng(ng), np(np) { + construct(name, opts); + } + + // Destructor + ~CasADiIterCallback() override = default; + + // Number of inputs and outputs + // boilerplate for us, since we don't use the inputs. + casadi_int get_n_in() override { return 6; } + casadi_int get_n_out() override { return 1; } + Sparsity get_sparsity_in(casadi_int i) override { + switch (static_cast(i)) { + case NLPSOL_F: + return Sparsity::scalar(); + case NLPSOL_X: + case NLPSOL_LAM_X: + return Sparsity::dense(nx); + case NLPSOL_LAM_G: + case NLPSOL_G: + return Sparsity::dense(ng); + case NLPSOL_LAM_P: + return Sparsity::dense(np); + case NLPSOL_NUM_OUT: + break; + } + return Sparsity(); + } + + // Evaluate numerically + std::vector eval(const std::vector& arg) const override { + int flag = trajopt::GetCancellationFlag(); + return {static_cast(flag)}; + } +}; +} // namespace trajopt diff --git a/src/optimization/CasADiOpti.cpp b/src/optimization/CasADiOpti.cpp index 0fcf991e..cb1bc6e3 100644 --- a/src/optimization/CasADiOpti.cpp +++ b/src/optimization/CasADiOpti.cpp @@ -1,6 +1,5 @@ // Copyright (c) TrajoptLib contributors -#ifdef OPTIMIZER_BACKEND_CASADI #include "optimization/CasADiOpti.h" #include @@ -9,34 +8,46 @@ #include #include "DebugOptions.h" +#include "optimization/Cancellation.h" +#include "optimization/CasADiIterCallback.h" + namespace trajopt { + casadi::MX CasADiOpti::DecisionVariable() { return opti.variable(); } + void CasADiOpti::Minimize(const casadi::MX& objective) { opti.minimize(objective); } + void CasADiOpti::SubjectTo(const casadi::MX& constraint) { opti.subject_to(constraint); } + void CasADiOpti::SetInitial(const casadi::MX& expression, double value) { opti.set_initial(expression, value); } + void CasADiOpti::Solve() { -#ifdef DEBUG_OUTPUT - // I don't try-catch this next line since it should always work. - // I'm assuming the dynamic lib is on the path and casadi can find it. - opti.solver("ipopt"); -#else + GetCancellationFlag() = 0; + const auto callback = + new const CasADiIterCallback("f", opti.nx(), opti.ng(), opti.np()); + auto pluginOptions = casadi::Dict(); + pluginOptions["iteration_callback"] = *callback; +#ifndef DEBUG_OUTPUT auto pluginOptions = casadi::Dict(); pluginOptions["ipopt.print_level"] = 0; pluginOptions["print_time"] = 0; pluginOptions["ipopt.sb"] = "yes"; - opti.solver("ipopt", pluginOptions); #endif + // I don't try-catch this next line since it should always work. + // I'm assuming the dynamic lib is on the path and casadi can find it. + opti.solver("ipopt", pluginOptions); solution = opti.solve(); } + double CasADiOpti::SolutionValue(const casadi::MX& expression) const { if (solution) { try { @@ -48,5 +59,5 @@ double CasADiOpti::SolutionValue(const casadi::MX& expression) const { throw std::runtime_error("Solution not generated properly"); } } + } // namespace trajopt -#endif diff --git a/src/optimization/CasADiOpti.h b/src/optimization/CasADiOpti.h index 1a9c9030..f63aa071 100644 --- a/src/optimization/CasADiOpti.h +++ b/src/optimization/CasADiOpti.h @@ -6,6 +6,7 @@ #include +#include "CasADiIterCallback.h" #include "optimization/OptiSys.h" namespace trajopt { diff --git a/src/optimization/HolonomicTrajoptUtil.h b/src/optimization/HolonomicTrajoptUtil.h index fd55c048..f3ff6436 100644 --- a/src/optimization/HolonomicTrajoptUtil.h +++ b/src/optimization/HolonomicTrajoptUtil.h @@ -5,9 +5,9 @@ #include #include -#include "obstacle/Obstacle.h" #include "optimization/TrajoptUtil.h" -#include "solution/HolonomicSolution.h" +#include "trajopt/obstacle/Obstacle.h" +#include "trajopt/solution/HolonomicSolution.h" namespace trajopt { diff --git a/src/optimization/HolonomicTrajoptUtil.inc b/src/optimization/HolonomicTrajoptUtil.inc index 52ce485f..cd9e11f7 100644 --- a/src/optimization/HolonomicTrajoptUtil.inc +++ b/src/optimization/HolonomicTrajoptUtil.inc @@ -5,18 +5,18 @@ #include #include "DebugOptions.h" -#include "TrajectoryGenerationException.h" -#include "constraint/Constraint.h" -#include "constraint/HeadingConstraint.h" -#include "constraint/LinePointConstraint.h" -#include "constraint/PointLineConstraint.h" -#include "constraint/PointPointConstraint.h" -#include "constraint/TranslationConstraint.h" -#include "obstacle/Obstacle.h" #include "optimization/HolonomicTrajoptUtil.h" #include "optimization/TrajoptUtil.h" -#include "path/Path.h" -#include "solution/HolonomicSolution.h" +#include "trajopt/TrajectoryGenerationException.h" +#include "trajopt/constraint/Constraint.h" +#include "trajopt/constraint/HeadingConstraint.h" +#include "trajopt/constraint/LinePointConstraint.h" +#include "trajopt/constraint/PointLineConstraint.h" +#include "trajopt/constraint/PointPointConstraint.h" +#include "trajopt/constraint/TranslationConstraint.h" +#include "trajopt/obstacle/Obstacle.h" +#include "trajopt/path/Path.h" +#include "trajopt/solution/HolonomicSolution.h" namespace trajopt { diff --git a/src/optimization/SleipnirOpti.cpp b/src/optimization/SleipnirOpti.cpp index 84c7284f..119fb636 100644 --- a/src/optimization/SleipnirOpti.cpp +++ b/src/optimization/SleipnirOpti.cpp @@ -1,6 +1,5 @@ // Copyright (c) TrajoptLib contributors -#ifdef OPTIMIZER_BACKEND_SLEIPNIR #include "optimization/SleipnirOpti.h" #include @@ -13,79 +12,114 @@ #include #include "DebugOptions.h" +#include "optimization/Cancellation.h" +#include "trajopt/TrajectoryGenerationException.h" + namespace trajopt { + SleipnirExpr::SleipnirExpr(sleipnir::Variable&& vari) : expr(std::move(vari)) {} + SleipnirExpr::SleipnirExpr(double value) : expr(value) {} + SleipnirExpr operator-(const SleipnirExpr& a) { return SleipnirExpr(-a.expr); } + SleipnirExpr operator+(const SleipnirExpr& a, const SleipnirExpr& b) { return SleipnirExpr(a.expr + b.expr); } + SleipnirExpr operator-(const SleipnirExpr& a, const SleipnirExpr& b) { return SleipnirExpr(a.expr - b.expr); } + SleipnirExpr operator*(const SleipnirExpr& a, const SleipnirExpr& b) { return SleipnirExpr(a.expr * b.expr); } + SleipnirExpr operator/(const SleipnirExpr& a, const SleipnirExpr& b) { return SleipnirExpr(a.expr / b.expr); } + void SleipnirExpr::operator+=(const SleipnirExpr& b) { expr += b.expr; } + SleipnirExpr sin(const SleipnirExpr& a) { - return SleipnirExpr(sin(a.expr)); // NOLINT + return SleipnirExpr(sleipnir::sin(a.expr)); } + SleipnirExpr cos(const SleipnirExpr& a) { - return SleipnirExpr(cos(a.expr)); // NOLINT + return SleipnirExpr(sleipnir::cos(a.expr)); } + SleipnirExpr fmax(const SleipnirExpr& a, const SleipnirExpr& b) { - return SleipnirExpr(+0.5 * (1 + sign(b.expr - a.expr)) * (b.expr - a.expr) + + return SleipnirExpr(+0.5 * (1 + sleipnir::sign(b.expr - a.expr)) * + (b.expr - a.expr) + a.expr); } + SleipnirExpr fmin(const SleipnirExpr& a, const SleipnirExpr& b) { - return SleipnirExpr(-0.5 * (1 + sign(b.expr - a.expr)) * (b.expr - a.expr) + + return SleipnirExpr(-0.5 * (1 + sleipnir::sign(b.expr - a.expr)) * + (b.expr - a.expr) + b.expr); } + sleipnir::EqualityConstraints operator==(const SleipnirExpr& a, const SleipnirExpr& b) { return a.expr == b.expr; } + sleipnir::InequalityConstraints operator>=(const SleipnirExpr& a, const SleipnirExpr& b) { return a.expr >= b.expr; } + sleipnir::InequalityConstraints operator<=(const SleipnirExpr& a, const SleipnirExpr& b) { return a.expr <= b.expr; } + trajopt::SleipnirExpr SleipnirOpti::DecisionVariable() { return SleipnirExpr(opti.DecisionVariable()); } + void SleipnirOpti::Minimize(trajopt::SleipnirExpr&& objective) { opti.Minimize(std::move(objective.expr)); } + void SleipnirOpti::Maximize(trajopt::SleipnirExpr&& objective) { opti.Maximize(std::move(objective.expr)); } + void SleipnirOpti::SubjectTo(sleipnir::InequalityConstraints&& relations) { opti.SubjectTo(std::move(relations)); } + void SleipnirOpti::SubjectTo(sleipnir::EqualityConstraints&& relations) { opti.SubjectTo(std::move(relations)); } + void SleipnirOpti::SetInitial(trajopt::SleipnirExpr& expr, double value) { - expr.expr = value; + expr.expr.SetValue(value); } + void SleipnirOpti::Solve() { - static sleipnir::SolverConfig config; - config.diagnostics = true; - config.maxIterations = std::numeric_limits::max(); - opti.Solve(config); + GetCancellationFlag() = 0; + opti.Callback([](const sleipnir::SolverIterationInfo&) -> bool { + return trajopt::GetCancellationFlag(); + }); + + auto status = opti.Solve({.diagnostics = true}); + + if (static_cast(status.exitCondition) < 0) { + throw TrajectoryGenerationException{ + sleipnir::ToMessage(status.exitCondition)}; + } } + double SleipnirOpti::SolutionValue(const SleipnirExpr& expression) const { return expression.expr.Value(); } + } // namespace trajopt -#endif diff --git a/src/optimization/SwerveTrajoptUtil.h b/src/optimization/SwerveTrajoptUtil.h index 8d145d32..fcf00fb0 100644 --- a/src/optimization/SwerveTrajoptUtil.h +++ b/src/optimization/SwerveTrajoptUtil.h @@ -5,12 +5,12 @@ #include #include -#include "drivetrain/SwerveDrivetrain.h" #include "optimization/HolonomicTrajoptUtil.h" #include "optimization/OptiSys.h" #include "optimization/TrajoptUtil.h" -#include "path/Path.h" -#include "solution/SwerveSolution.h" +#include "trajopt/drivetrain/SwerveDrivetrain.h" +#include "trajopt/path/Path.h" +#include "trajopt/solution/SwerveSolution.h" namespace trajopt { diff --git a/src/optimization/SwerveTrajoptUtil.inc b/src/optimization/SwerveTrajoptUtil.inc index 22577166..d2875fe3 100644 --- a/src/optimization/SwerveTrajoptUtil.inc +++ b/src/optimization/SwerveTrajoptUtil.inc @@ -8,12 +8,12 @@ #include #include "DebugOptions.h" -#include "TrajectoryGenerationException.h" -#include "drivetrain/SwerveDrivetrain.h" #include "optimization/SwerveTrajoptUtil.h" #include "optimization/TrajoptUtil.h" -#include "path/Path.h" -#include "solution/SwerveSolution.h" +#include "trajopt/TrajectoryGenerationException.h" +#include "trajopt/drivetrain/SwerveDrivetrain.h" +#include "trajopt/path/Path.h" +#include "trajopt/solution/SwerveSolution.h" namespace trajopt { @@ -101,7 +101,7 @@ void ApplyDynamicsConstraints(Opti& opti, const Expr& ax, const Expr& ay, const Expr& Fy_net, const Expr& tau_net, double mass, double moi) { opti.SubjectTo(Fx_net == mass * ax); - opti.SubjectTo(Fy_net == mass * ax); + opti.SubjectTo(Fy_net == mass * ay); opti.SubjectTo(tau_net == moi * alpha); } @@ -163,18 +163,17 @@ SwerveSolution ConstructSwerveSolution( dtPerSamp.push_back(dt_val); } } - return SwerveSolution{dtPerSamp, - RowSolutionValue(opti, x), - RowSolutionValue(opti, y), - RowSolutionValue(opti, theta), - RowSolutionValue(opti, vx), - RowSolutionValue(opti, vy), - RowSolutionValue(opti, omega), - RowSolutionValue(opti, ax), - RowSolutionValue(opti, ay), - RowSolutionValue(opti, alpha), - MatrixSolutionValue(opti, Fx), - MatrixSolutionValue(opti, Fy)}; + return SwerveSolution{ + {{dtPerSamp, RowSolutionValue(opti, x), RowSolutionValue(opti, y), + RowSolutionValue(opti, theta)}, + RowSolutionValue(opti, vx), + RowSolutionValue(opti, vy), + RowSolutionValue(opti, omega), + RowSolutionValue(opti, ax), + RowSolutionValue(opti, ay), + RowSolutionValue(opti, alpha)}, + MatrixSolutionValue(opti, Fx), + MatrixSolutionValue(opti, Fy)}; } } // namespace trajopt diff --git a/src/optimization/TrajoptUtil.h b/src/optimization/TrajoptUtil.h index 2fa62162..07091a41 100644 --- a/src/optimization/TrajoptUtil.h +++ b/src/optimization/TrajoptUtil.h @@ -5,13 +5,14 @@ #include #include -#include "constraint/Constraint.h" -#include "obstacle/Obstacle.h" #include "optimization/OptiSys.h" -#include "path/Path.h" -#include "set/IntervalSet1d.h" -#include "set/Set2d.h" -#include "solution/Solution.h" +#include "trajopt/constraint/Constraint.h" +#include "trajopt/obstacle/Obstacle.h" +#include "trajopt/path/InitialGuessPoint.h" +#include "trajopt/path/Path.h" +#include "trajopt/set/IntervalSet1d.h" +#include "trajopt/set/Set2d.h" +#include "trajopt/solution/Solution.h" namespace trajopt { @@ -97,9 +98,12 @@ inline Solution GenerateLinearInitialGuess( template requires OptiSys -static void ApplyInitialGuess(Opti& opti, const Solution& solution, - std::vector& x, std::vector& y, - std::vector& theta); +void ApplyInitialGuess(Opti& opti, const Solution& solution, + std::vector& x, std::vector& y, + std::vector& theta, std::vector& vx, + std::vector& vy, std::vector& omega, + std::vector& ax, std::vector& ay, + std::vector& alpha); } // namespace trajopt #include "optimization/TrajoptUtil.inc" diff --git a/src/optimization/TrajoptUtil.inc b/src/optimization/TrajoptUtil.inc index c557f1d5..54bff731 100644 --- a/src/optimization/TrajoptUtil.inc +++ b/src/optimization/TrajoptUtil.inc @@ -12,20 +12,20 @@ #include #include "DebugOptions.h" -#include "TrajectoryGenerationException.h" -#include "constraint/LinePointConstraint.h" -#include "constraint/PointLineConstraint.h" -#include "constraint/PointPointConstraint.h" -#include "constraint/TranslationConstraint.h" -#include "obstacle/Obstacle.h" #include "optimization/TrajoptUtil.h" -#include "path/Path.h" -#include "set/ConeSet2d.h" -#include "set/EllipticalSet2d.h" -#include "set/IntervalSet1d.h" -#include "set/LinearSet2d.h" -#include "set/RectangularSet2d.h" -#include "set/Set2d.h" +#include "trajopt/TrajectoryGenerationException.h" +#include "trajopt/constraint/LinePointConstraint.h" +#include "trajopt/constraint/PointLineConstraint.h" +#include "trajopt/constraint/PointPointConstraint.h" +#include "trajopt/constraint/TranslationConstraint.h" +#include "trajopt/obstacle/Obstacle.h" +#include "trajopt/path/Path.h" +#include "trajopt/set/ConeSet2d.h" +#include "trajopt/set/EllipticalSet2d.h" +#include "trajopt/set/IntervalSet1d.h" +#include "trajopt/set/LinearSet2d.h" +#include "trajopt/set/RectangularSet2d.h" +#include "trajopt/set/Set2d.h" namespace trajopt { @@ -280,9 +280,13 @@ Solution GenerateLinearInitialGuess( initialGuess.x.reserve(sampTot); initialGuess.y.reserve(sampTot); initialGuess.theta.reserve(sampTot); + initialGuess.dt.reserve(sampTot); initialGuess.x.push_back(initialGuessPoints.front().front().x); initialGuess.y.push_back(initialGuessPoints.front().front().y); initialGuess.theta.push_back(initialGuessPoints.front().front().heading); + for (size_t i = 0; i < sampTot; i++) { + initialGuess.dt.push_back((wptCnt * 5.0) / sampTot); + } for (size_t wptIdx = 1; wptIdx < wptCnt; wptIdx++) { size_t N_sgmt = controlIntervalCounts.at(wptIdx - 1); size_t guessPointCount = initialGuessPoints.at(wptIdx).size(); @@ -341,13 +345,44 @@ template requires OptiSys void ApplyInitialGuess(Opti& opti, const Solution& solution, std::vector& x, std::vector& y, - std::vector& theta) { + std::vector& theta, std::vector& vx, + std::vector& vy, std::vector& omega, + std::vector& ax, std::vector& ay, + std::vector& alpha) { size_t sampleTotal = x.size(); for (size_t sampleIndex = 0; sampleIndex < sampleTotal; sampleIndex++) { opti.SetInitial(x[sampleIndex], solution.x[sampleIndex]); opti.SetInitial(y[sampleIndex], solution.y[sampleIndex]); opti.SetInitial(theta[sampleIndex], solution.theta[sampleIndex]); } + opti.SetInitial(vx[0], 0.0); + opti.SetInitial(vy[0], 0.0); + opti.SetInitial(omega[0], 0.0); + opti.SetInitial(ax[0], 0.0); + opti.SetInitial(ay[0], 0.0); + opti.SetInitial(alpha[0], 0.0); + for (size_t sampleIndex = 1; sampleIndex < sampleTotal; sampleIndex++) { + opti.SetInitial(vx[sampleIndex], + (solution.x[sampleIndex] - solution.x[sampleIndex - 1]) / + solution.dt[sampleIndex]); + opti.SetInitial(vy[sampleIndex], + (solution.y[sampleIndex] - solution.y[sampleIndex - 1]) / + solution.dt[sampleIndex]); + opti.SetInitial(omega[sampleIndex], (solution.theta[sampleIndex] - + solution.theta[sampleIndex - 1]) / + solution.dt[sampleIndex]); + + opti.SetInitial(ax[sampleIndex], (opti.SolutionValue(vx[sampleIndex]) - + opti.SolutionValue(vx[sampleIndex - 1])) / + solution.dt[sampleIndex]); + opti.SetInitial(ay[sampleIndex], (opti.SolutionValue(vy[sampleIndex]) - + opti.SolutionValue(vy[sampleIndex - 1])) / + solution.dt[sampleIndex]); + opti.SetInitial(alpha[sampleIndex], + (opti.SolutionValue(omega[sampleIndex]) - + opti.SolutionValue(omega[sampleIndex - 1])) / + solution.dt[sampleIndex]); + } } } // namespace trajopt diff --git a/src/optimization/algorithms/SwerveDiscreteOptimal.h b/src/optimization/algorithms/SwerveDiscreteOptimal.h index 54dd805d..da594446 100644 --- a/src/optimization/algorithms/SwerveDiscreteOptimal.h +++ b/src/optimization/algorithms/SwerveDiscreteOptimal.h @@ -4,12 +4,12 @@ #include -#include "drivetrain/SwerveDrivetrain.h" #include "optimization/HolonomicTrajoptUtil.h" #include "optimization/OptiSys.h" #include "optimization/TrajoptUtil.h" -#include "path/Path.h" -#include "solution/SwerveSolution.h" +#include "trajopt/drivetrain/SwerveDrivetrain.h" +#include "trajopt/path/Path.h" +#include "trajopt/solution/SwerveSolution.h" namespace trajopt { diff --git a/src/optimization/algorithms/SwerveDiscreteOptimal.inc b/src/optimization/algorithms/SwerveDiscreteOptimal.inc index 3094b54e..c07b76ea 100644 --- a/src/optimization/algorithms/SwerveDiscreteOptimal.inc +++ b/src/optimization/algorithms/SwerveDiscreteOptimal.inc @@ -2,18 +2,18 @@ #pragma once -#include +#include #include #include #include "DebugOptions.h" -#include "TrajectoryGenerationException.h" -#include "drivetrain/SwerveDrivetrain.h" #include "optimization/SwerveTrajoptUtil.h" #include "optimization/TrajoptUtil.h" #include "optimization/algorithms/SwerveDiscreteOptimal.h" -#include "path/Path.h" -#include "solution/SwerveSolution.h" +#include "trajopt/TrajectoryGenerationException.h" +#include "trajopt/drivetrain/SwerveDrivetrain.h" +#include "trajopt/path/Path.h" +#include "trajopt/solution/SwerveSolution.h" namespace trajopt { @@ -25,8 +25,8 @@ SwerveSolution SwerveDiscreteOptimal::Generate() { return ConstructSwerveSolution(opti, x, y, theta, vx, vy, omega, ax, ay, alpha, Fx, Fy, dt, N); } catch (const std::exception& e) { - throw TrajectoryGenerationException("Error optimizing trajectory: " + - std::string(e.what())); + throw TrajectoryGenerationException{ + std::string{"Error optimizing trajectory: {}"} + e.what()}; } } @@ -79,8 +79,29 @@ SwerveDiscreteOptimal::SwerveDiscreteOptimal( } } + double minWidth = INFINITY; + for (size_t i = 1; i < path.drivetrain.modules.size(); i++) { + if (std::abs(path.drivetrain.modules.at(i - 1).x - + path.drivetrain.modules.at(i).x) != 0) { + minWidth = + std::min(minWidth, std::abs(path.drivetrain.modules.at(i - 1).x - + path.drivetrain.modules.at(i).x)); + } + if (std::abs(path.drivetrain.modules.at(i - 1).y - + path.drivetrain.modules.at(i).y) != 0) { + minWidth = + std::min(minWidth, std::abs(path.drivetrain.modules.at(i - 1).y - + path.drivetrain.modules.at(i).y)); + } + } + for (size_t sgmtIdx = 0; sgmtIdx < sgmtCnt; ++sgmtIdx) { dt.emplace_back(opti.DecisionVariable()); + for (auto module : path.drivetrain.modules) { + opti.SubjectTo(dt.at(sgmtIdx) * module.wheelRadius * + module.wheelMaxAngularVelocity <= + minWidth); + } } ApplyDiscreteTimeObjective(opti, dt, N); @@ -122,6 +143,7 @@ SwerveDiscreteOptimal::SwerveDiscreteOptimal( } } - ApplyInitialGuess(opti, initialGuess, x, y, theta); + ApplyInitialGuess(opti, initialGuess, x, y, theta, vx, vy, omega, ax, ay, + alpha); } } // namespace trajopt diff --git a/src/path/SwervePathBuilder.cpp b/src/path/SwervePathBuilder.cpp index 7d4a5877..adba18b4 100644 --- a/src/path/SwervePathBuilder.cpp +++ b/src/path/SwervePathBuilder.cpp @@ -1,28 +1,30 @@ // Copyright (c) TrajoptLib contributors -#include "path/SwervePathBuilder.h" +#include "trajopt/path/SwervePathBuilder.h" +#include #include #include #include -#include "constraint/AngularVelocityConstraint.h" -#include "constraint/Constraint.h" -#include "constraint/HeadingConstraint.h" -#include "constraint/LinePointConstraint.h" -#include "constraint/PointLineConstraint.h" -#include "constraint/TranslationConstraint.h" -#include "constraint/holonomic/HolonomicVelocityConstraint.h" -#include "drivetrain/SwerveDrivetrain.h" -#include "obstacle/Obstacle.h" +#include "optimization/Cancellation.h" #include "optimization/TrajoptUtil.h" -#include "path/InitialGuessPoint.h" -#include "path/Path.h" -#include "set/EllipticalSet2d.h" -#include "set/IntervalSet1d.h" -#include "set/LinearSet2d.h" -#include "set/RectangularSet2d.h" -#include "solution/Solution.h" +#include "trajopt/constraint/AngularVelocityConstraint.h" +#include "trajopt/constraint/Constraint.h" +#include "trajopt/constraint/HeadingConstraint.h" +#include "trajopt/constraint/LinePointConstraint.h" +#include "trajopt/constraint/PointLineConstraint.h" +#include "trajopt/constraint/TranslationConstraint.h" +#include "trajopt/constraint/holonomic/HolonomicVelocityConstraint.h" +#include "trajopt/drivetrain/SwerveDrivetrain.h" +#include "trajopt/obstacle/Obstacle.h" +#include "trajopt/path/InitialGuessPoint.h" +#include "trajopt/path/Path.h" +#include "trajopt/set/EllipticalSet2d.h" +#include "trajopt/set/IntervalSet1d.h" +#include "trajopt/set/LinearSet2d.h" +#include "trajopt/set/RectangularSet2d.h" +#include "trajopt/solution/Solution.h" namespace trajopt { @@ -73,9 +75,13 @@ void SwervePathBuilder::WptVelocityDirection(size_t idx, double angle) { } void SwervePathBuilder::WptVelocityMagnitude(size_t idx, double v) { - WptConstraint(idx, - HolonomicVelocityConstraint{EllipticalSet2d::CircularSet2d(v), - CoordinateSystem::kField}); + if (std::abs(v) < 1e-4) { + WptZeroVelocity(idx); + } else { + WptConstraint(idx, + HolonomicVelocityConstraint{EllipticalSet2d::CircularSet2d(v), + CoordinateSystem::kField}); + } } void SwervePathBuilder::WptZeroVelocity(size_t idx) { @@ -103,10 +109,12 @@ void SwervePathBuilder::SgmtVelocityDirection(size_t fromIdx, size_t toIdx, void SwervePathBuilder::SgmtVelocityMagnitude(size_t fromIdx, size_t toIdx, double v, bool includeWpts) { + Set2d set = EllipticalSet2d{v, v, EllipticalSet2d::Direction::kInside}; + if (std::abs(v) < 1e-4) { + set = RectangularSet2d{0.0, 0.0}; + } SgmtConstraint(fromIdx, toIdx, - HolonomicVelocityConstraint{ - EllipticalSet2d{v, v, EllipticalSet2d::Direction::kInside}, - CoordinateSystem::kField}, + HolonomicVelocityConstraint{set, CoordinateSystem::kField}, includeWpts); } @@ -133,9 +141,9 @@ void SwervePathBuilder::SgmtConstraint(size_t fromIdx, size_t toIdx, } for (size_t idx = fromIdx + 1; idx <= toIdx; idx++) { if (includeWpts) { - path.waypoints.at(fromIdx).waypointConstraints.push_back(constraint); + path.waypoints.at(idx).waypointConstraints.push_back(constraint); } - path.waypoints.at(fromIdx).segmentConstraints.push_back(constraint); + path.waypoints.at(idx).segmentConstraints.push_back(constraint); } } @@ -192,8 +200,8 @@ void SwervePathBuilder::NewWpts(size_t finalIndex) { std::vector SwervePathBuilder::GetConstraintsForObstacle( const Bumpers& bumpers, const Obstacle& obstacle) { - auto distConst = - IntervalSet1d::LessThan(bumpers.safetyDistance + obstacle.safetyDistance); + auto distConst = IntervalSet1d::GreaterThan(bumpers.safetyDistance + + obstacle.safetyDistance); size_t bumperCornerCount = bumpers.points.size(); size_t obstacleCornerCount = obstacle.points.size(); @@ -230,23 +238,34 @@ std::vector SwervePathBuilder::GetConstraintsForObstacle( // obstacle edge to bumper corner constraints for (auto& bumperCorner : bumpers.points) { - for (size_t obstacleCornerIndex = 0; - obstacleCornerIndex < obstacleCornerCount - 1; obstacleCornerIndex++) { - constraints.emplace_back(PointLineConstraint{ - bumperCorner.x, bumperCorner.y, - obstacle.points.at(obstacleCornerIndex).x, - obstacle.points.at(obstacleCornerIndex).y, - obstacle.points.at(obstacleCornerIndex + 1).x, - obstacle.points.at(obstacleCornerIndex + 1).y, distConst}); - } - if (obstacleCornerCount >= 3) { - constraints.emplace_back(PointLineConstraint{ - bumperCorner.x, bumperCorner.y, - obstacle.points.at(bumperCornerCount - 1).x, - obstacle.points.at(bumperCornerCount - 1).y, obstacle.points.at(0).x, + if (obstacleCornerCount > 1) { + for (size_t obstacleCornerIndex = 0; + obstacleCornerIndex < obstacleCornerCount - 1; + obstacleCornerIndex++) { + constraints.emplace_back(PointLineConstraint{ + bumperCorner.x, bumperCorner.y, + obstacle.points.at(obstacleCornerIndex).x, + obstacle.points.at(obstacleCornerIndex).y, + obstacle.points.at(obstacleCornerIndex + 1).x, + obstacle.points.at(obstacleCornerIndex + 1).y, distConst}); + } + if (obstacleCornerCount >= 3) { + constraints.emplace_back(PointLineConstraint{ + bumperCorner.x, bumperCorner.y, + obstacle.points.at(bumperCornerCount - 1).x, + obstacle.points.at(bumperCornerCount - 1).y, + obstacle.points.at(0).x, obstacle.points.at(0).y, distConst}); + } + } else { + constraints.emplace_back(PointPointConstraint{ + bumperCorner.x, bumperCorner.y, obstacle.points.at(0).x, obstacle.points.at(0).y, distConst}); } } return constraints; } + +void SwervePathBuilder::CancelAll() { + trajopt::GetCancellationFlag() = 1; +} } // namespace trajopt diff --git a/src/set/ConeSet2d.cpp b/src/set/ConeSet2d.cpp index 3e34428b..594e291f 100644 --- a/src/set/ConeSet2d.cpp +++ b/src/set/ConeSet2d.cpp @@ -1,30 +1,13 @@ // Copyright (c) TrajoptLib contributors -#include "set/ConeSet2d.h" +#include "trajopt/set/ConeSet2d.h" -#include #include -#include - -#include "solution/SolutionChecking.h" namespace trajopt { -std::optional ConeSet2d::CheckVector( - double xComp, double yComp, - const SolutionTolerances& tolerances) const noexcept { - if (!(xComp * std::sin(thetaBound.upper) >= - yComp * std::cos(thetaBound.upper) && - xComp * std::sin(thetaBound.lower) <= - yComp * std::cos(thetaBound.lower))) { - double rComp = std::hypot(xComp, yComp); - double thetaComp = std::atan2(yComp, xComp); - return SolutionError{fmt::format("(r, θ) = ({}, {})", rComp, thetaComp)}; - } - return std::nullopt; -} - bool ConeSet2d::IsValid() const noexcept { return thetaBound.Range() > 0.0 && thetaBound.Range() <= std::numbers::pi; } + } // namespace trajopt diff --git a/src/set/EllipticalSet2d.cpp b/src/set/EllipticalSet2d.cpp index 369f2739..4bd77f8d 100644 --- a/src/set/EllipticalSet2d.cpp +++ b/src/set/EllipticalSet2d.cpp @@ -1,13 +1,8 @@ // Copyright (c) TrajoptLib contributors -#include "set/EllipticalSet2d.h" +#include "trajopt/set/EllipticalSet2d.h" -#include #include -#include -#include - -#include "solution/SolutionChecking.h" namespace trajopt { @@ -25,42 +20,8 @@ bool EllipticalSet2d::IsR2() const noexcept { yRadius >= std::numeric_limits::infinity(); } -std::optional EllipticalSet2d::CheckVector( - double xComp, double yComp, - const SolutionTolerances& tolerances) const noexcept { - auto scaledVectorXSquared = (xComp * xComp) / (xRadius * xRadius); - auto scaledVectorYSquared = (yComp * yComp) / (yRadius * yRadius); - auto lhs = scaledVectorXSquared + scaledVectorYSquared; - switch (direction) { - case Direction::kInside: - if (lhs <= 1.0) { - return SolutionError{ - fmt::format("({}, {}) is not on or inside an ellipse with x radius " - "of {} and y radius of {}", - xComp, yComp, xRadius, yRadius)}; - } - break; - case Direction::kCentered: - if (std::abs(lhs - 1.0) <= tolerances.errorMargin) { - return SolutionError{ - fmt::format("({}, {}) is not on an ellipse with x " - "radius of {} and y radius of {}", - xComp, yComp, xRadius, yRadius)}; - } - break; - case Direction::kOutside: - if (lhs >= 1.0) { - return SolutionError{fmt::format( - "({}, {}) is not on or outside an ellipse with x radius " - "of {} and y radius of {}", - xComp, yComp, xRadius, yRadius)}; - } - break; - } - return std::nullopt; -} - bool EllipticalSet2d::IsValid() const noexcept { return xRadius > 0.0 && yRadius > 0.0; } + } // namespace trajopt diff --git a/src/set/IntervalSet1d.cpp b/src/set/IntervalSet1d.cpp index d866c819..a9b75929 100644 --- a/src/set/IntervalSet1d.cpp +++ b/src/set/IntervalSet1d.cpp @@ -1,12 +1,8 @@ // Copyright (c) TrajoptLib contributors -#include "set/IntervalSet1d.h" +#include "trajopt/set/IntervalSet1d.h" -#include #include -#include - -#include "solution/SolutionChecking.h" namespace trajopt { @@ -47,16 +43,8 @@ bool IntervalSet1d::IsUpperBounded() const noexcept { return upper < +std::numeric_limits::infinity(); } -std::optional IntervalSet1d::CheckScalar( - double scalar, const SolutionTolerances& tolerances) const noexcept { - if ((IsExact() && std::abs(scalar - lower) > tolerances.errorMargin) || - scalar < lower || scalar > upper) { - return SolutionError{fmt::format("= {}", scalar)}; - } - return std::nullopt; -} - bool IntervalSet1d::IsValid() const noexcept { return lower <= upper; } + } // namespace trajopt diff --git a/src/set/LinearSet2d.cpp b/src/set/LinearSet2d.cpp index a9480c2c..811170d9 100644 --- a/src/set/LinearSet2d.cpp +++ b/src/set/LinearSet2d.cpp @@ -1,28 +1,14 @@ // Copyright (c) TrajoptLib contributors -#include "set/LinearSet2d.h" +#include "trajopt/set/LinearSet2d.h" #include -#include -#include "set/IntervalSet1d.h" -#include "set/RectangularSet2d.h" -#include "solution/SolutionChecking.h" +#include "trajopt/set/IntervalSet1d.h" +#include "trajopt/set/RectangularSet2d.h" namespace trajopt { -std::optional LinearSet2d::CheckVector( - double xComp, double yComp, - const SolutionTolerances& tolerances) const noexcept { - if (std::abs(xComp * std::sin(theta) - yComp * std::cos(theta)) > - tolerances.errorMargin) { - double rComp = std::hypot(xComp, yComp); - double thetaComp = std::atan2(yComp, xComp); - return SolutionError{fmt::format("(r, θ) = ({}, {})", rComp, thetaComp)}; - } - return std::nullopt; -} - RectangularSet2d LinearSet2d::RBoundToRectangular(double theta, const IntervalSet1d& rBound) { double sinTheta = std::sin(theta); diff --git a/src/set/RectangularSet2d.cpp b/src/set/RectangularSet2d.cpp index f31ef960..2c8cbc40 100644 --- a/src/set/RectangularSet2d.cpp +++ b/src/set/RectangularSet2d.cpp @@ -1,12 +1,10 @@ // Copyright (c) TrajoptLib contributors -#include "set/RectangularSet2d.h" +#include "trajopt/set/RectangularSet2d.h" #include -#include -#include "set/IntervalSet1d.h" -#include "solution/SolutionChecking.h" +#include "trajopt/set/IntervalSet1d.h" namespace trajopt { @@ -18,21 +16,8 @@ RectangularSet2d RectangularSet2d::R2() { return RectangularSet2d{IntervalSet1d::R1(), IntervalSet1d::R1()}; } -std::optional RectangularSet2d::CheckVector( - double xComp, double yComp, - const SolutionTolerances& tolerances) const noexcept { - auto xCheck = xBound.CheckScalar(xComp, tolerances); - if (xCheck.has_value()) { - return SolutionError{fmt::format("x ", xCheck->errorMessage)}; - } - auto yCheck = yBound.CheckScalar(yComp, tolerances); - if (yCheck.has_value()) { - return SolutionError{fmt::format("y ", yCheck->errorMessage)}; - } - return std::nullopt; -} - bool RectangularSet2d::IsValid() const noexcept { return xBound.IsValid() && yBound.IsValid(); } + } // namespace trajopt diff --git a/src/set/Set2d.cpp b/src/set/Set2d.cpp deleted file mode 100644 index 882f93f6..00000000 --- a/src/set/Set2d.cpp +++ /dev/null @@ -1,31 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#include "set/Set2d.h" - -#include -#include - -#include "set/ConeSet2d.h" -#include "set/EllipticalSet2d.h" -#include "set/LinearSet2d.h" -#include "set/RectangularSet2d.h" -#include "solution/SolutionChecking.h" - -namespace trajopt { - -std::optional CheckVector(const Set2d& set2d, double xComp, - double yComp, - const SolutionTolerances& tolerances) { - if (std::holds_alternative(set2d)) { - return std::get(set2d).CheckVector(xComp, yComp, - tolerances); - } else if (std::holds_alternative(set2d)) { - return std::get(set2d).CheckVector(xComp, yComp, tolerances); - } else if (std::holds_alternative(set2d)) { - return std::get(set2d).CheckVector(xComp, yComp, - tolerances); - } else /*if (IsCone())*/ { - return std::get(set2d).CheckVector(xComp, yComp, tolerances); - } -} -} // namespace trajopt diff --git a/src/set/setnotation.txt b/src/set/setnotation.txt index 90c54a13..f7903f8c 100644 --- a/src/set/setnotation.txt +++ b/src/set/setnotation.txt @@ -57,3 +57,54 @@ AngularVelocityHolonmicConstraint: Constraint: format_string = "constraint: {constraint}" violation_string = "({constraint}) violated: {constraint violation_string}" + + +Other ways of formatting/examples: + +"translation constraint: position x ∈ [1, 2], y ∈ [1, 2]" applied to waypoint index 0 violated: position x = 6 is outside the interval [1, 2]. +translation constraint applied to segment index 0 violated: + +position x of 3 is outside bound [{}, {}] + +Set 1d: + Interval Set 1d: + if lower == upper: + "= {lower}" + else: + "∈ [{lower}, {upper}]" + +Set 2d: + + + Rectangular Set 2d: + "x-component {xBound}, y-component {yBound}"" + + Linear Set 2d: + (x-component, y-component) must be on the polar line θ = {theta} + Cone Set 2d: + (x-component, y-component) must be within the polar cone r ≥ 0, {} ≤ θ ≤ {} + + Elliptical Set 2d: + "(x-component, y-component) must be " + = + if inside: + "on or within" + else if centered: + "on" + else if outside: + "on or outside" + = + if circular: + "a circle of radius = {xRadius}" + else: + "an ellipse of rₓ = {xRadius}, rᵧ = {yRadius}" + + +Constraint: + HeadingConstraint: + "constraint heading {headingBound} violated: theta = {theta}" + TranslationConstraint: + PoseConstraint: + + +heading constraint: θ diff --git a/src/trajectory/HolonomicTrajectory.cpp b/src/trajectory/HolonomicTrajectory.cpp deleted file mode 100644 index 72603642..00000000 --- a/src/trajectory/HolonomicTrajectory.cpp +++ /dev/null @@ -1,26 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#include "trajectory/HolonomicTrajectory.h" - -#include - -#include "solution/HolonomicSolution.h" - -namespace trajopt { - -HolonomicTrajectory::HolonomicTrajectory( - std::vector samples) - : samples(std::move(samples)) {} - -HolonomicTrajectory::HolonomicTrajectory(const HolonomicSolution& solution) { - double ts = 0.0; - for (size_t samp = 0; samp < solution.x.size(); samp++) { - if (samp != 0) { - ts += solution.dt[samp - 1]; - } - samples.emplace_back(ts, solution.x[samp], solution.y[samp], - solution.theta[samp], solution.vx[samp], - solution.vy[samp], solution.omega[samp]); - } -} -} // namespace trajopt diff --git a/src/trajectory/HolonomicTrajectorySample.cpp b/src/trajectory/HolonomicTrajectorySample.cpp deleted file mode 100644 index 0c6e9b42..00000000 --- a/src/trajectory/HolonomicTrajectorySample.cpp +++ /dev/null @@ -1,20 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#include "trajectory/HolonomicTrajectorySample.h" - -namespace trajopt { - -HolonomicTrajectorySample::HolonomicTrajectorySample(double timestamp, double x, - double y, double heading, - double velocityX, - double velocityY, - double angularVelocity) - : timestamp(timestamp), - x(x), - y(y), - heading(heading), - velocityX(velocityX), - velocityY(velocityY), - angularVelocity(angularVelocity) {} - -} // namespace trajopt diff --git a/test/.styleguide b/test/.styleguide new file mode 100644 index 00000000..c65e7ea6 --- /dev/null +++ b/test/.styleguide @@ -0,0 +1,30 @@ +cppHeaderFileInclude { + \.h$ + \.hpp$ + \.inc$ +} + +cppSrcFileInclude { + \.cpp$ +} + +modifiableFileExclude { + jni/jni\.h$ + jni/darwin/jni_md\.h$ + jni/linux/jni_md\.h$ + jni/win32/jni_md\.h$ + \.jar$ +} + +licenseUpdateExclude { + java/src/main/java/org/team2363/util/DependencyExtractor\.java$ + java/src/main/java/org/team2363/util/RuntimeDetector\.java$ + java/src/main/java/org/team2363/util/RuntimeLoader\.java$ +} + +includeOtherLibs { + ^Eigen/ + ^casadi/ + ^sleipnir/ + ^trajopt/ +} diff --git a/test/src/Main.cpp b/test/src/Main.cpp new file mode 100644 index 00000000..882f2789 --- /dev/null +++ b/test/src/Main.cpp @@ -0,0 +1,10 @@ +// Copyright (c) TrajoptLib contributors + +#include + +int main(int argc, char* argv[]) { + Catch::Session session; + session.configData().allowZeroTests = true; + + return session.run(argc, argv); +} diff --git a/test/src/ObstacleTest.cpp b/test/src/ObstacleTest.cpp new file mode 100644 index 00000000..ae34fea4 --- /dev/null +++ b/test/src/ObstacleTest.cpp @@ -0,0 +1,39 @@ +// Copyright (c) TrajoptLib contributors + +#include + +#include +#include +#include +#include + +TEST_CASE("Obstacle - Linear initial guess", "[Obstacle]") { + SKIP("Fails"); + + using namespace trajopt; + + SwerveDrivetrain swerveDrivetrain{.mass = 45, + .moi = 6, + .modules = {{+0.6, +0.6, 0.04, 70, 2}, + {+0.6, -0.6, 0.04, 70, 2}, + {-0.6, +0.6, 0.04, 70, 2}, + {-0.6, -0.6, 0.04, 70, 2}}}; + + trajopt::SwervePathBuilder path; + path.PoseWpt(0, 0.0, 0.0, 0.0); + path.PoseWpt(1, 2.0, 2.0, 0.0); + + constexpr double length = 0.7; + constexpr double width = 0.7; + path.AddBumpers(trajopt::Bumpers{.safetyDistance = 0.1, + .points = {{+length / 2, +width / 2}, + {-length / 2, +width / 2}, + {-length / 2, -width / 2}, + {+length / 2, -width / 2}}}); + + path.SgmtObstacle( + 0, 1, trajopt::Obstacle{.safetyDistance = 1.0, .points = {{1.0, 1.0}}}); + + path.ControlIntervalCounts({10}); + CHECK_NOTHROW(trajopt::OptimalTrajectoryGenerator::Generate(path)); +} diff --git a/test/src/SwervePathBuilderTest.cpp b/test/src/SwervePathBuilderTest.cpp index 19840474..2bfc289d 100644 --- a/test/src/SwervePathBuilderTest.cpp +++ b/test/src/SwervePathBuilderTest.cpp @@ -2,13 +2,13 @@ #include -#include +#include +#include +#include -#include "path/InitialGuessPoint.h" -#include "path/SwervePathBuilder.h" - -TEST(SwervePathBuilderTest, GenerateLinearInitialGuess) { +TEST_CASE("SwervePathBuilder - Linear initial guess", "[SwervePathBuilder]") { using namespace trajopt; + trajopt::SwervePathBuilder path; path.WptInitialGuessPoint(0, InitialGuessPoint{0.0, 0.0, 0.0}); // at 0 @@ -24,5 +24,5 @@ TEST(SwervePathBuilderTest, GenerateLinearInitialGuess) { std::vector result = path.CalculateInitialGuess().x; std::vector expected = {0.0, 1.0, 2.0, 1.0, 3.0, 5.0}; - ASSERT_EQ(result, expected); + CHECK(result == expected); } diff --git a/test/src/SwerveTrajoptProblemTest.cpp b/test/src/SwerveTrajoptProblemTest.cpp deleted file mode 100644 index 30cf55bf..00000000 --- a/test/src/SwerveTrajoptProblemTest.cpp +++ /dev/null @@ -1,24 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#include - -#include - -#include "TestOpti.h" -#include "optimization/SwerveTrajoptUtil.h" -#include "optimization/TrajoptUtil.h" -#include "path/InitialGuessPoint.h" -#include "set/IntervalSet1d.h" - -TEST(TrajoptUtilTest, ApplyKinematicsConstraints) { - TestOpti opti; - // 0 0 1 0 1 2 3 - std::vector x{1.0, -1.0, 9.0, -19.0, 33.0, 101.0, 41.0}; - std::vector v{1.0, -1.0, 5.0, -7.0, 13.0, 17.0, -15.0}; - std::vector a{1.0, -1.0, 3.0, -3.0, 5.0, 1.0, -8.0}; - // dt{ 2.0, 2.0, 4.0, 4.0, 4.0, 4.0} - std::vector dt{2.0, 4.0}; - std::vector N{2, 4}; - trajopt::ApplyKinematicsConstraints(opti, x, x, x, v, v, v, a, a, a, dt, N); - EXPECT_FALSE(opti.IsViolating()); -} diff --git a/test/src/SwerveTrajoptUtilTest.cpp b/test/src/SwerveTrajoptUtilTest.cpp new file mode 100644 index 00000000..06b07d66 --- /dev/null +++ b/test/src/SwerveTrajoptUtilTest.cpp @@ -0,0 +1,89 @@ +// Copyright (c) TrajoptLib contributors + +#include + +#include +#include +#include +#include + +#include "TestOpti.h" +#include "optimization/SwerveTrajoptUtil.h" +#include "optimization/TrajoptUtil.h" + +TEST_CASE("SwerveTrajoptUtil - SolveNetForce()", "[SwerveTrajoptUtil]") { + std::vector Fx{4.0, 1.0, 3.0, 5.0}; + std::vector Fy{0.0, 2.0, -5.0, 2.0}; + + auto [Fx_net, Fy_net] = trajopt::SolveNetForce(Fx, Fy); + + CHECK(Fx_net == Catch::Approx(13.0).margin(1e-3)); + CHECK(Fy_net == Catch::Approx(-1.0).margin(1e-3)); +} + +TEST_CASE("SwerveTrajoptUtil - SolveNetTorque()", "[SwerveTrajoptUtil]") { + constexpr double theta = 1.0; + + std::vector Fx{-3.0, -5.0, -4.0, 2.0}; + std::vector Fy{4.0, -5.0, -2.0, -4.0}; + std::vector swerveModules = { + {.x = 1.0, + .y = 1.0, + .wheelRadius = 0.0, + .wheelMaxAngularVelocity = 0.0, + .wheelMaxTorque = 0.0}, + {.x = 1.0, + .y = -1.0, + .wheelRadius = 0.0, + .wheelMaxAngularVelocity = 0.0, + .wheelMaxTorque = 0.0}, + {.x = -1.0, + .y = 1.0, + .wheelRadius = 0.0, + .wheelMaxAngularVelocity = 0.0, + .wheelMaxTorque = 0.0}, + {.x = -1.0, + .y = -1.0, + .wheelRadius = 0.0, + .wheelMaxAngularVelocity = 0.0, + .wheelMaxTorque = 0.0}, + }; + + double tau_net = trajopt::SolveNetTorque(theta, Fx, Fy, swerveModules); + + CHECK(tau_net == Catch::Approx(0.6553658).margin(1e-3)); +} + +TEST_CASE("SwerveTrajoptUtil - ApplyKinematicsConstraints()", + "[SwerveTrajoptUtil]") { + TestOpti opti; + // 0 0 1 0 1 2 3 + std::vector x{1.0, -1.0, 9.0, -19.0, 33.0, 101.0, 41.0}; + std::vector v{1.0, -1.0, 5.0, -7.0, 13.0, 17.0, -15.0}; + std::vector a{1.0, -1.0, 3.0, -3.0, 5.0, 1.0, -8.0}; + // dt{ 2.0, 2.0, 4.0, 4.0, 4.0, 4.0} + std::vector dt{2.0, 4.0}; + std::vector N{2, 4}; + + trajopt::ApplyKinematicsConstraints(opti, x, x, x, v, v, v, a, a, a, dt, N); + + CHECK_FALSE(opti.IsViolating()); +} + +TEST_CASE("SwerveTrajoptUtil - ApplyDynamicsConstraints()", + "[SwerveTrajoptUtil]") { + TestOpti opti; + constexpr double ax = 1.0; + constexpr double ay = -2.0; + constexpr double alpha = 9.0; + constexpr double Fx_net = 45.0; + constexpr double Fy_net = -90.0; + constexpr double tau_net = 54.0; + constexpr double mass = 45.0; + constexpr double moi = 6.0; + + trajopt::ApplyDynamicsConstraints(opti, ax, ay, alpha, Fx_net, Fy_net, + tau_net, mass, moi); + + CHECK_FALSE(opti.IsViolating()); +} diff --git a/test/src/TrajoptUtilTest.cpp b/test/src/TrajoptUtilTest.cpp index 19a7acfd..8369b59a 100644 --- a/test/src/TrajoptUtilTest.cpp +++ b/test/src/TrajoptUtilTest.cpp @@ -2,34 +2,34 @@ #include -#include +#include +#include +#include #include "TestOpti.h" #include "optimization/TrajoptUtil.h" -#include "path/InitialGuessPoint.h" -#include "set/IntervalSet1d.h" -TEST(TrajoptUtilTest, GetIdx) { +TEST_CASE("TrajoptUtil - GetIdx()", "[TrajoptUtil]") { auto result0 = trajopt::GetIdx({2, 3}, 0, 0); auto result1 = trajopt::GetIdx({2, 3}, 1, 1); auto result2 = trajopt::GetIdx({2, 3}, 2, 2); auto result3 = trajopt::GetIdx({2, 3}, 3, 0); - EXPECT_EQ(result0, 0); - EXPECT_EQ(result1, 2); - EXPECT_EQ(result2, 5); - EXPECT_EQ(result3, 6); + CHECK(result0 == 0); + CHECK(result1 == 2); + CHECK(result2 == 5); + CHECK(result3 == 6); } -TEST(TrajoptUtilTest, ApplyDiscreteTimeObjective) { +TEST_CASE("TrajoptUtil - ApplyDiscreteTimeObjective()", "[TrajoptUtil]") { TestOpti opti; std::vector dt = {-1, 3}; std::vector N = {20, 15}; trajopt::ApplyDiscreteTimeObjective(opti, dt, N); - EXPECT_EQ(opti.GetMinimizeObjective(), 25); - EXPECT_TRUE(opti.IsViolating()); + CHECK(opti.GetMinimizeObjective() == 25); + CHECK(opti.IsViolating()); } -TEST(TrajoptUtilTest, ApplyIntervalSet1d) { +TEST_CASE("TrajoptUtil - ApplyIntervalSet1d()", "[TrajoptUtil]") { auto case1 = trajopt::IntervalSet1d(1, 3); auto case2 = trajopt::IntervalSet1d(-4); auto case3 = trajopt::IntervalSet1d::LessThan(-6); @@ -60,22 +60,22 @@ TEST(TrajoptUtilTest, ApplyIntervalSet1d) { {case4, 5.0, true}}) { TestOpti opti; trajopt::ApplyIntervalSet1dConstraint(opti, test.val, test.set); - EXPECT_EQ(opti.IsViolating(), test.isViolating); + CHECK(opti.IsViolating() == test.isViolating); } } -TEST(TrajoptUtilTest, Linspace) { +TEST_CASE("TrajoptUtil - Linspace()", "[TrajoptUtil]") { auto result = trajopt::Linspace(0.0, 2.0, 2); std::vector correct{1.0, 2.0}; - EXPECT_EQ(result, correct); + CHECK(result == correct); } -TEST(TrajoptUtilTest, LinearInitialGuess) { +TEST_CASE("TrajoptUtil - Linear initial guess", "[TrajoptUtil]") { std::vector> initialGuessPoints{ {{1, 0, 0}}, {{2, 0, 0}, {3, 0, 0}}, {{6, 0, 0}}}; std::vector controlIntervalCounts{2, 3}; std::vector expectedX{1, 2, 3, 4, 5, 6}; auto result = trajopt::GenerateLinearInitialGuess(initialGuessPoints, controlIntervalCounts); - EXPECT_EQ(expectedX, result.x); + CHECK(expectedX == result.x); }